// ----------------------------------------------------------------------------------------
#region Static
public static bool IsNaN(Single3x3 value)
{
return
(float.IsNaN(value.XX) || float.IsNaN(value.XY) || float.IsNaN(value.XZ) ||
float.IsNaN(value.YX) || float.IsNaN(value.YY) || float.IsNaN(value.YZ) ||
float.IsNaN(value.ZX) || float.IsNaN(value.ZY) || float.IsNaN(value.ZZ));
}
public Single3x3 Orthogonalize(Single3x3 defaultResult)
{
// Gram-Schmidtmethod
// ToDo: Improve with Singular Value Decomposition A = UWVT, and set W = I
Single3x3 result = defaultResult;
Single3 x1 = Row1, x2 = Row2, x3 = Row3;
Single3 y1 = x1;
if (y1.LengthSqr() == 0f)
{
return(result);
}
y1 = y1.Normalize();
Single3 y2 = x2 - (x2 * y1) * y1;
if (y2.LengthSqr() == 0f)
{
return(result);
}
y2 = y2.Normalize();
Single3 y3 = x3 - (x3 * y1) * y1 - (x3 * y2) * y2;
if (y3.LengthSqr() == 0f)
{
return(result);
}
y3 = y3.Normalize();
return(new Single3x3(
y1.X, y1.Y, y1.Z,
y2.X, y2.Y, y2.Z,
y3.X, y3.Y, y3.Z
));
}
public static Single3x3 RotationMatrix(float orientationAngle, float rotationAngle, float tiltAngle)
{
Single3x3 matrix = RotationXMatrix(tiltAngle);
matrix = RotationYMatrix(rotationAngle) * matrix;
matrix = RotationZMatrix(orientationAngle) * matrix;
return(matrix);
}
public static bool IsOrthogonal(Single3x3 m)
{
if (!IsOrthogonal(m.Col1, m.Col2, m.Col3))
{
return(false);
}
if (!IsOrthogonal(m.Row1, m.Row2, m.Row3))
{
return(false);
}
return(true);
}
// ----------------------------------------------------------------------------------------
#region Object
public override bool Equals(object o)
{
Single3x3 value = (Single3x3)o;
if (IsNaN(this) && IsNaN(value))
{
return(true);
}
if (XX != value.XX)
{
return(false);
}
if (XY != value.XY)
{
return(false);
}
if (XZ != value.XZ)
{
return(false);
}
if (YX != value.YX)
{
return(false);
}
if (YY != value.YY)
{
return(false);
}
if (YZ != value.YZ)
{
return(false);
}
if (ZX != value.ZX)
{
return(false);
}
if (ZY != value.ZY)
{
return(false);
}
if (ZZ != value.ZZ)
{
return(false);
}
return(true);
}
