implement camera offset

4 files changed, 516 insertions, 338 deletions

diff --git a/facetracknoir/curve-config.cpp b/facetracknoir/curve-config.cpp
index db5a4a75..ac34e57a 100644
--- a/facetracknoir/curve-config.cpp
+++ b/facetracknoir/curve-config.cpp
@@ -76,6 +76,9 @@ MapWidget::MapWidget(Mappings& m, main_settings& s) :
     tie_setting(s.a_x.src, ui.src_x);
     tie_setting(s.a_y.src, ui.src_y);
     tie_setting(s.a_z.src, ui.src_z);
+    
+    tie_setting(s.camera_yaw, ui.camera_yaw);
+    tie_setting(s.camera_pitch, ui.camera_pitch);
 }
 
 void MapWidget::doOK() {
diff --git a/facetracknoir/mapping.ui b/facetracknoir/mapping.ui
index 9f422b9b..574f320f 100644
--- a/facetracknoir/mapping.ui
+++ b/facetracknoir/mapping.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>970</width>
-    <height>655</height>
+    <height>664</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -626,350 +626,400 @@
          <property name="checkable">
           <bool>false</bool>
          </property>
-         <layout class="QGridLayout" name="gridLayout_4" rowstretch="0,0,0,0,0,0,0" columnstretch="0,0,0">
+         <layout class="QGridLayout" name="gridLayout_4" rowstretch="0,0,0,0,0,0,0,0" columnstretch="0,0,0">
           <property name="sizeConstraint">
            <enum>QLayout::SetMinAndMaxSize</enum>
           </property>
           <property name="spacing">
            <number>6</number>
           </property>
-          <item row="4" column="0">
-           <widget class="QLabel" name="label_10">
-            <property name="text">
-             <string>X</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label_7">
-            <property name="text">
-             <string>Yaw</string>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="0">
-           <widget class="QLabel" name="label_8">
-            <property name="text">
-             <string>Pitch</string>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="0">
-           <widget class="QLabel" name="label_11">
-            <property name="text">
-             <string>Y</string>
-            </property>
-           </widget>
-          </item>
-          <item row="6" column="0">
-           <widget class="QLabel" name="label_12">
-            <property name="text">
-             <string>Z</string>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="0">
-           <widget class="QLabel" name="label_9">
-            <property name="text">
-             <string>Roll</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="2">
-           <widget class="QCheckBox" name="invert_yaw">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="2" column="2">
-           <widget class="QCheckBox" name="invert_pitch">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="2">
-           <widget class="QCheckBox" name="invert_x">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="2">
-           <widget class="QCheckBox" name="invert_roll">
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_20">
             <property name="text">
-             <string/>
+             <string>Assign input axis to output axis.</string>
             </property>
-           </widget>
-          </item>
-          <item row="5" column="2">
-           <widget class="QCheckBox" name="invert_y">
-            <property name="text">
-             <string/>
+            <property name="alignment">
+             <set>Qt::AlignJustify|Qt::AlignVCenter</set>
             </property>
-           </widget>
-          </item>
-          <item row="6" column="2">
-           <widget class="QCheckBox" name="invert_z">
-            <property name="text">
-             <string/>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="1">
-           <widget class="QComboBox" name="src_yaw">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="2" column="1">
-           <widget class="QComboBox" name="src_pitch">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="3" column="1">
-           <widget class="QComboBox" name="src_roll">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="4" column="1">
-           <widget class="QComboBox" name="src_x">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="5" column="1">
-           <widget class="QComboBox" name="src_y">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="6" column="1">
-           <widget class="QComboBox" name="src_z">
-            <item>
-             <property name="text">
-              <string>X</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Y</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Z</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Yaw</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Pitch</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Roll</string>
-             </property>
-            </item>
-            <item>
-             <property name="text">
-              <string>Disabled</string>
-             </property>
-            </item>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="QLabel" name="label_13">
-            <property name="text">
-             <string>Source</string>
+            <property name="wordWrap">
+             <bool>true</bool>
             </property>
            </widget>
           </item>
-          <item row="0" column="2">
-           <widget class="QLabel" name="label_14">
-            <property name="text">
-             <string>Invert</string>
+          <item row="1" column="0">
+           <widget class="QFrame" name="frame_2">
+            <property name="frameShape">
+             <enum>QFrame::NoFrame</enum>
             </property>
-           </widget>
-          </item>
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_15">
-            <property name="text">
-             <string>Destination</string>
+            <property name="frameShadow">
+             <enum>QFrame::Raised</enum>
             </property>
+            <layout class="QGridLayout" name="gridLayout_5">
+             <item row="7" column="2">
+              <widget class="QCheckBox" name="invert_y">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+             <item row="5" column="0">
+              <widget class="QLabel" name="label_9">
+               <property name="text">
+                <string>Roll</string>
+               </property>
+              </widget>
+             </item>
+             <item row="6" column="0">
+              <widget class="QLabel" name="label_10">
+               <property name="text">
+                <string>X</string>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="2">
+              <widget class="QLabel" name="label_14">
+               <property name="text">
+                <string>Invert</string>
+               </property>
+              </widget>
+             </item>
+             <item row="6" column="1">
+              <widget class="QComboBox" name="src_x">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="4" column="0">
+              <widget class="QLabel" name="label_8">
+               <property name="text">
+                <string>Pitch</string>
+               </property>
+              </widget>
+             </item>
+             <item row="7" column="1">
+              <widget class="QComboBox" name="src_y">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="3" column="1">
+              <widget class="QComboBox" name="src_yaw">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="5" column="2">
+              <widget class="QCheckBox" name="invert_roll">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+             <item row="7" column="0">
+              <widget class="QLabel" name="label_11">
+               <property name="text">
+                <string>Y</string>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="0">
+              <widget class="QLabel" name="label_15">
+               <property name="text">
+                <string>Destination</string>
+               </property>
+              </widget>
+             </item>
+             <item row="4" column="2">
+              <widget class="QCheckBox" name="invert_pitch">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+             <item row="3" column="0">
+              <widget class="QLabel" name="label_7">
+               <property name="text">
+                <string>Yaw</string>
+               </property>
+              </widget>
+             </item>
+             <item row="6" column="2">
+              <widget class="QCheckBox" name="invert_x">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="1">
+              <widget class="QLabel" name="label_13">
+               <property name="text">
+                <string>Source</string>
+               </property>
+              </widget>
+             </item>
+             <item row="4" column="1">
+              <widget class="QComboBox" name="src_pitch">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="3" column="2">
+              <widget class="QCheckBox" name="invert_yaw">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+             <item row="5" column="1">
+              <widget class="QComboBox" name="src_roll">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="8" column="0">
+              <widget class="QLabel" name="label_12">
+               <property name="text">
+                <string>Z</string>
+               </property>
+              </widget>
+             </item>
+             <item row="8" column="1">
+              <widget class="QComboBox" name="src_z">
+               <item>
+                <property name="text">
+                 <string>X</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Y</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Z</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Yaw</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Pitch</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Roll</string>
+                </property>
+               </item>
+               <item>
+                <property name="text">
+                 <string>Disabled</string>
+                </property>
+               </item>
+              </widget>
+             </item>
+             <item row="8" column="2">
+              <widget class="QCheckBox" name="invert_z">
+               <property name="text">
+                <string/>
+               </property>
+              </widget>
+             </item>
+            </layout>
+            <zorder>label_15</zorder>
+            <zorder>label_14</zorder>
+            <zorder>label_13</zorder>
+            <zorder>label_15</zorder>
+            <zorder>label_15</zorder>
+            <zorder>label_13</zorder>
+            <zorder>label_14</zorder>
+            <zorder>src_yaw</zorder>
+            <zorder>invert_yaw</zorder>
+            <zorder>label_7</zorder>
+            <zorder>src_pitch</zorder>
+            <zorder>label_8</zorder>
+            <zorder>invert_pitch</zorder>
+            <zorder>label_9</zorder>
+            <zorder>src_roll</zorder>
+            <zorder>invert_roll</zorder>
+            <zorder>label_10</zorder>
+            <zorder>src_x</zorder>
+            <zorder>invert_x</zorder>
+            <zorder>label_11</zorder>
+            <zorder>src_y</zorder>
+            <zorder>invert_y</zorder>
+            <zorder>label_12</zorder>
+            <zorder>src_z</zorder>
+            <zorder>invert_z</zorder>
            </widget>
           </item>
          </layout>
@@ -993,7 +1043,7 @@
           <bool>true</bool>
          </property>
          <layout class="QFormLayout" name="formLayout_2">
-          <item row="0" column="0">
+          <item row="1" column="0">
            <widget class="QCheckBox" name="tcomp_enable">
             <property name="sizePolicy">
              <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -1009,7 +1059,7 @@
             </property>
            </widget>
           </item>
-          <item row="1" column="0">
+          <item row="2" column="0">
            <widget class="QCheckBox" name="tcomp_rz">
             <property name="styleSheet">
              <string notr="true"/>
@@ -1019,6 +1069,117 @@
             </property>
            </widget>
           </item>
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_16">
+            <property name="text">
+             <string>With compensation on, translation is applied after rotation. For example, rotating +180 degrees yaw and moving backwards results in moving forward as a result of that rotation.</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignJustify|Qt::AlignVCenter</set>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item row="1" column="1">
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
+         <property name="title">
+          <string>Camera offset</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_2">
+          <item row="3" column="0">
+           <widget class="QFrame" name="frame">
+            <property name="frameShape">
+             <enum>QFrame::NoFrame</enum>
+            </property>
+            <property name="frameShadow">
+             <enum>QFrame::Raised</enum>
+            </property>
+            <layout class="QGridLayout" name="gridLayout_3">
+             <item row="0" column="0">
+              <widget class="QLabel" name="label_18">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Maximum" vsizetype="Preferred">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Yaw</string>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="1">
+              <widget class="QSpinBox" name="camera_yaw">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="minimum">
+                <number>-90</number>
+               </property>
+               <property name="maximum">
+                <number>90</number>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="0">
+              <widget class="QLabel" name="label_19">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Maximum" vsizetype="Preferred">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Pitch</string>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="1">
+              <widget class="QSpinBox" name="camera_pitch">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                 <horstretch>0</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="minimum">
+                <number>-90</number>
+               </property>
+               <property name="maximum">
+                <number>90</number>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </widget>
+          </item>
+          <item row="2" column="0">
+           <widget class="QLabel" name="label_17">
+            <property name="text">
+             <string>Specify an angle for off-center camera as a basis for which direction is which, avoiding axis interconnect.</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignJustify|Qt::AlignVCenter</set>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>
diff --git a/opentrack/main-settings.hpp b/opentrack/main-settings.hpp
index 2c909d27..def21f31 100644
--- a/opentrack/main-settings.hpp
+++ b/opentrack/main-settings.hpp
@@ -28,6 +28,7 @@ struct main_settings {
     axis_opts a_x, a_y, a_z, a_yaw, a_pitch, a_roll;
     value<bool> tcomp_p, tcomp_tz;
     value<bool> tray_enabled;
+    value<int> camera_yaw, camera_pitch;
     main_settings(pbundle b) :
         b(b),
         tracker_dll(b, "tracker-dll", ""),
@@ -42,6 +43,8 @@ struct main_settings {
         a_roll(b, "roll", Roll),
         tcomp_p(b, "compensate-translation", true),
         tcomp_tz(b, "compensate-translation-disable-z-axis", false),
-        tray_enabled(b, "use-system-tray", false)
+        tray_enabled(b, "use-system-tray", false),
+        camera_yaw(b, "camera-yaw", 0),
+        camera_pitch(b, "camera-pitch", 0)
     {}
 };
diff --git a/opentrack/tracker.cpp b/opentrack/tracker.cpp
index f621083b..60ed4c33 100644
--- a/opentrack/tracker.cpp
+++ b/opentrack/tracker.cpp
@@ -94,22 +94,33 @@ void Tracker::logic()
             raw(i) = value(i) = t_b[i];
         }
     }
+
+    const double off[] = {
+        s.camera_yaw,
+        s.camera_pitch,
+        0.
+    };
+    const rmat cam = rmat::euler_to_rmat(off);
+    rmat r = rmat::euler_to_rmat(&value[Yaw]);
+    dmat<3, 1> t { value(0), value(1), value(3) };
+    
+    r = cam * r;
+    t = cam * t;
     
     if (centerp)
     {
         centerp = false;
         for (int i = 0; i < 3; i++)
-            t_b[i] = value(i);
-        r_b = rmat::euler_to_rmat(&value[Yaw]);
+            t_b[i] = t(i, 0);
+        r_b = r;
     }
     
     {
-        const rmat r = rmat::euler_to_rmat(&value[Yaw]);
         const rmat m_ = r * r_b.t();
         const dmat<3, 1> euler = rmat::rmat_to_euler(m_);
         for (int i = 0; i < 3; i++)
         {
-            value(i) -= t_b[i];
+            value(i) = t(i, 0) - t_b[i];
             value(i+3) = euler(i, 0) * r2d;
         }
     }