switch input axis for freepie-udp

Issue: #90
Requested-by: @KyokushinPL

4 files changed, 147 insertions, 34 deletions

diff --git a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
index 48290bf2..c0b1de8a 100644
--- a/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
+++ b/ftnoir_tracker_freepie-udp/freepie-udp-controls.ui
@@ -9,48 +9,141 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>198</width>
-    <height>71</height>
+    <width>302</width>
+    <height>231</height>
    </rect>
   </property>
   <property name="windowTitle">
-   <string>UDP tracker settings</string>
+   <string>Tracker settings</string>
   </property>
   <property name="windowIcon">
    <iconset>
     <normaloff>../facetracknoir/images/facetracknoir.png</normaloff>../facetracknoir/images/facetracknoir.png</iconset>
   </property>
-  <property name="layoutDirection">
-   <enum>Qt::LeftToRight</enum>
-  </property>
-  <property name="autoFillBackground">
-   <bool>false</bool>
-  </property>
-  <layout class="QGridLayout" name="gridLayout">
-   <item row="0" column="0">
-    <widget class="QLabel" name="label_5">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Maximum" vsizetype="Preferred">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QFrame" name="frame">
+     <property name="frameShape">
+      <enum>QFrame::NoFrame</enum>
      </property>
-     <property name="text">
-      <string>UDP port</string>
+     <property name="frameShadow">
+      <enum>QFrame::Raised</enum>
      </property>
+     <layout class="QHBoxLayout" name="horizontalLayout">
+      <item>
+       <widget class="QLabel" name="label_5">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+          <horstretch>0</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="text">
+         <string>UDP port</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QSpinBox" name="port">
+        <property name="minimum">
+         <number>0</number>
+        </property>
+        <property name="maximum">
+         <number>65535</number>
+        </property>
+       </widget>
+      </item>
+     </layout>
     </widget>
    </item>
-   <item row="0" column="1">
-    <widget class="QSpinBox" name="port">
-     <property name="minimum">
-      <number>0</number>
-     </property>
-     <property name="maximum">
-      <number>65535</number>
+   <item>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Axis order</string>
      </property>
+     <layout class="QGridLayout" name="gridLayout">
+      <item row="0" column="0" rowspan="2">
+       <widget class="QLabel" name="label">
+        <property name="text">
+         <string>output yaw</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QComboBox" name="input_x">
+        <item>
+         <property name="text">
+          <string>input yaw</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input pitch</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input roll</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+      <item row="1" column="1" rowspan="2">
+       <widget class="QComboBox" name="input_y">
+        <item>
+         <property name="text">
+          <string>input yaw</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input pitch</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input roll</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+      <item row="2" column="0" rowspan="2">
+       <widget class="QLabel" name="label_2">
+        <property name="text">
+         <string>output pitch</string>
+        </property>
+       </widget>
+      </item>
+      <item row="3" column="1" rowspan="2">
+       <widget class="QComboBox" name="input_z">
+        <item>
+         <property name="text">
+          <string>input yaw</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input pitch</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>input roll</string>
+         </property>
+        </item>
+       </widget>
+      </item>
+      <item row="4" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>output roll</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
     </widget>
    </item>
-   <item row="1" column="0" colspan="2">
+   <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="standardButtons">
       <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
index eff1103b..6807419b 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.cpp
@@ -14,6 +14,16 @@ TrackerImpl::~TrackerImpl()
     wait();
 }
 
+template<typename t>
+static const t bound(t datum, t least, t max)
+{
+    if (datum < least)
+        return least;
+    if (datum > max)
+        return max;
+    return datum;
+}
+
 void TrackerImpl::run() {
 #pragma pack(push, 1)
     struct {
@@ -29,10 +39,12 @@ void TrackerImpl::run() {
         Mask = flag_Raw | flag_Orient
     };
 
-    while (1) {
-        if (should_quit)
-            break;
-
+    while (!should_quit) {
+        int order[] = {
+            bound<int>(s.idx_x, 0, 2),
+            bound<int>(s.idx_y, 0, 2),
+            bound<int>(s.idx_z, 0, 2)
+        };
         float orient[3];
         bool filled = false;
 
@@ -67,7 +79,9 @@ void TrackerImpl::run() {
             QMutexLocker foo(&mtx);
             static constexpr double d2r = 57.295781;
             for (int i = 0; i < 3; i++)
-                pose[Yaw + i] = d2r * orient[i];
+            {
+                pose[Yaw + i] = d2r * orient[order[i]];
+            }
         }
         usleep(4000);
     }
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
index 0cec952f..2dfb4e8f 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp.h
@@ -15,10 +15,13 @@ using namespace options;
 
 struct settings {
     pbundle b;
-    value<int> port;
+    value<int> port, idx_x, idx_y, idx_z;
     settings() :
         b(bundle("freepie-udp-tracker")),
-        port(b, "port", 4237)
+        port(b, "port", 4237),
+        idx_x(b, "axis-index-x", 0),
+        idx_y(b, "axis-index-y", 1),
+        idx_z(b, "axis-index-z", 2)
     {}
 };
 
diff --git a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
index ec95da90..4840a56b 100644
--- a/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
+++ b/ftnoir_tracker_freepie-udp/ftnoir_tracker_freepie-udp_dialog.cpp
@@ -9,6 +9,9 @@ TrackerDialog::TrackerDialog()
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
 
     tie_setting(s.port, ui.port);
+    tie_setting(s.idx_x, ui.input_x);
+    tie_setting(s.idx_y, ui.input_y);
+    tie_setting(s.idx_z, ui.input_z);
 }
 
 void TrackerDialog::doOK() {