From c63c29d0e09a939c45f2a209ec7c6f0998138589 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 12 Aug 2016 17:07:38 +0200
Subject: logic/simple-mat: add untested tait-bryan rotation composition
 function

---
 opentrack-logic/simple-mat.cpp | 42 +++++++++++++++++++++++++++++++++++++++++-
 opentrack-logic/simple-mat.hpp |  8 ++++++--
 2 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/opentrack-logic/simple-mat.cpp b/opentrack-logic/simple-mat.cpp
index 1aaeb296..8c97f192 100644
--- a/opentrack-logic/simple-mat.cpp
+++ b/opentrack-logic/simple-mat.cpp
@@ -4,7 +4,7 @@
 
 namespace euler {
 
-euler_t OPENTRACK_LOGIC_EXPORT rmat_to_euler(const dmat<3, 3>& R)
+euler_t OPENTRACK_LOGIC_EXPORT rmat_to_euler(const rmat& R)
 {
     using std::atan2;
     using std::sqrt;
@@ -54,4 +54,44 @@ rmat OPENTRACK_LOGIC_EXPORT euler_to_rmat(const euler_t& input)
                 );
 }
 
+// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations
+void OPENTRACK_LOGIC_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-logic/simple-mat.hpp b/opentrack-logic/simple-mat.hpp
index 09de6004..514e845f 100644
--- a/opentrack-logic/simple-mat.hpp
+++ b/opentrack-logic/simple-mat.hpp
@@ -269,7 +269,11 @@ using euler_t = dmat<3, 1>;
 
 rmat OPENTRACK_LOGIC_EXPORT euler_to_rmat(const euler_t& input);
 
-// http://stackoverflow.com/a/18436193
-euler_t OPENTRACK_LOGIC_EXPORT rmat_to_euler(const dmat<3, 3>& R);
+euler_t OPENTRACK_LOGIC_EXPORT rmat_to_euler(const rmat& R);
+
+void OPENTRACK_LOGIC_EXPORT tait_bryan_to_matrices(const euler_t& input,
+                                                   rmat& r_roll,
+                                                   rmat& r_pitch,
+                                                   rmat& r_yaw);
 
 } // end ns euler
-- 
cgit v1.2.3