public static FixTrans3 operator *(FixTrans3 lhs, FixTrans3 rhs)
{
FixTrans3 t = new FixTrans3();
t.m = lhs.m * rhs.m;
return(t);
}
public FixQuaternion(ref FixTrans3 matrix)
{
Fix determinant = (matrix.M11 * (matrix.M22 * matrix.M33 - matrix.M32 * matrix.M23)) -
(matrix.M12 * (matrix.M21 * matrix.M33 - matrix.M31 * matrix.M23)) +
(matrix.M13 * (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22));
Fix scale = FixMath.Pow(determinant, Fix.One / 3);
Fix x, y, z;
w = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 + matrix.M22 + matrix.M33)) / 2);
x = (FixMath.Sqrt(FixMath.Max(0, scale + matrix.M11 - matrix.M22 - matrix.M33)) / 2);
y = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 + matrix.M22 - matrix.M33)) / 2);
z = (FixMath.Sqrt(FixMath.Max(0, scale - matrix.M11 - matrix.M22 + matrix.M33)) / 2);
xyz = new FixVec3(x, y, z);
if (matrix.M32 - matrix.M23 < 0)
{
X = -X;
}
if (matrix.M13 - matrix.M31 < 0)
{
Y = -Y;
}
if (matrix.M21 - matrix.M12 < 0)
{
Z = -Z;
}
}
public FixTrans3 Translate(FixVec3 delta)
{
FixTrans3 ft = new FixTrans3(m);
ft.m[0, 3] += delta.x;
ft.m[1, 3] += delta.y;
ft.m[2, 3] += delta.z;
return(ft);
}
