summaryrefslogtreecommitdiffhomepage
path: root/opentrack/quat.hpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-24 04:55:35 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-24 07:04:17 +0200
commit85aa1942a789c3df99a3a5b1a87485cc050be28e (patch)
tree1dfdecb7e7a10694557687854606baa583a4bfb2 /opentrack/quat.hpp
parent69eb5f7b8c712482bb960f49e5c9ec717a902858 (diff)
Rotation basis algebra for centering
Reported-by: @doveman months ago, many times Issue: #63 @dbaarda please confirm that- or -whether- it makes any sense. Issue: #86 It could be either worse or better than before. Please specify. Sadly, no time to plug the videos as camera input to PT as of yet. Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
Diffstat (limited to 'opentrack/quat.hpp')
-rw-r--r--opentrack/quat.hpp48
1 files changed, 29 insertions, 19 deletions
diff --git a/opentrack/quat.hpp b/opentrack/quat.hpp
index 6d777b28..aec87f4c 100644
--- a/opentrack/quat.hpp
+++ b/opentrack/quat.hpp
@@ -12,19 +12,19 @@ class Quat {
private:
static constexpr double pi = 3.141592653;
static constexpr double r2d = 180./pi;
- double a,b,c,d; // quaternion coefficients
+ double _0,_1,_2,_3; // quaternion coefficients
public:
- 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) {}
+ Quat() : _0(1.),_1(0.),_2(0.),_3(0.) {}
+ Quat(double yaw, double pitch, double roll) { *this = from_euler_rads(yaw, pitch, roll); }
+ Quat(double a, double b, double c, double d) : _0(a),_1(b),_2(c),_3(d) {}
- Quat inv(){
- return Quat(a,-b,-c, -d);
+ Quat inv() const {
+ return Quat(_0,-_1,-_2, -_3);
}
// conversions
// see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
- void from_euler_rads(double yaw, double pitch, double roll)
+ static Quat from_euler_rads(double yaw, double pitch, double roll)
{
const double sin_phi = sin(roll/2.);
@@ -34,17 +34,21 @@ public:
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;
- 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;
+ Quat q;
+
+ q._0 = cos_phi*cos_the*cos_psi + sin_phi*sin_the*sin_psi;
+ q._1 = sin_phi*cos_the*cos_psi - cos_phi*sin_the*sin_psi;
+ q._2 = cos_phi*sin_the*cos_psi + sin_phi*cos_the*sin_psi;
+ q._3 = cos_phi*cos_the*sin_psi - sin_phi*sin_the*cos_psi;
+
+ return q;
}
void to_euler_rads(double& yaw, double& pitch, double& roll) const
{
- 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));
+ roll = atan2(2.*(_0*_1 + _2*_3), 1. - 2.*(_1*_1 + _2*_2));
+ pitch = asin(2.*(_0*_2 - _1*_3));
+ yaw = atan2(2.*(_0*_3 + _1*_2), 1. - 2.*(_2*_2 + _3*_3));
}
void to_euler_degrees(double& yaw, double& pitch, double& roll) const
@@ -58,9 +62,15 @@ public:
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);
- }
+ const Quat ret(A._0*B._0 - A._1*B._1 - A._2*B._2 - A._3*B._3,
+ A._0*B._1 + A._1*B._0 + A._2*B._3 - A._3*B._2,
+ A._0*B._2 - A._1*B._3 + A._2*B._0 + A._3*B._1,
+ A._0*B._3 + A._1*B._2 - A._2*B._1 + A._3*B._0);
+ const double mag_2 = ret._0 * ret._0 +
+ ret._1 * ret._1 +
+ ret._2 * ret._2 +
+ ret._3 * ret._3;
+ const double inv_mag = 1./sqrt(mag_2);
+ return Quat(1., ret._1 * inv_mag, ret._2 * inv_mag, ret._3 * inv_mag);
+ }
};