diff options
Diffstat (limited to 'facetracknoir/quat.hpp')
| -rw-r--r-- | facetracknoir/quat.hpp | 26 |
1 files changed, 13 insertions, 13 deletions
diff --git a/facetracknoir/quat.hpp b/facetracknoir/quat.hpp index 1e268963..6d777b28 100644 --- a/facetracknoir/quat.hpp +++ b/facetracknoir/quat.hpp @@ -14,7 +14,7 @@ private: 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() : a(1.),b(0.),c(0.),d(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) {} @@ -27,12 +27,12 @@ public: 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); + const double sin_phi = sin(roll/2.); + const double cos_phi = cos(roll/2.); + const double sin_the = sin(pitch/2.); + const double cos_the = cos(pitch/2.); + const double sin_psi = sin(yaw/2.); + const double cos_psi = cos(yaw/2.); 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; @@ -42,9 +42,9 @@ public: 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)); + roll = atan2(2.*(a*b + c*d), 1. - 2.*(b*b + c*c)); + pitch = asin(2.*(a*c - b*d)); + yaw = atan2(2.*(a*d + b*c), 1. - 2.*(c*c + d*d)); } void to_euler_degrees(double& yaw, double& pitch, double& roll) const @@ -59,8 +59,8 @@ public: { 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); + 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); } }; |