Implement Vector3::Orthonormalize() AreOrthonormal()

include/J3ML/LinearAlgebra/Vector3.h

@@ -87,7 +87,7 @@ public:
     // Constructs a new Vector3 with the value (X, Y, Z)
     Vector3(float X, float Y, float Z);
     Vector3(const Vector2& XY, float Z);
-    Vector3(const Vector3& rhs); // Copy Constructor
+    Vector3(const Vector3& rhs) = default; // Copy Constructor
     Vector3(Vector3&&) = default; // Move Constructor
     /// Constructs this float3 from a C array, to the value (data[0], data[1], data[2]).
     /** @param data An array containing three elements for x, y and z. This pointer may not be null. */
@@ -126,7 +126,19 @@ public:
     float At(int index) const;
     float &At(int index);
 
+    /// Makes the given vectors linearly independent and normalized in length.
+    /** This function directly follows the Gram-Schmidt procedure on the input vectors.
+        The vector a is first normalized, and vector b is modified to be perpendicular to a, and also normalized.
+        Finally, if specified, the vector c is adjusted to be perpendicular to a and b, and normalized.
+        @note If any of the input vectors is zero, then the resulting set of vectors cannot be made orthonormal.
+        @see Orthogonalize(), AreOrthogonal(), AreOrthonormal(). */
     static void Orthonormalize(Vector3& a, Vector3& b);
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
+
+    /// Returns true if the given vectors are orthogonal to each other and all of length 1.
+    /** @see Orthogonalize(), AreOrthogonal(), Orthonormalize(), AreCollinear(). */
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon = 1e-3f);
+    static bool AreOrthonormal(const Vector3& a, const Vector3& b, const Vector3& c, float epsilon = 1e-3f);
 
     Vector3 Abs() const;
     static Vector3 Abs(const Vector3& rhs);
@@ -134,9 +146,9 @@ public:
     /// Returns the DirectionVector for a given angle.
     static Vector3 Direction(const Vector3 &rhs) ;
 
-    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);
 
-    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon);
+
+
 
     Vector3 ProjectToNorm(const Vector3& direction) const;
 
@@ -226,8 +238,23 @@ public:
     static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);
 
     /// Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
-    Vector3 Normalize() const;
+    /// @note If the vector is zero and cannot be normalized, the vector (1, 0, 0) is returned, and an error message is printed.
-    static Vector3 Normalize(const Vector3& targ);
+    /// If you do not want to generate an error message on failure, but want to handle the failure yourself, use the Normalize() function instead.
+    /// @see Normalize()
+    Vector3 Normalized() const;
+    static Vector3 Normalized(const Vector3& targ);
+
+    /// Normalizes this Vector3.
+    /** In the case of failure, this vector is set to (1, 0, 0), so calling this function will never result in an
+        un-normalized vector.
+        @note If this function fails to normalize the vector, no error message is printed, the vector is set to (1,0,0) and
+        an error code 0 is returned. This is different than the behavior of the Normalized() function, which prints an
+        error if normalization fails.
+        @note This function operates in-place.
+        @return The old length of this vector, or 0 if normalization fails.
+        @see Normalized(). */
+    float Normalize();
+
 
     /// Linearly interpolates between two points.
     /// Interpolates between the points and b by the interpolant t.
@@ -290,6 +317,7 @@ public:
     Vector3& operator*=(float scalar);
     Vector3& operator/=(float scalar);
 
+
     void Set(float d, float d1, float d2);
 };
 

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -64,7 +64,7 @@ namespace J3ML::LinearAlgebra {
 
     Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z) {}
 
-    Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {}
+    //Vector3::Vector3(const Vector3& rhs) : x(rhs.x), y(rhs.y), z(rhs.z) {}
 
     Vector3& Vector3::operator=(const Vector3& rhs)
     {
@@ -178,7 +178,21 @@ namespace J3ML::LinearAlgebra {
         };
     }
 
-    Vector3 Vector3::Normalize() const
+    float Vector3::Normalize()
+    {
+        assert(IsFinite());
+        float length = Length();
+        if (length > 1e-6f)
+        {
+            *this *= 1.f / length;
+            return length;
+        } else {
+            Set(1.f, 0.f, 0.f); // We will always produce a normalized vector.
+            return 0; // But signal failure, so user knows we have generated an arbitrary normalization.
+        }
+    }
+
+    Vector3 Vector3::Normalized() const
     {
         if (Length() > 0)
             return {
@@ -248,8 +262,8 @@ namespace J3ML::LinearAlgebra {
         return lhs.Cross(rhs);
     }
 
-    Vector3 Vector3::Normalize(const Vector3 &targ) {
+    Vector3 Vector3::Normalized(const Vector3 &targ) {
-        return targ.Normalize();
+        return targ.Normalized();
     }
 
     Vector3 Vector3::Project(const Vector3 &lhs, const Vector3 &rhs) {
@@ -329,18 +343,18 @@ namespace J3ML::LinearAlgebra {
     }
 
     void Vector3::Orthonormalize(Vector3 &a, Vector3 &b) {
-        a = a.Normalize();
+        a = a.Normalized();
         b = b - b.ProjectToNorm(a);
-        b = b.Normalize();
+        b = b.Normalized();
     }
 
     void Vector3::Orthonormalize(Vector3 &a, Vector3 &b, Vector3 &c) {
-        a = a.Normalize();
+        a = a.Normalized();
         b = b - b.ProjectToNorm(a);
-        b = b.Normalize();
+        b = b.Normalized();
         c = c - c.ProjectToNorm(a);
         c = c - c.ProjectToNorm(b);
-        c = c.Normalize();
+        c = c.Normalized();
     }
 
     Vector3 Vector3::ProjectToNorm(const Vector3 &direction) const {
@@ -506,5 +520,14 @@ namespace J3ML::LinearAlgebra {
         return from.DistanceSquared(to);
     }
 
+    bool Vector3::AreOrthonormal(const Vector3 &a, const Vector3 &b, const Vector3 &c, float epsilon) {
+        return a.IsPerpendicular(b, epsilon) &&
+               a.IsPerpendicular(c, epsilon) &&
+               b.IsPerpendicular(c, epsilon) &&
+               a.IsNormalized(epsilon*epsilon) &&
+               b.IsNormalized(epsilon*epsilon) &&
+               c.IsNormalized(epsilon*epsilon);
+    }
+
 
 }