You can do this Shamino.

You will be working with 4x4 matrices nearly all the time.

Code:

void MatrixMultiply(float matrix1[4][4],float matrix2[4][4],float result[4][4])
{
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
for (int k=0;k<4;k++)
{
result[i][j]+=matrix1[i][k]*matrix2[k][j];
}
}
}
}

I would recommend unrolling the loops however or coding it like:

Code:

matrix[0][0]=matrix1[..][..]*matrix2[...][...];

I would give you a matrix library, but mine are for DX and yours will be for OpenGL. The matrices are diff for each of those.

For vectors you can do this:

Code:

//Multiplys a inVec by matrix and returns result in outVec
//Transforms a vector by a matrix
void Vec3TransformCoord(Vector3 &inVec,float matrix[4][4],Vector3 &outVec)
{
outVec.x=inVec.x*matrix[0][0]+inVec.y*matrix[0][1]+inVec.z*matrix[0][2]+matrix[0][3];
outVec.y=inVec.x*matrix[1][0]+inVec.y*matrix[1][1]+inVec.z*matrix[1][2]+matrix[1][3];
outVec.z=inVec.x*matrix[2][0]+inVec.y*matrix[2][1]+inVec.z*matrix[2][2]+matrix[2][3];
}

Code:

//Normalizes a vector - normalized vector is outVec
void Vec3Normalize(Vec3 &outVec,Vec3 &inVec)
{
float length=sqrtf(inVec.x*inVec.x+inVec.y*inVec.y+inVec.z*inVec.z);
float oneOverLength=1.0f/length;
outVec.x=inVec.x*oneOverLength;
outVec.y=inVec.y*oneOverLength;
outVec.z=inVec.z*oneOverLength;
}
//Returns dot product of vec1 and vec2
//vec1 and vec2 are assumed to be non-normal
float Vec3NormalDot(Vec3 &vec1,Vec3 &vec2)
{
Vec3 toVec2=Vec3(vec2.x-vec1.x,vec2.y-vec1.y,vec2.z-vec1.z);
Vec3Normalize(&toVec2,&toVec2);
return (toVec2.x*toVec2.x+toVec2.y*toVec2.y+toVec2.z*toVec2.z);
}