public static Matrix3P operator *(Matrix3P a, Matrix3P b)
{
var res = new Matrix3P();
float x, y, z;
x = b[0, 0];
y = b[1, 0];
z = b[2, 0];
res[0, 0] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z;
res[1, 0] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z;
res[2, 0] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z;
x = b[0, 1];
y = b[1, 1];
z = b[2, 1];
res[0, 1] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z;
res[1, 1] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z;
res[2, 1] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z;
x = b[0, 2];
y = b[1, 2];
z = b[2, 2];
res[0, 2] = a[0, 0] * x + a[0, 1] * y + a[0, 2] * z;
res[1, 2] = a[1, 0] * x + a[1, 1] * y + a[1, 2] * z;
res[2, 2] = a[2, 0] * x + a[2, 1] * y + a[2, 2] * z;
return(res);
}
/// <summary>
/// for unit quaternions only?
/// </summary>
/// <returns></returns>
public Matrix3P AsRotationMatrix()
{
var rotMatrix = new Matrix3P();
double xy = x * y;
double wz = w * z;
double wx = w * x;
double wy = w * y;
double xz = x * z;
double yz = y * z;
double zz = z * z;
double yy = y * y;
double xx = x * x;
rotMatrix[0, 0] = (float)(1 - 2 * (yy + zz)); //1-2y2-2z2// need .997
rotMatrix[0, 1] = (float)(2 * (xy - wz)); //2xy-2wz -0.033
rotMatrix[0, 2] = (float)(2 * (xz + wy)); //// 2xz+2wy//0.063
rotMatrix[1, 0] = (float)(2 * (xy + wz)); //2xy+2wz 0.024
rotMatrix[1, 1] = (float)(1 - 2 * (xx + zz)); //1-2x2-2z2
rotMatrix[1, 2] = (float)(2 * (yz - wx)); //2yz+2wx////////////////
rotMatrix[2, 0] = (float)(2 * (xz - wy)); //2xz-2wy
rotMatrix[2, 1] = (float)(2 * (yz + wx)); //2yz-2wx/////////
rotMatrix[2, 2] = (float)(1 - 2 * (xx + yy)); //1-2x2-2y2
return(rotMatrix);
}
private Matrix4P(Matrix3P orientation, Vector3P position)
{
Orientation = orientation;
Position = position;
}