Implementing Vector3 Unit Tests

src/J3ML/LinearAlgebra/Vector3.cpp

@@ -269,7 +269,7 @@ namespace LinearAlgebra {
     }
 
     Vector3 Vector3::Sub(const Vector3 &lhs, const Vector3 &rhs) {
-        lhs.Sub(rhs);
+        return lhs.Sub(rhs);
     }
 
     Vector3 Vector3::Mul(float scalar) const {
@@ -288,5 +288,18 @@ namespace LinearAlgebra {
         return lhs.Div(rhs);
     }
 
+    Angle2D Vector3::AngleBetween(const Vector3 &rhs) const {
+        const auto Pi_x_180 = 180.f / M_PI;
+        auto dist = this->Distance(rhs);
+        float x = -(asinf((rhs.y - this->y) / dist));
+        float y = (atan2f(rhs.x - this->x,rhs.z - this->z));
+        return {x, y};
+    }
+
+    Angle2D Vector3::AngleBetween(const Vector3 &lhs, const Vector3 &rhs) // TODO: 3D Angle representation?
+    {
+        return lhs.AngleBetween(rhs);
+    }
+
 #pragma endregion
 }