From 3fc21e23083bce507fc9832073297c9a861c3505 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 14 Dec 2013 17:45:05 +0100
Subject: allow for Z swapping for tcomp

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
---
 facetracknoir/curve-config.cpp  | 11 +++++--
 facetracknoir/facetracknoir.cpp |  1 +
 facetracknoir/ftnoir_curves.ui  | 66 +++++++++++++++++++++++++++++------------
 facetracknoir/tracker.cpp       | 12 ++++----
 facetracknoir/tracker.h         |  1 +
 5 files changed, 64 insertions(+), 27 deletions(-)

(limited to 'facetracknoir')

diff --git a/facetracknoir/curve-config.cpp b/facetracknoir/curve-config.cpp
index ca9e0d19..2e731892 100644
--- a/facetracknoir/curve-config.cpp
+++ b/facetracknoir/curve-config.cpp
@@ -96,6 +96,7 @@ void CurveConfigurationDialog::loadSettings() {
 
     iniFile.beginGroup("Tracking");
 
+    ui.tcomp_rz->setChecked(iniFile.value("tcomp-rz", false).toBool());
     ui.checkBox->setChecked(iniFile.value("compensate", true).toBool());
 
     for (int i = 0; i < 6; i++)
@@ -184,7 +185,7 @@ void CurveConfigurationDialog::loadSettings() {
         connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(curveChanged(int)), Qt::UniqueConnection);
         mainApp->axis(i).zero = widgets3[i]->value();
     }
-    
+
     settingsDirty = false;
 }
 
@@ -213,16 +214,20 @@ void CurveConfigurationDialog::save() {
     ui.ryconfig_alt->saveSettings(currentFile);
     ui.rzconfig_alt->saveSettings(currentFile);
 
+    bool tcomp_rz = false, compensate = true;
+
     QSettings iniFile( currentFile, QSettings::IniFormat );		// Application settings (in INI-file)
 
     iniFile.beginGroup("Tracking");
 
-    bool compensate = true;
-
+    iniFile.setValue("tcomp-rz", tcomp_rz = ui.tcomp_rz->checkState() != Qt::Unchecked);
     iniFile.setValue("compensate", compensate = (bool) !!ui.checkBox->isChecked());
 
     if (mainApp->tracker)
+    {
         mainApp->tracker->compensate = compensate;
+        mainApp->tracker->tcomp_rz = tcomp_rz;
+    }
 
     iniFile.setValue("rx_alt", ui.rx_altp->checkState() != Qt::Unchecked);
     iniFile.setValue("ry_alt", ui.ry_altp->checkState() != Qt::Unchecked);
diff --git a/facetracknoir/facetracknoir.cpp b/facetracknoir/facetracknoir.cpp
index 1f3ae3a2..75ac737d 100644
--- a/facetracknoir/facetracknoir.cpp
+++ b/facetracknoir/facetracknoir.cpp
@@ -495,6 +495,7 @@ void FaceTrackNoIR::startTracker( ) {
     tracker = new Tracker ( this );
 
     tracker->compensate = iniFile.value("compensate", true).toBool();
+    tracker->tcomp_rz = iniFile.value("tcomp-rz", false).toBool();
 
     iniFile.endGroup();
 
diff --git a/facetracknoir/ftnoir_curves.ui b/facetracknoir/ftnoir_curves.ui
index 69440a5b..87c449ba 100644
--- a/facetracknoir/ftnoir_curves.ui
+++ b/facetracknoir/ftnoir_curves.ui
@@ -579,25 +579,6 @@
       <attribute name="title">
        <string>Options</string>
       </attribute>
-      <widget class="QCheckBox" name="checkBox">
-       <property name="geometry">
-        <rect>
-         <x>310</x>
-         <y>10</y>
-         <width>192</width>
-         <height>21</height>
-        </rect>
-       </property>
-       <property name="sizePolicy">
-        <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-         <horstretch>0</horstretch>
-         <verstretch>0</verstretch>
-        </sizepolicy>
-       </property>
-       <property name="text">
-        <string>Translation compensation</string>
-       </property>
-      </widget>
       <widget class="QGroupBox" name="groupBox">
        <property name="geometry">
         <rect>
@@ -760,6 +741,53 @@
         </item>
        </layout>
       </widget>
+      <widget class="QGroupBox" name="groupBox_2">
+       <property name="geometry">
+        <rect>
+         <x>10</x>
+         <y>140</y>
+         <width>211</width>
+         <height>81</height>
+        </rect>
+       </property>
+       <property name="styleSheet">
+        <string notr="true"/>
+       </property>
+       <property name="title">
+        <string>Translation compensation</string>
+       </property>
+       <property name="flat">
+        <bool>true</bool>
+       </property>
+       <layout class="QVBoxLayout" name="verticalLayout_2">
+        <item>
+         <widget class="QCheckBox" name="checkBox">
+          <property name="sizePolicy">
+           <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+            <horstretch>0</horstretch>
+            <verstretch>0</verstretch>
+           </sizepolicy>
+          </property>
+          <property name="styleSheet">
+           <string notr="true"/>
+          </property>
+          <property name="text">
+           <string>Enablement</string>
+          </property>
+         </widget>
+        </item>
+        <item>
+         <widget class="QCheckBox" name="tcomp_rz">
+          <property name="styleSheet">
+           <string notr="true"/>
+          </property>
+          <property name="text">
+           <string>Reverse Z axis</string>
+          </property>
+         </widget>
+        </item>
+       </layout>
+      </widget>
      </widget>
     </widget>
    </item>
diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 8f8ad356..0c5501d4 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -26,7 +26,8 @@ Tracker::Tracker( FaceTrackNoIR *parent ) :
     should_quit(false),
     do_center(false),
     enabled(true),
-    compensate(true)
+    compensate(true),
+    tcomp_rz(false)
 {
     mainApp = parent;
 }
@@ -50,8 +51,9 @@ static void get_curve(double pos, double& out, THeadPoseDOF& axis) {
     out += axis.zero;
 }
 
-static void t_compensate(double* input, double* output)
+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;
@@ -60,8 +62,8 @@ static void t_compensate(double* input, double* output)
     const auto sinH = sin(H);
     const auto cosP = cos(P);
     const auto sinP = sin(P);
-    const auto cosB = cos(B);
-    const auto sinB = sin(B);
+    const auto cosB = cos(B * z);
+    const auto sinB = sin(B * z);
 
     double foo[] = {
         cosH * cosB - sinH * sinP * sinB,
@@ -152,7 +154,7 @@ void Tracker::run() {
             }
 
             if (compensate)
-                t_compensate(output_camera.axes, output_camera.axes);
+                t_compensate(output_camera.axes, output_camera.axes, tcomp_rz);
 
             if (Libraries->pProtocol) {
                 Libraries->pProtocol->sendHeadposeToGame( output_camera.axes );	// degrees & centimeters
diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h
index 1094c61b..02824206 100644
--- a/facetracknoir/tracker.h
+++ b/facetracknoir/tracker.h
@@ -84,6 +84,7 @@ public:
     volatile bool do_center;
     volatile bool enabled;
     volatile bool compensate;
+    volatile bool tcomp_rz;
     
     T6DOF output_camera;
 };
-- 
cgit v1.2.3