public Double3x3 Orthogonalize(Double3x3 defaultResult)
{
// Gram-Schmidtmethod
// ToDo: Improve with Singular Value Decomposition A = UWVT, and set W = I
Double3x3 result = defaultResult;
Double3 x1 = Row1, x2 = Row2, x3 = Row3;
Double3 y1 = x1;
if (y1.LengthSqr() == 0d)
{
return(result);
}
y1 = y1.Normalize();
Double3 y2 = x2 - (x2 * y1) * y1;
if (y2.LengthSqr() == 0d)
{
return(result);
}
y2 = y2.Normalize();
Double3 y3 = x3 - (x3 * y1) * y1 - (x3 * y2) * y2;
if (y3.LengthSqr() == 0d)
{
return(result);
}
y3 = y3.Normalize();
return(new Double3x3(
y1.X, y1.Y, y1.Z,
y2.X, y2.Y, y2.Z,
y3.X, y3.Y, y3.Z
));
}
// ----------------------------------------------------------------------------------------
#region Static
public static bool IsNaN(Double3x3 value)
{
return
(double.IsNaN(value.XX) || double.IsNaN(value.XY) || double.IsNaN(value.XZ) ||
double.IsNaN(value.YX) || double.IsNaN(value.YY) || double.IsNaN(value.YZ) ||
double.IsNaN(value.ZX) || double.IsNaN(value.ZY) || double.IsNaN(value.ZZ));
}
public static Double3x3 RotationMatrix(double orientationAngle, double rotationAngle, double tiltAngle)
{
Double3x3 matrix = RotationXMatrix(tiltAngle);
matrix = RotationYMatrix(rotationAngle) * matrix;
matrix = RotationZMatrix(orientationAngle) * matrix;
return(matrix);
}
// ----------------------------------------------------------------------------------------
#region Object
public override bool Equals(object o)
{
Double3x3 value = (Double3x3)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);
}