Merge pull request #1666 from tombrazier/nm_filter

2 files changed, 185 insertions, 0 deletions

diff --git a/compat/hamilton-tools.cpp b/compat/hamilton-tools.cpp
new file mode 100644
index 00000000..eb0ef984
--- /dev/null
+++ b/compat/hamilton-tools.cpp
@@ -0,0 +1,109 @@
+#include "hamilton-tools.h"
+#include <cmath>
+
+tQuat QuatFromAngleAxe(const double angle, const tVector& axe)
+{
+    double a = TO_RAD * 0.5 * angle;
+    double d = sin(a) / VectorLength(axe);
+    return  ( tQuat (
+                        axe.v[0] * d,
+                        axe.v[1] * d,
+                        axe.v[2] * d,
+                        cos(a)
+                    )
+            );
+}
+		
+tQuat QuatMultiply(const tQuat& qL, const tQuat& qR)
+{
+    tQuat Q;
+    Q.x = qL.w*qR.x + qL.x*qR.w + qL.y*qR.z - qL.z*qR.y;
+    Q.y = qL.w*qR.y + qL.y*qR.w + qL.z*qR.x - qL.x*qR.z;
+    Q.z = qL.w*qR.z + qL.z*qR.w + qL.x*qR.y - qL.y*qR.x;
+    Q.w = qL.w*qR.w - qL.x*qR.x - qL.y*qR.y - qL.z*qR.z;
+    return(Q);
+}
+
+tQuat QuatFromYPR(const double YPR[])
+{
+    tQuat	Q, Qp, Qy;
+    Q  = QuatFromAngleAxe( -YPR[2], {0, 0, 1} ); //Roll,  Z axe
+    Qp = QuatFromAngleAxe( -YPR[1], {1, 0, 0} ); //Pitch, X axe
+    Qy = QuatFromAngleAxe( -YPR[0], {0, 1, 0} ); //Yaw,   Y axe
+
+    Q  = QuatMultiply(Qp, Q);
+    return(QuatMultiply(Qy, Q));
+}
+
+void Normalize(tQuat& Q)
+{
+    double m = sqrt(Q.x*Q.x + Q.y*Q.y + Q.z*Q.z + Q.w*Q.w);
+    if (m > EPSILON)
+    {
+        m   = 1 / m;
+        Q.x = Q.x * m;
+        Q.y = Q.y * m;
+        Q.z = Q.z * m;
+        Q.w = Q.w * m;
+    }
+    else Q = tQuat(0, 0, 0, 1);
+}
+
+tQuat Slerp(const tQuat& S, const tQuat& D, const double alpha)
+{
+    // calc cosine of half angle
+    double cosin = S.x*D.x + S.y*D.y + S.z*D.z + S.w*D.w;
+
+    // select nearest rotation direction
+    tQuat Q;
+    if (cosin < 0)
+    {
+        cosin = - cosin;
+        Q.x   = - D.x;
+        Q.y   = - D.y;
+        Q.z   = - D.z;
+        Q.w   = - D.w;
+    }
+    else Q = D;
+
+    // calculate coefficients
+    double scale0, scale1;
+    if ((1.0 - cosin) > EPSILON)
+    {
+        double omega = acos(cosin);
+        double sinus = 1 / sin(omega);
+        scale0 = sin((1.0 - alpha) * omega) * sinus;
+        scale1 = sin(alpha * omega)* sinus;
+    }
+    else
+    {
+        scale0 = 1.0 - alpha;
+        scale1 = alpha;
+    }
+
+    Q.x = scale0 * S.x + scale1 * Q.x;
+    Q.y = scale0 * S.y + scale1 * Q.y;
+    Q.z = scale0 * S.z + scale1 * Q.z;
+    Q.w = scale0 * S.w + scale1 * Q.w;
+	
+    Normalize(Q);
+
+    return( Q );
+}
+
+void QuatToYPR(const tQuat& Q, double YPR[])
+{
+    const double xx = Q.x * Q.x;
+    const double xy = Q.x * Q.y;
+    const double xz = Q.x * Q.z;
+    const double xw = Q.x * Q.w;
+    const double yy = Q.y * Q.y;
+    const double yz = Q.y * Q.z;
+    const double yw = Q.y * Q.w;
+    const double zz = Q.z * Q.z;
+    const double zw = Q.z * Q.w;
+
+    YPR[0] = TO_DEG * ( -atan2( 2 * ( xz + yw ), 1 - 2 * ( xx + yy ) ));
+    YPR[1] = TO_DEG * (  asin ( 2 * ( yz - xw ) ));
+    YPR[2] = TO_DEG * ( -atan2( 2 * ( xy + zw ), 1 - 2 * ( xx + zz ) ));
+}
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[]);