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 bool IsOrthogonal(Single3 c1, Single3 c2, Single3 c3)
{
if (!IsUnitVector(c1))
{
return(false);
}
if (!IsUnitVector(c2))
{
return(false);
}
if (!IsUnitVector(c3))
{
return(false);
}
if (!IsOrthogonal(c1, c2))
{
return(false);
}
if (!IsOrthogonal(c2, c3))
{
return(false);
}
if (!IsOrthogonal(c3, c1))
{
return(false);
}
return(true);
}
public Single3 Normalize(Single3 direction)
{
Single3 value = Normalize();
if (direction * value < 0)
{
value = -value;
}
return(value);
}
// ----------------------------------------------------------------------------------------
#region Object
public override bool Equals(object o)
{
Single3 value = (Single3)o;
if (IsNaN(this) && IsNaN(value))
{
return(true);
}
if (X != value.X)
{
return(false);
}
if (Y != value.Y)
{
return(false);
}
if (Z != value.Z)
{
return(false);
}
return(true);
}
public Single3x3(Single3 rotationAxis, float rotationAngle)
{
float a = rotationAngle.ToRadians();
float cs = (float)Cos(a);
float sn = (float)Sin(a);
float ux = rotationAxis.X;
float uy = rotationAxis.Y;
float uz = rotationAxis.Z;
XX = cs + ux * ux * (1 - cs);
YX = uy * ux * (1 - cs) + uz * sn;
ZX = uz * ux * (1 - cs) - uy * sn;
XY = ux * uy * (1 - cs) - uz * sn;
YY = cs + uy * uy * (1 - cs);
ZY = uz * uy * (1 - cs) + ux * sn;
XZ = ux * uz * (1 - cs) + uy * sn;
YZ = uy * uz * (1 - cs) - ux * sn;
ZZ = cs + uz * uz * (1 - cs);
}
public static float AngleBetweenVectors(Single3 p1, Single3 p2)
{
double l1 = p1.Length();
double l2 = p2.Length();
if ((l1 == 0) || (l2 == 0))
{
throw new Exception("Angle between vectors is not defined if length of the vector(s) is zero");
}
double cs = (p1 * p2) / (l1 * l2);
double ad;
if (Abs(cs) < 1d)
{
ad = Acos(cs).ToDegrees();
}
else
{
ad = (cs > 0) ? 0d : 180d;
}
return((float)ad);
}
// ----------------------------------------------------------------------------------------
#region Static
public static bool IsNaN(Single3 value)
{
return(float.IsNaN(value.X) || float.IsNaN(value.Y) || float.IsNaN(value.Z));
}
public static bool IsOrthogonal(Single3 p1, Single3 p2)
{
const double Tiny = 0.000001f;
return(Abs(p1 * p2) < Tiny);
}
public static bool IsUnitVector(Single3 p)
{
const double Tiny = 0.000001f;
return(Abs(p.Length() - 1f) < Tiny);
}
