allow for Z swapping for tcomp

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

5 files changed, 64 insertions, 27 deletions

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;
 };