Merge pull request #1666 from tombrazier/nm_filter

1 files changed, 76 insertions, 0 deletions

diff --git a/compat/hamilton-tools.h b/compat/hamilton-tools.h
new file mode 100644
index 00000000..885e9f79
--- /dev/null
+++ b/compat/hamilton-tools.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include <algorithm>
+#include "export.hpp"
+#include "compat/math.hpp"
+
+constexpr double TO_RAD = (M_PI / 180);
+constexpr double TO_DEG = (180 / M_PI);
+constexpr double EPSILON = 1e-30;
+
+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(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
+{
+    double x, y, z, w;
+    tQuat(double X = 0, double Y = 0, double Z = 0, double W = 1)
+        {x = X; y = Y; z = Z; w = W;}
+};
+
+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[]);