Implement D3DOrtho

include/J3ML/LinearAlgebra/Vector3.h

@@ -30,6 +30,33 @@ public:
     static const Vector3 Backward;
     static const Vector3 NaN;
 
+    static void Orthonormalize(Vector3& a, Vector3& b)
+    {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+    }
+
+    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c)
+    {
+        a = a.Normalize();
+        b = b - b.ProjectToNorm(a);
+        b = b.Normalize();
+        c = c - c.ProjectToNorm(a);
+        c = c - c.ProjectToNorm(b);
+        c = c.Normalize();
+    }
+
+    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
+    {
+
+    }
+
+    Vector3 ProjectToNorm(const Vector3& direction)
+    {
+        return direction * this->Dot(direction);
+    }
+
     float GetX() const;
     float GetY() const;
     float GetZ() const;