diff options
Diffstat (limited to 'facetracknoir')
| -rw-r--r-- | facetracknoir/quat.hpp (renamed from facetracknoir/rotation.h) | 22 | ||||
| -rw-r--r-- | facetracknoir/tracker.h | 2 | ||||
| -rw-r--r-- | facetracknoir/tracker_types.h | 14 |
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; } |