Added new vector and quaternion functions and inlined many of them

1 files changed, 53 insertions, 7 deletions

diff --git a/compat/hamilton-tools.h b/compat/hamilton-tools.h
index 73bc882c..d4604301 100644
--- a/compat/hamilton-tools.h
+++ b/compat/hamilton-tools.h
@@ -11,7 +11,50 @@ struct tVector
 {
     double v[3];
     tVector(double X = 0, double Y = 0, double Z = 0) {v[0]=X; v[1]=Y; v[2]=Z;}
-    tVector(double V[]) {v[0]=V[0]; v[1]=V[1]; v[2]=V[2];}
+    tVector(const double V[]) {v[0]=V[0]; v[1]=V[1]; v[2]=V[2];}
+
+    void operator=(const tVector& other)
+    {
+        std::copy(other.v, other.v + 3, v);
+    }
+    inline const tVector operator+(const tVector& other) const
+    {
+        return tVector(v[0] + other.v[0], v[1] + other.v[1], v[2] + other.v[2]);
+    }
+    void operator+=(const tVector& other)
+    {
+        v[0] += other.v[0];
+        v[1] += other.v[1];
+        v[2] += other.v[2];
+    }
+    const tVector operator-(const tVector& other) const
+    {
+        return tVector(v[0] - other.v[0], v[1] - other.v[1], v[2] - other.v[2]);
+    }
+    void operator-=(const tVector& other)
+    {
+        v[0] -= other.v[0];
+        v[1] -= other.v[1];
+        v[2] -= other.v[2];
+    }
+    const tVector operator*(double scalar) const
+    {
+        return tVector(v[0] * scalar, v[1] * scalar, v[2] * scalar);
+    }
+    void operator*=(double scalar)
+    {
+        v[0] *= scalar;
+        v[1] *= scalar;
+        v[2] *= scalar;
+    }
+    const tVector operator/(double scalar) const
+    {
+        return *this * (1.0 / scalar);
+    }
+    void operator/= (double scalar)
+    {
+        *this *= 1.0 / scalar;
+    }
 };
 
 struct tQuat
@@ -21,9 +64,12 @@ struct tQuat
         {x = X; y = Y; z = Z; w = W;}
 };
 
-double  OTR_COMPAT_EXPORT VectorDistance(const double v1[], const tVector& v2);
-tVector OTR_COMPAT_EXPORT Lerp          (const tVector& s, const double d[], const double alpha);
-tQuat   OTR_COMPAT_EXPORT QuatFromYPR   (const double YPR[]);
-double  OTR_COMPAT_EXPORT AngleBetween  (const tQuat& S, const tQuat& D);
-tQuat   OTR_COMPAT_EXPORT Slerp         (const tQuat& S, const tQuat& D, const double alpha);
-void    OTR_COMPAT_EXPORT QuatToYPR     (const tQuat& Q, double YPR[]);
+inline double VectorLength(const tVector& v) { return sqrt(v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2]); }
+inline double VectorDistance(const tVector& v1, const tVector& v2) { return VectorLength(v2 - v1); }
+inline tVector Lerp(const tVector& s, const tVector& d, const double alpha) { return s + (d - s) * alpha; }
+tQuat OTR_COMPAT_EXPORT QuatFromYPR(const double YPR[]);
+tQuat OTR_COMPAT_EXPORT QuatMultiply(const tQuat& qL, const tQuat& qR);
+inline tQuat QuatDivide(const tQuat& qL, const tQuat& qR) { return QuatMultiply(qL, tQuat(-qR.x, -qR.y, -qR.z, qR.w)); }
+inline double AngleBetween(const tQuat& S, const tQuat& D) { return TO_DEG * 2 * acos(fabs(S.x * D.x + S.y * D.y + S.z * D.z + S.w * D.w)); }
+tQuat OTR_COMPAT_EXPORT Slerp(const tQuat& S, const tQuat& D, const double alpha);
+void OTR_COMPAT_EXPORT QuatToYPR(const tQuat& Q, double YPR[]);