Added new vector and quaternion functions and inlined many of them

2 files changed, 53 insertions, 33 deletions

diff --git a/compat/hamilton-tools.cpp b/compat/hamilton-tools.cpp
index dfbb4630..eb0ef984 100644
--- a/compat/hamilton-tools.cpp
+++ b/compat/hamilton-tools.cpp
@@ -1,27 +1,6 @@
 #include "hamilton-tools.h"
 #include <cmath>
 
-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]));
-}
-		
-double sqr(const double v) { return(v*v); }
-
-double VectorDistance(const double v1[], const tVector& v2)
-{
-    return(sqrt(sqr(v2.v[0]-v1[0])+sqr(v2.v[1]-v1[1])+sqr(v2.v[2]-v1[2])));
-}
-
-tVector Lerp(const tVector& s, const double d[], const double alpha)
-{
-    tVector V;
-    V.v[0] = s.v[0] + (d[0] - s.v[0]) * alpha;
-    V.v[1] = s.v[1] + (d[1] - s.v[1]) * alpha;
-    V.v[2] = s.v[2] + (d[2] - s.v[2]) * alpha;
-    return(V);
-}	
-
 tQuat QuatFromAngleAxe(const double angle, const tVector& axe)
 {
     double a = TO_RAD * 0.5 * angle;
@@ -45,11 +24,6 @@ tQuat QuatMultiply(const tQuat& qL, const tQuat& qR)
     return(Q);
 }
 
-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 QuatFromYPR(const double YPR[])
 {
     tQuat	Q, Qp, Qy;
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[]);