public static Vector3D Vec3MatrixMult(Vector3D v, Matrix3x3 m)
{
Vector3D newVector = new Vector3D();
newVector.x = v.x * m.m0 + v.y * m.m1 + v.z * m.m2;
newVector.y = v.x * m.m3 + v.y * m.m4 + v.z * m.m5;
newVector.z = v.x * m.m6 + v.y * m.m7 + v.z * m.m8;
return newVector;
}
public static Matrix3x3 QuaternionToRotationMatrix(Vector4D q)
{
Matrix3x3 newMatrix = new Matrix3x3();
newMatrix.m0 = 1- 2*q.y*q.y- 2*q.z*q.z;
newMatrix.m1 = 2*q.x*q.y- 2*q.w*q.z;
newMatrix.m2 = 2*q.x*q.z+ 2*q.w*q.y;
newMatrix.m3 = 2*q.x*q.y+ 2*q.w*q.z;
newMatrix.m4 = 1- 2*q.x*q.x- 2*q.z*q.z;
newMatrix.m5 = 2*q.y*q.z- 2*q.w*q.x;
newMatrix.m6 = 2*q.x*q.z- 2*q.w*q.y;
newMatrix.m7 = 2*q.y*q.z+ 2*q.w*q.x;
newMatrix.m8 = 1- 2*q.x*q.x- 2*q.y*q.y;
return newMatrix;
}
