api: backport simple-mat as whole file

2 files changed, 195 insertions, 117 deletions

diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp
new file mode 100644
index 00000000..6a53e6ad
--- /dev/null
+++ b/opentrack/simple-mat.cpp
@@ -0,0 +1,96 @@
+#include "simple-mat.hpp"
+#include <cmath>
+
+namespace euler {
+
+euler_t OPENTRACK_API_EXPORT rmat_to_euler(const rmat& R)
+{
+    using std::atan2;
+    using std::sqrt;
+
+    const double cy = sqrt(R(2,2)*R(2, 2) + R(2, 1)*R(2, 1));
+    const bool large_enough = cy > 1e-10;
+    if (large_enough)
+        return euler_t(atan2(-R(1, 0), R(0, 0)),
+                       atan2(R(2, 0), cy),
+                       atan2(-R(2, 1), R(2, 2)));
+    else
+        return euler_t(atan2(R(0, 1), R(1, 1)),
+                       atan2(R(2, 0), cy),
+                       0);
+}
+
+// tait-bryan angles, not euler
+rmat OPENTRACK_API_EXPORT euler_to_rmat(const euler_t& input)
+{
+    const double H = -input(0);
+    const double P = -input(1);
+    const double B = -input(2);
+
+    using std::cos;
+    using std::sin;
+
+    const auto c1 = cos(H);
+    const auto s1 = sin(H);
+    const auto c2 = cos(P);
+    const auto s2 = sin(P);
+    const auto c3 = cos(B);
+    const auto s3 = sin(B);
+
+    return rmat(
+                // z
+                c1 * c2,
+                c1 * s2 * s3 - c3 * s1,
+                s1 * s3 + c1 * c3 * s2,
+                // y
+                c2 * s1,
+                c1 * c3 + s1 * s2 * s3,
+                c3 * s1 * s2 - c1 * s3,
+                // x
+                -s2,
+                c2 * s3,
+                c2 * c3
+                );
+}
+
+// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations
+void OPENTRACK_API_EXPORT tait_bryan_to_matrices(const euler_t& input,
+                                                 rmat& r_roll,
+                                                 rmat& r_pitch,
+                                                 rmat& r_yaw)
+{
+    using std::cos;
+    using std::sin;
+
+    {
+        const double phi = -input(2);
+        const double sin_phi = sin(phi);
+        const double cos_phi = cos(phi);
+
+        r_roll = rmat(1, 0, 0,
+                      0, cos_phi, -sin_phi,
+                      0, sin_phi, cos_phi);
+    }
+
+    {
+        const double theta = input(1);
+        const double sin_theta = sin(theta);
+        const double cos_theta = cos(theta);
+
+        r_pitch = rmat(cos_theta, 0, -sin_theta,
+                       0, 1, 0,
+                       sin_theta, 0, cos_theta);
+    }
+
+    {
+        const double psi = -input(0);
+        const double sin_psi = sin(psi);
+        const double cos_psi = cos(psi);
+
+        r_yaw = rmat(cos_psi, -sin_psi, 0,
+                     sin_psi, cos_psi, 0,
+                     0, 0, 1);
+    }
+}
+
+} // end ns euler
diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp
index 4f885f4f..e70c24a8 100644
--- a/opentrack/simple-mat.hpp
+++ b/opentrack/simple-mat.hpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2014-2016, Stanislaw Halik <sthalik@misaki.pl>
 
  * Permission to use, copy, modify, and/or distribute this
  * software for any purpose with or without fee is hereby granted,
@@ -7,14 +7,16 @@
  */
 
 #pragma once
-#include <algorithm>
+
+#include "export.hpp"
+
 #include <initializer_list>
 #include <type_traits>
-#include <cmath>
+#include <utility>
 
 namespace {
     // last param to fool SFINAE into overloading
-    template<int i, int j, int ignored>
+    template<int i, int j, int>
     struct equals
     {
         enum { value = i == j };
@@ -52,75 +54,75 @@ namespace {
 template<typename num, int h_, int w_>
 class Mat
 {
-#ifdef __GNUC__
-    __restrict
-#endif
-    num data[h_][w_];
-
     static_assert(h_ > 0 && w_ > 0, "must have positive mat dimensions");
-
-    Mat(std::initializer_list<num>&& xs) = delete;
+    num data[h_][w_];
 
 public:
-
-    // parameters w_ and h_ are rebound so that SFINAE occurs
-    // removing them causes a compile-time error -sh 20150811
-    
     template<int Q = w_> typename std::enable_if<equals<Q, 1, 0>::value, num>::type
     inline operator()(int i) const { return data[i][0]; }
-    
+
     template<int P = h_> typename std::enable_if<equals<P, 1, 1>::value, num>::type
     inline operator()(int i) const { return data[0][i]; }
-    
+
     template<int Q = w_> typename std::enable_if<equals<Q, 1, 2>::value, num&>::type
     inline operator()(int i) { return data[i][0]; }
-    
+
     template<int P = h_> typename std::enable_if<equals<P, 1, 3>::value, num&>::type
     inline operator()(int i) { return data[0][i]; }
-    
+
+#define OPENTRACK_ASSERT_SWIZZLE static_assert(P == h_ && Q == w_, "")
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num>::type
-    inline x() const { return operator()(0); }
-    
+    x() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num>::type
-    inline y() const { return operator()(1); }
-    
+    y() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num>::type
-    inline z() const { return operator()(2); }
-    
+    z() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num>::type
-    inline w() const { return operator()(3); }
-    
+    w() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num&>::type
-    inline x() { return operator()(0); }
-    
+    x() { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num&>::type
-    inline y() { return operator()(1); }
-    
+    y() { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num&>::type
-    inline z() { return operator()(2); }
-    
+    z() { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); }
+
     template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num&>::type
-    inline w() { return operator()(3); }
-    
+    w() { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); }
+    // parameters w_ and h_ are rebound so that SFINAE occurs
+    // removing them causes a compile-time error -sh 20150811
+
     template<int R, int S, int P = h_, int Q = w_>
     typename std::enable_if<is_vector_pair<R, S, P, Q>::value, num>::type
-    dot(const Mat<num, R, S>& p2) const {
+    dot(const Mat<num, R, S>& p2) const
+    {
+        static_assert(P == h_ && Q == w_, "");
+
         num ret = 0;
         constexpr int len = vector_len<R, S>::value;
         for (int i = 0; i < len; i++)
             ret += operator()(i) * p2(i);
         return ret;
     }
-    
+
     template<int R, int S, int P = h_, int Q = w_>
     typename std::enable_if<is_dim3<P, Q, R, S>::value, Mat<num, is_dim3<P, Q, R, S>::P, is_dim3<P, Q, R, S>::Q>>::type
     cross(const Mat<num, R, S>& p2) const
     {
-        return Mat<num, R, S>(y() * p2.z() - p2.y() * z(),
-                              p2.x() * z() - x() * p2.z(),
-                              x() * p2.y() - y() * p2.x());
+        static_assert(P == h_ && Q == w_, "");
+        decltype(*this)& p1 = *this;
+
+        return Mat<num, R, S>(p1.y() * p2.z() - p2.y() * p1.z(),
+                              p2.x() * p1.z() - p1.x() * p2.z(),
+                              p1.x() * p2.y() - p1.y() * p2.x());
     }
-    
+
     Mat<num, h_, w_> operator+(const Mat<num, h_, w_>& other) const
     {
         Mat<num, h_, w_> ret;
@@ -129,7 +131,7 @@ public:
                 ret(j, i) = data[j][i] + other.data[j][i];
         return ret;
     }
-    
+
     Mat<num, h_, w_> operator-(const Mat<num, h_, w_>& other) const
     {
         Mat<num, h_, w_> ret;
@@ -138,7 +140,7 @@ public:
                 ret(j, i) = data[j][i] - other.data[j][i];
         return ret;
     }
-    
+
     Mat<num, h_, w_> operator+(const num& other) const
     {
         Mat<num, h_, w_> ret;
@@ -147,7 +149,7 @@ public:
                 ret(j, i) = data[j][i] + other;
         return ret;
     }
-    
+
     Mat<num, h_, w_> operator-(const num& other) const
     {
         Mat<num, h_, w_> ret;
@@ -156,16 +158,7 @@ public:
                 ret(j, i) = data[j][i] - other;
         return ret;
     }
-    
-    Mat<num, h_, w_> operator*(const num other) const
-    {
-        Mat<num, h_, w_> ret;
-        for (int j = 0; j < h_; j++)
-            for (int i = 0; i < w_; i++)
-                ret(j, i) = data[j][i] * other;
-        return ret;
-    }
-    
+
     template<int p>
     Mat<num, h_, p> operator*(const Mat<num, w_, p>& other) const
     {
@@ -187,11 +180,11 @@ public:
              typename = typename std::enable_if<is_arglist_correct<num, h__, w__, ts...>::value>::type>
     Mat(const ts... xs)
     {
-        const std::initializer_list<num> init = { static_cast<num>(xs)... };
-        auto iter = init.begin();
-        for (int j = 0; j < h_; j++)
-            for (int i = 0; i < w_; i++)
-                data[j][i] = *iter++;
+        static_assert(h__ == h_ && w__ == w_, "");
+
+        std::initializer_list<num> init = { static_cast<num>(xs)... };
+
+        *this = Mat(std::move(init));
     }
 
     Mat()
@@ -208,13 +201,25 @@ public:
                 data[j][i] = mem[i*h_+j];
     }
 
-    Mat(num* mem) : Mat(const_cast<const num*>(mem)) {}
+    Mat(std::initializer_list<num>&& init)
+    {
+        auto iter = init.begin();
+        for (int j = 0; j < h_; j++)
+            for (int i = 0; i < w_; i++)
+                data[j][i] = *iter++;
+    }
+
+    operator num*() { return reinterpret_cast<num*>(data); }
+    operator const num*() const { return reinterpret_cast<const num*>(data); }
 
     // XXX add more operators as needed, third-party dependencies mostly
     // not needed merely for matrix algebra -sh 20141030
 
-    static Mat<num, h_, h_> eye()
+    template<int h__ = h_>
+    static typename std::enable_if<h_ == w_, Mat<num, h__, h__>>::type eye()
     {
+        static_assert(h_ == h__, "");
+
         Mat<num, h_, h_> ret;
         for (int j = 0; j < h_; j++)
             for (int i = 0; i < w_; i++)
@@ -236,61 +241,38 @@ public:
 
         return ret;
     }
-    
-    template<int h__, int w__> using dmat = Mat<double, h__, w__>;
-    
-    // http://stackoverflow.com/a/18436193
-    static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R)
-    {
-        static constexpr double pi = 3.141592653;
-        const double pitch_1 = asin(-R(0, 2));
-        const double pitch_2 = pi - pitch_1;
-        const double cos_p1 = cos(pitch_1), cos_p2 = cos(pitch_2);
-        const double roll_1 = atan2(R(1, 2) / cos_p1, R(2, 2) / cos_p1);
-        const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2);
-        const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1);
-        const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2);
-        if (std::abs(pitch_1) + std::abs(roll_1) + std::abs(yaw_1) > std::abs(pitch_2) + std::abs(roll_2) + std::abs(yaw_2))
-        {
-            bool fix_neg_pitch = pitch_1 < 0;
-            return dmat<3, 1>(yaw_2, std::fmod(fix_neg_pitch ? -pi - pitch_1 : pitch_2, pi), roll_2);
-        }
-        else
-            return dmat<3, 1>(yaw_1, pitch_1, roll_1);
-    }
-    
-    // tait-bryan angles, not euler
-    static dmat<3, 3> euler_to_rmat(const double* input)
-    {
-        static constexpr double pi = 3.141592653;
-        auto H = input[0] * pi / 180;
-        auto P = input[1] * pi / 180;
-        auto B = input[2] * pi / 180;
-    
-        const auto c1 = cos(H);
-        const auto s1 = sin(H);
-        const auto c2 = cos(P);
-        const auto s2 = sin(P);
-        const auto c3 = cos(B);
-        const auto s3 = sin(B);
-    
-        double foo[] = {
-            // z
-            c1 * c2,
-            c1 * s2 * s3 - c3 * s1,
-            s1 * s3 + c1 * c3 * s2,
-            // y
-            c2 * s1,
-            c1 * c3 + s1 * s2 * s3,
-            c3 * s1 * s2 - c1 * s3,
-            // x
-            -s2,
-            c2 * s3,
-            c2 * c3
-        };
-    
-        return dmat<3, 3>(foo);
-    }
 };
-   
+
 template<int h_, int w_> using dmat = Mat<double, h_, w_>;
+
+template<typename num, int h, int w>
+Mat<num, h, w> operator*(num scalar, const Mat<num, h, w>& mat)
+{
+    return mat * scalar;
+}
+
+template<typename num, int h_, int w_>
+Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other)
+{
+    Mat<num, h_, w_> ret;
+    for (int j = 0; j < h_; j++)
+        for (int i = 0; i < w_; i++)
+            ret(j, i) = self(j, i) * other;
+    return ret;
+}
+
+namespace euler {
+
+using rmat = dmat<3, 3>;
+using euler_t = dmat<3, 1>;
+
+rmat OPENTRACK_API_EXPORT euler_to_rmat(const euler_t& input);
+
+euler_t OPENTRACK_API_EXPORT rmat_to_euler(const rmat& R);
+
+void OPENTRACK_API_EXPORT tait_bryan_to_matrices(const euler_t& input,
+                                                 rmat& r_roll,
+                                                 rmat& r_pitch,
+                                                 rmat& r_yaw);
+
+} // end ns euler