From 0284b597bf018f34b92e78ea0711afb812208287 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 14 Dec 2013 19:18:57 +0100
Subject: don't compensate Z axis as an option

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
---
 facetracknoir/ftnoir_curves.ui | 4 ++--
 facetracknoir/tracker.cpp      | 9 +++++----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/facetracknoir/ftnoir_curves.ui b/facetracknoir/ftnoir_curves.ui
index 87c449ba..77df9a20 100644
--- a/facetracknoir/ftnoir_curves.ui
+++ b/facetracknoir/ftnoir_curves.ui
@@ -746,7 +746,7 @@
         <rect>
          <x>10</x>
          <y>140</y>
-         <width>211</width>
+         <width>291</width>
          <height>81</height>
         </rect>
        </property>
@@ -782,7 +782,7 @@
            <string notr="true"/>
           </property>
           <property name="text">
-           <string>Reverse Z axis</string>
+           <string>Disable Z axis compensation</string>
           </property>
          </widget>
         </item>
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 0c5501d4..590d44bf 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -53,7 +53,6 @@ static void get_curve(double pos, double& out, THeadPoseDOF& axis) {
 
 static void t_compensate(double* input, double* output, bool rz)
 {
-    double z = rz ? -1 : 1;
     const auto H = input[Yaw] * M_PI / 180;
     const auto P = input[Pitch] * M_PI / 180;
     const auto B = input[Roll] * M_PI / 180;
@@ -62,8 +61,8 @@ static void t_compensate(double* input, double* output, bool rz)
     const auto sinH = sin(H);
     const auto cosP = cos(P);
     const auto sinP = sin(P);
-    const auto cosB = cos(B * z);
-    const auto sinB = sin(B * z);
+    const auto cosB = cos(B);
+    const auto sinB = sin(B);
 
     double foo[] = {
         cosH * cosB - sinH * sinP * sinB,
@@ -81,7 +80,9 @@ static void t_compensate(double* input, double* output, bool rz)
     const cv::Mat tvec(3, 1, CV_64F, input);
     cv::Mat ret = rmat * tvec;
 
-    for (int i = 0; i < 3; i++)
+    const int max = !rz ? 3 : 2;
+
+    for (int i = 0; i < max; i++)
         output[i] = ret.at<double>(i);
 }
 
-- 
cgit v1.2.3