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); }