flush

3 files changed, 22 insertions, 16 deletions

diff --git a/facetracknoir/rotation.h b/facetracknoir/quat.hpp
index b3bb891e..1e268963 100644
--- a/facetracknoir/rotation.h
+++ b/facetracknoir/quat.hpp
@@ -9,10 +9,13 @@
 #include <cmath>
 
 class Quat {
-
+private:
+    static constexpr double pi = 3.141592653;
+    static constexpr double r2d = 180./pi;
+    double a,b,c,d; // quaternion coefficients
 public:
     Quat() : a(1.0),b(0.0),c(0.0),d(0.0) {}
-    Quat(double yaw, double pitch, double roll) { fromEuler(yaw, pitch, roll); }
+    Quat(double yaw, double pitch, double roll) { from_euler_rads(yaw, pitch, roll); }
     Quat(double a, double b, double c, double d) : a(a),b(b),c(c),d(d) {}
 
     Quat inv(){
@@ -21,7 +24,7 @@ public:
 
     // conversions
     // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
-    void fromEuler(double yaw, double pitch, double roll)
+    void from_euler_rads(double yaw, double pitch, double roll)
     {
 
         double sin_phi = sin(roll/2.0);
@@ -37,13 +40,21 @@ public:
         d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
     }
 
-    void toEuler(double& yaw, double& pitch, double& roll) const
+    void to_euler_rads(double& yaw, double& pitch, double& roll) const
     {
         roll = atan2(2.0*(a*b + c*d), 1.0 - 2.0*(b*b + c*c));
         pitch = asin(2.0*(a*c - b*d));
         yaw =  atan2(2.0*(a*d + b*c), 1.0 - 2.0*(c*c + d*d));
     }
 
+    void to_euler_degrees(double& yaw, double& pitch, double& roll) const
+    {
+        to_euler_rads(yaw, pitch, roll);
+        yaw *= r2d;
+        pitch *= r2d;
+        roll *= r2d;
+    }
+
     const Quat operator*(const Quat& B) const
     {
         const Quat& A = *this;
@@ -52,7 +63,4 @@ public:
                         A.a*B.c - A.b*B.d + A.c*B.a + A.d*B.b,
                         A.a*B.d + A.b*B.c - A.c*B.b + A.d*B.a);
     }
-
-private:
-    double a,b,c,d; // quaternion coefficients
 };
diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h
index 05ae4180..3d9a3858 100644
--- a/facetracknoir/tracker.h
+++ b/facetracknoir/tracker.h
@@ -22,8 +22,6 @@
 #include "facetracknoir/options.h"
 #include "facetracknoir/timer.hpp"
 
-
-
 class Tracker : protected QThread {
     Q_OBJECT
 private:
diff --git a/facetracknoir/tracker_types.h b/facetracknoir/tracker_types.h
index c667498e..02aacdcf 100644
--- a/facetracknoir/tracker_types.h
+++ b/facetracknoir/tracker_types.h
@@ -2,14 +2,14 @@
 
 #include <utility>
 #include <algorithm>
-#include "rotation.h"
-#include "plugin-api.hpp"
+#include "./quat.hpp"
+#include "./plugin-api.hpp"
 
 struct T6DOF {
 private:
-    static constexpr double PI = 3.14159265358979323846264;
-    static constexpr double D2R = PI/180.0;
-    static constexpr double R2D = 180.0/PI;
+    static constexpr double pi = 3.141592653;
+    static constexpr double d2r = pi/180.0;
+    static constexpr double r2d = 180./pi;
 
     double axes[6];
 public:
@@ -23,13 +23,13 @@ public:
 
     Quat quat() const
     {
-        return Quat(axes[Yaw]*D2R, axes[Pitch]*D2R, axes[Roll]*D2R);
+        return Quat(axes[Yaw]*d2r, axes[Pitch]*d2r, axes[Roll]*d2r);
     }
 
     static T6DOF fromQuat(const Quat& q)
     {
         T6DOF ret;
-        q.toEuler(ret(Yaw), ret(Pitch), ret(Roll));
+        q.to_euler_rads(ret(Yaw), ret(Pitch), ret(Roll));
         return ret;
     }