From cbabfdd99bc892311c0bee4143aa7242400d39ce Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 25 Oct 2014 21:22:07 +0200
Subject: Change rot order tait-bryan -> mat

Issue: 63
---
 opentrack/tracker.cpp | 38 +++++++++++++++++++-------------------
 1 file changed, 19 insertions(+), 19 deletions(-)

(limited to 'opentrack')

diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index 2a93898c..c0169a14 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -54,16 +54,16 @@ static cv::Vec3d rmat_to_euler(const cv::Matx33d& R)
     const double beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
     const double alpha = atan2( R(1,0), R(0,0));
     const double gamma = atan2( R(2,1), R(2,2));
-    return cv::Vec3d { alpha, -beta, gamma };
+    return cv::Vec3d { alpha, beta, gamma };
 }
 
 // tait-bryan angles, not euler
 static cv::Matx33d euler_to_rmat(const double* input)
 {
     static constexpr double pi = 3.141592653;
-    const auto H = input[1] * pi / -180;
-    const auto P = input[2] * pi / -180;
-    const auto B = input[0] * pi / 180;
+    const auto H = input[0] * pi / 180;
+    const auto P = input[1] * pi / 180;
+    const auto B = input[2] * pi / 180;
 
     const auto c1 = cos(H);
     const auto s1 = sin(H);
@@ -73,18 +73,18 @@ static cv::Matx33d euler_to_rmat(const double* input)
     const auto s3 = sin(B);
 
     double foo[] = {
-       // z
-       c1 * c3 - s1 * s2 * s3,
-       - s3 * c2,
-       s1 * c3 + c1 * s2 * s3,
-       // x
-       c1 * s3 + s1 * s2 * c3,
-       c3 * c2,
-       s3 * s1 - c1 * s2 * c3,
-       // y
-       - s1 * c2,
-       - s2,
-       c1 * c2,
+        // z
+        c1 * c2,
+        c1 * s2 * s3 - c3 * s1,
+        s1 * s3 + c1 * c3 * s2,
+        // y
+        c2 * s1,
+        c1 * c3 + s1 * s2 * s3,
+        c3 * s1 * s2 - c1 * s3,
+        // x
+        -s2,
+        c2 * s3,
+        c2 * c3
     };
 
     return cv::Matx33d(foo);
@@ -92,11 +92,11 @@ static cv::Matx33d euler_to_rmat(const double* input)
 
 void Tracker::t_compensate(const cv::Matx33d& rmat, const double* xyz, double* output, bool rz)
 {
-    const double xyz_[3] = { xyz[1], -xyz[0], xyz[2] };
+    const double xyz_[3] = { xyz[0], xyz[1], xyz[2] };
     cv::Matx31d tvec(xyz_);
     const cv::Matx31d ret = rmat * tvec;
-    output[0] = ret(1, 0);
-    output[1] = -ret(0, 0);
+    output[0] = ret(0, 0);
+    output[1] = ret(1, 0);
     if (!rz)
         output[2] = ret(2, 0);
 }
-- 
cgit v1.2.3