compat/euler: add incomplete & untested quat code

3 files changed, 105 insertions, 0 deletions

diff --git a/compat/euler.cpp b/compat/euler.cpp
index ab119d3e..e48d977b 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -93,4 +93,43 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
     }
 }
 
+// from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+Quat matrix_to_quat(const rmat& M)
+{
+    Quat q(1, 0, 0, 0);
+
+    using std::sqrt;
+
+    double trace = M(0, 0) + M(1, 1) + M(2, 2); // I removed + 1.0; see discussion with Ethan
+    if( trace > 0 ) {// I changed M_EPSILON to 0
+        double s = .5 / std::sqrt(trace + 1);
+        q.w() = .25 / s;
+        q.x() = ( M(2, 1) - M(1, 2) ) * s;
+        q.y() = ( M(0, 2) - M(2, 0) ) * s;
+        q.z() = ( M(1, 0) - M(0, 1) ) * s;
+    } else {
+        if ( M(0, 0) > M(1, 1) && M(0, 0) > M(2, 2) ) {
+            double s = 2.0 * sqrt( 1.0 + M(0, 0) - M(1, 1) - M(2, 2));
+            q.w() = (M(2, 1) - M(1, 2) ) / s;
+            q.x() = .25 * s;
+            q.y() = (M(0, 1) + M(1, 0) ) / s;
+            q.z() = (M(0, 2) + M(2, 0) ) / s;
+        } else if (M(1, 1) > M(2, 2)) {
+            double s = 2.0 * sqrt( 1.0 + M(1, 1) - M(0, 0) - M(2, 2));
+            q.w() = (M(0, 2) - M(2, 0) ) / s;
+            q.x() = (M(0, 1) + M(1, 0) ) / s;
+            q.y() = .25 * s;
+            q.z() = (M(1, 2) + M(2, 1) ) / s;
+        } else {
+            double s = 2.0 * sqrt( 1.0 + M(2, 2) - M(0, 0) - M(1, 1) );
+            q.w() = (M(1, 0) - M(0, 1) ) / s;
+            q.x() = (M(0, 2) + M(2, 0) ) / s;
+            q.y() = (M(1, 2) + M(2, 1) ) / s;
+            q.z() = .25 * s;
+        }
+    }
+
+    return q;
+}
+
 } // end ns euler
diff --git a/compat/euler.hpp b/compat/euler.hpp
index c50cf052..4eb6e00c 100644
--- a/compat/euler.hpp
+++ b/compat/euler.hpp
@@ -22,4 +22,7 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
                                                    rmat& r_pitch,
                                                    rmat& r_yaw);
 
+Quat OTR_COMPAT_EXPORT matrix_to_quat(const rmat& M);
+//XXX TODO rmat OTR_COMPAT_EXPORT quat_to_matrix(const Quat<double>& Q);
+
 } // end ns euler
diff --git a/compat/simple-mat.hpp b/compat/simple-mat.hpp
index fbd0a75e..906c0969 100644
--- a/compat/simple-mat.hpp
+++ b/compat/simple-mat.hpp
@@ -273,3 +273,66 @@ Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other)
             ret(j, i) = self(j, i) * other;
     return ret;
 }
+
+template<typename num>
+class Quat : Mat<num, 4, 1>
+{
+    using quat = Quat<num>;
+
+    enum idx { qw, qx, qy, qz };
+
+    static quat _from_array(const num* data)
+    {
+        return quat(data[qw], data[qx], data[qy], data[qz]);
+    }
+
+    inline num elt(idx k) const { return operator()(k); }
+    inline num& elt(idx k) { return operator()(k); }
+public:
+    Quat(num w, num x, num y, num z) : Mat<num, 4, 1>(w, x, y, z)
+    {
+    }
+
+    Quat() : quat(1, 0, 0, 0) {}
+
+    static quat from_vector(const Mat<num, 4, 1>& data)
+    {
+        return _from_array(data);
+    }
+
+    static quat from_vector(const Mat<num, 1, 4>& data)
+    {
+        return _from_array(data);
+    }
+
+    quat normalized() const
+    {
+        const num x = elt(qx), y = elt(qy), z = elt(qz), w = elt(qw);
+        const num inv_n = 1./std::sqrt(x*x + y*y + z*z + w*w);
+
+        return Quat<num>(elt(qw) * inv_n,
+                         elt(qx) * inv_n,
+                         elt(qy) * inv_n,
+                         elt(qz) * inv_n);
+    }
+
+    quat operator*(const quat& q2)
+    {
+        const quat& OTR_RESTRICT q1 = *this;
+        return quat(-q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z() + q1.w() * q2.w(),
+                     q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y() + q1.w() * q2.x(),
+                    -q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x() + q1.w() * q2.y(),
+                     q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w() + q1.w() * q2.z());
+    }
+
+
+    inline num w() const { return elt(qw); }
+    inline num x() const { return elt(qx); }
+    inline num y() const { return elt(qy); }
+    inline num z() const { return elt(qz); }
+
+    inline num& w() { return elt(qw); }
+    inline num& x() { return elt(qx); }
+    inline num& y() { return elt(qy); }
+    inline num& z() { return elt(qz); }
+};