From cfffa29e29db6b2234c7f534b1ebcd612b7f4914 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 5 Oct 2014 01:22:11 +0200 Subject: flush --- facetracknoir/quat.hpp | 66 +++++++++++++++++++++++++++++++++++++++++++ facetracknoir/rotation.h | 58 ------------------------------------- facetracknoir/tracker.h | 2 -- facetracknoir/tracker_types.h | 14 ++++----- 4 files changed, 73 insertions(+), 67 deletions(-) create mode 100644 facetracknoir/quat.hpp delete mode 100644 facetracknoir/rotation.h (limited to 'facetracknoir') diff --git a/facetracknoir/quat.hpp b/facetracknoir/quat.hpp new file mode 100644 index 00000000..1e268963 --- /dev/null +++ b/facetracknoir/quat.hpp @@ -0,0 +1,66 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once +#include + +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) { 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(){ + return Quat(a,-b,-c, -d); + } + + // conversions + // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + void from_euler_rads(double yaw, double pitch, double roll) + { + + double sin_phi = sin(roll/2.0); + double cos_phi = cos(roll/2.0); + double sin_the = sin(pitch/2.0); + double cos_the = cos(pitch/2.0); + double sin_psi = sin(yaw/2.0); + double cos_psi = cos(yaw/2.0); + + a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi; + b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi; + c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi; + d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi; + } + + 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; + return Quat(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication + A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c, + 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); + } +}; diff --git a/facetracknoir/rotation.h b/facetracknoir/rotation.h deleted file mode 100644 index b3bb891e..00000000 --- a/facetracknoir/rotation.h +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2012 Patrick Ruoff - * - * Permission to use, copy, modify, and/or distribute this software for any - * purpose with or without fee is hereby granted, provided that the above - * copyright notice and this permission notice appear in all copies. - */ - -#pragma once -#include - -class Quat { - -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 a, double b, double c, double d) : a(a),b(b),c(c),d(d) {} - - Quat inv(){ - return Quat(a,-b,-c, -d); - } - - // conversions - // see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - void fromEuler(double yaw, double pitch, double roll) - { - - double sin_phi = sin(roll/2.0); - double cos_phi = cos(roll/2.0); - double sin_the = sin(pitch/2.0); - double cos_the = cos(pitch/2.0); - double sin_psi = sin(yaw/2.0); - double cos_psi = cos(yaw/2.0); - - a = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi; - b = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi; - c = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi; - d = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi; - } - - void toEuler(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)); - } - - const Quat operator*(const Quat& B) const - { - const Quat& A = *this; - return Quat(A.a*B.a - A.b*B.b - A.c*B.c - A.d*B.d, // quaternion multiplication - A.a*B.b + A.b*B.a + A.c*B.d - A.d*B.c, - 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 #include -#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; } -- cgit v1.2.3