gui, logic: add neck displacement feature

With it enabled, user's neck will be treated as separate from the rotation
pivot. Rotating to the left will reposition to the left in addition to the
rotation, and so on.

This feature is subtle but apparently very popular with DCS users.

4 files changed, 447 insertions, 274 deletions

diff --git a/gui/options-dialog.cpp b/gui/options-dialog.cpp
index bafddb57..8190b0f7 100644
--- a/gui/options-dialog.cpp
+++ b/gui/options-dialog.cpp
@@ -59,6 +59,9 @@ OptionsDialog::OptionsDialog(std::function<void(bool)> pause_keybindings) :
     tie_setting(main.tcomp_disable_src_pitch, ui.tcomp_src_pitch_disable);
     tie_setting(main.tcomp_disable_src_roll, ui.tcomp_src_roll_disable);
 
+    tie_setting(main.neck_y, ui.neck_y);
+    tie_setting(main.neck_z, ui.neck_z);
+
     tie_setting(main.a_x.zero, ui.pos_tx);
     tie_setting(main.a_y.zero, ui.pos_ty);
     tie_setting(main.a_z.zero, ui.pos_tz);
diff --git a/gui/options-dialog.ui b/gui/options-dialog.ui
index 2168fa8d..f49d6cde 100644
--- a/gui/options-dialog.ui
+++ b/gui/options-dialog.ui
@@ -6,10 +6,16 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>450</width>
+    <width>455</width>
     <height>599</height>
    </rect>
   </property>
+  <property name="sizePolicy">
+   <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+    <horstretch>0</horstretch>
+    <verstretch>0</verstretch>
+   </sizepolicy>
+  </property>
   <property name="minimumSize">
    <size>
     <width>450</width>
@@ -337,6 +343,12 @@
        </item>
        <item>
         <widget class="QCheckBox" name="center_at_startup">
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
          <property name="text">
           <string>Center at startup</string>
          </property>
@@ -874,175 +886,8 @@
        <string>Output</string>
       </attribute>
       <layout class="QVBoxLayout" name="verticalLayout_4">
-       <item>
-        <widget class="QGroupBox" name="groupBox_">
-         <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="title">
-          <string>Relative translation</string>
-         </property>
-         <property name="flat">
-          <bool>false</bool>
-         </property>
-         <layout class="QVBoxLayout" name="verticalLayout_7">
-          <item>
-           <widget class="QLabel" name="label_16">
-            <property name="text">
-             <string>With relative mode 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>
-            <property name="margin">
-             <number>2</number>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QCheckBox" name="tcomp_enable">
-            <property name="styleSheet">
-             <string notr="true"/>
-            </property>
-            <property name="text">
-             <string>Enable</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QFrame" name="frame_2">
-            <property name="frameShape">
-             <enum>QFrame::NoFrame</enum>
-            </property>
-            <property name="frameShadow">
-             <enum>QFrame::Raised</enum>
-            </property>
-            <layout class="QGridLayout" name="gridLayout">
-             <property name="leftMargin">
-              <number>0</number>
-             </property>
-             <property name="topMargin">
-              <number>0</number>
-             </property>
-             <property name="rightMargin">
-              <number>0</number>
-             </property>
-             <property name="bottomMargin">
-              <number>0</number>
-             </property>
-             <property name="spacing">
-              <number>0</number>
-             </property>
-             <item row="2" column="0">
-              <widget class="QCheckBox" name="tcomp_tx_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>3</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="styleSheet">
-                <string notr="true"/>
-               </property>
-               <property name="text">
-                <string>Disable for X</string>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="0">
-              <widget class="QCheckBox" name="tcomp_ty_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>3</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="styleSheet">
-                <string notr="true"/>
-               </property>
-               <property name="text">
-                <string>Disable for Y</string>
-               </property>
-              </widget>
-             </item>
-             <item row="4" column="0">
-              <widget class="QCheckBox" name="tcomp_tz_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>3</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="styleSheet">
-                <string notr="true"/>
-               </property>
-               <property name="text">
-                <string>Disable for Z (for zoom on Z axis)</string>
-               </property>
-              </widget>
-             </item>
-             <item row="2" column="1">
-              <widget class="QCheckBox" name="tcomp_src_yaw_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>2</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>Disable effect by yaw</string>
-               </property>
-              </widget>
-             </item>
-             <item row="3" column="1">
-              <widget class="QCheckBox" name="tcomp_src_pitch_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>2</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>Disable effect by pitch</string>
-               </property>
-              </widget>
-             </item>
-             <item row="4" column="1">
-              <widget class="QCheckBox" name="tcomp_src_roll_disable">
-               <property name="sizePolicy">
-                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-                 <horstretch>2</horstretch>
-                 <verstretch>0</verstretch>
-                </sizepolicy>
-               </property>
-               <property name="text">
-                <string>Disable effect by roll</string>
-               </property>
-              </widget>
-             </item>
-            </layout>
-           </widget>
-          </item>
-         </layout>
-        </widget>
-       </item>
        <item alignment="Qt::AlignTop">
         <widget class="QGroupBox" name="groupBox_4">
-         <property name="maximumSize">
-          <size>
-           <width>65536</width>
-           <height>65536</height>
-          </size>
-         </property>
          <property name="font">
           <font>
            <kerning>true</kerning>
@@ -1067,19 +912,6 @@
           <property name="spacing">
            <number>6</number>
           </property>
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_20">
-            <property name="text">
-             <string>Assign input axis to output axis.</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignJustify|Qt::AlignVCenter</set>
-            </property>
-            <property name="wordWrap">
-             <bool>true</bool>
-            </property>
-           </widget>
-          </item>
           <item row="1" column="0">
            <widget class="QGroupBox" name="groupBox_2">
             <property name="styleSheet">
@@ -1452,6 +1284,19 @@
             <zorder>invert_z</zorder>
            </widget>
           </item>
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_20">
+            <property name="text">
+             <string>Assign input axis to output axis.</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignJustify|Qt::AlignVCenter</set>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>
@@ -1510,6 +1355,305 @@
        </item>
       </layout>
      </widget>
+     <widget class="QWidget" name="tab_2">
+      <attribute name="title">
+       <string>Relative translation</string>
+      </attribute>
+      <layout class="QVBoxLayout" name="verticalLayout_14">
+       <item>
+        <widget class="QGroupBox" name="groupBox_">
+         <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="title">
+          <string>Relative translation</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_7">
+          <item>
+           <widget class="QLabel" name="label_16">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="text">
+             <string>With relative mode 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>
+            <property name="margin">
+             <number>2</number>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QCheckBox" name="tcomp_enable">
+            <property name="styleSheet">
+             <string notr="true"/>
+            </property>
+            <property name="text">
+             <string>Enable</string>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QFrame" name="frame_2">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+            <property name="frameShape">
+             <enum>QFrame::NoFrame</enum>
+            </property>
+            <property name="frameShadow">
+             <enum>QFrame::Raised</enum>
+            </property>
+            <layout class="QGridLayout" name="gridLayout">
+             <property name="leftMargin">
+              <number>0</number>
+             </property>
+             <property name="topMargin">
+              <number>0</number>
+             </property>
+             <property name="rightMargin">
+              <number>0</number>
+             </property>
+             <property name="bottomMargin">
+              <number>0</number>
+             </property>
+             <property name="spacing">
+              <number>0</number>
+             </property>
+             <item row="2" column="0">
+              <widget class="QCheckBox" name="tcomp_tx_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>3</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="styleSheet">
+                <string notr="true"/>
+               </property>
+               <property name="text">
+                <string>Disable for X</string>
+               </property>
+              </widget>
+             </item>
+             <item row="3" column="0">
+              <widget class="QCheckBox" name="tcomp_ty_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>3</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="styleSheet">
+                <string notr="true"/>
+               </property>
+               <property name="text">
+                <string>Disable for Y</string>
+               </property>
+              </widget>
+             </item>
+             <item row="4" column="0">
+              <widget class="QCheckBox" name="tcomp_tz_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>3</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="styleSheet">
+                <string notr="true"/>
+               </property>
+               <property name="text">
+                <string>Disable for Z (for zoom on Z axis)</string>
+               </property>
+              </widget>
+             </item>
+             <item row="2" column="1">
+              <widget class="QCheckBox" name="tcomp_src_yaw_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>2</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Disable effect by yaw</string>
+               </property>
+              </widget>
+             </item>
+             <item row="3" column="1">
+              <widget class="QCheckBox" name="tcomp_src_pitch_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>2</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Disable effect by pitch</string>
+               </property>
+              </widget>
+             </item>
+             <item row="4" column="1">
+              <widget class="QCheckBox" name="tcomp_src_roll_disable">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+                 <horstretch>2</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Disable effect by roll</string>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_12">
+         <property name="title">
+          <string>Neck displacement</string>
+         </property>
+         <layout class="QVBoxLayout" name="verticalLayout_13">
+          <item>
+           <widget class="QLabel" name="label_33">
+            <property name="text">
+             <string>Eyes will be offset from the pivot of rotation, assumed to be the neck. Set to zeros in order to disable.
+
+It also works with relative translation disabled.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item>
+           <widget class="QFrame" name="frame_4">
+            <property name="frameShape">
+             <enum>QFrame::NoFrame</enum>
+            </property>
+            <layout class="QGridLayout" name="gridLayout_9">
+             <property name="leftMargin">
+              <number>0</number>
+             </property>
+             <property name="topMargin">
+              <number>0</number>
+             </property>
+             <property name="rightMargin">
+              <number>0</number>
+             </property>
+             <property name="bottomMargin">
+              <number>0</number>
+             </property>
+             <property name="spacing">
+              <number>0</number>
+             </property>
+             <item row="0" column="0">
+              <widget class="QLabel" name="label_31">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+                 <horstretch>15</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Height from center of rotation</string>
+               </property>
+              </widget>
+             </item>
+             <item row="0" column="1">
+              <widget class="QSpinBox" name="neck_y">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+                 <horstretch>4</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
+               <property name="suffix">
+                <string> cm</string>
+               </property>
+               <property name="maximum">
+                <number>50</number>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="0">
+              <widget class="QLabel" name="label_32">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+                 <horstretch>15</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="text">
+                <string>Forward from center of rotation</string>
+               </property>
+              </widget>
+             </item>
+             <item row="1" column="1">
+              <widget class="QSpinBox" name="neck_z">
+               <property name="sizePolicy">
+                <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+                 <horstretch>4</horstretch>
+                 <verstretch>0</verstretch>
+                </sizepolicy>
+               </property>
+               <property name="alignment">
+                <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+               </property>
+               <property name="suffix">
+                <string> cm</string>
+               </property>
+               <property name="maximum">
+                <number>50</number>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
+       <item>
+        <spacer name="verticalSpacer_4">
+         <property name="orientation">
+          <enum>Qt::Vertical</enum>
+         </property>
+         <property name="sizeHint" stdset="0">
+          <size>
+           <width>20</width>
+           <height>0</height>
+          </size>
+         </property>
+        </spacer>
+       </item>
+      </layout>
+     </widget>
      <widget class="QWidget" name="tab_5">
       <attribute name="title">
        <string>Game detection</string>
@@ -1544,7 +1688,14 @@
            </widget>
           </item>
           <item>
-           <widget class="process_detector" name="game_detector" native="true"/>
+           <widget class="process_detector" name="game_detector" native="true">
+            <property name="sizePolicy">
+             <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+              <horstretch>0</horstretch>
+              <verstretch>0</verstretch>
+             </sizepolicy>
+            </property>
+           </widget>
           </item>
          </layout>
         </widget>
@@ -1607,13 +1758,6 @@
   <tabstop>src_z</tabstop>
   <tabstop>invert_z</tabstop>
   <tabstop>tracklogging_enabled</tabstop>
-  <tabstop>tcomp_enable</tabstop>
-  <tabstop>tcomp_tx_disable</tabstop>
-  <tabstop>tcomp_ty_disable</tabstop>
-  <tabstop>tcomp_tz_disable</tabstop>
-  <tabstop>tcomp_src_yaw_disable</tabstop>
-  <tabstop>tcomp_src_pitch_disable</tabstop>
-  <tabstop>tcomp_src_roll_disable</tabstop>
  </tabstops>
  <resources>
   <include location="opentrack-res.qrc"/>
diff --git a/logic/main-settings.hpp b/logic/main-settings.hpp
index 53c5fae1..ea1d93f3 100644
--- a/logic/main-settings.hpp
+++ b/logic/main-settings.hpp
@@ -74,6 +74,7 @@ struct main_settings
     value<bool> use_camera_offset_from_centering;
     value<bool> center_at_startup;
     value<int> center_method;
+    value<int> neck_y, neck_z;
     key_opts key_start_tracking, key_stop_tracking, key_toggle_tracking, key_restart_tracking;
     key_opts key_center, key_toggle, key_zero;
     key_opts key_toggle_press, key_zero_press;
@@ -104,6 +105,8 @@ struct main_settings
         use_camera_offset_from_centering(b, "use-camera-offset-from-centering", false),
         center_at_startup(b, "center-at-startup", true),
         center_method(b, "centering-method", true),
+        neck_y(b, "neck-height", 0),
+        neck_z(b, "neck-depth", 0),
         key_start_tracking(b, "start-tracking"),
         key_stop_tracking(b, "stop-tracking"),
         key_toggle_tracking(b, "toggle-tracking"),
diff --git a/logic/tracker.cpp b/logic/tracker.cpp
index a5892dd0..25e067a7 100644
--- a/logic/tracker.cpp
+++ b/logic/tracker.cpp
@@ -156,8 +156,7 @@ void Tracker::logic()
 
     logger.write_pose(raw); // raw
 
-    if (is_nan(raw))
-        raw = last_raw;
+    bool nanp = is_nan(raw) | is_nan(value);
 
     // TODO split this function, it's too big
 
@@ -171,58 +170,57 @@ void Tracker::logic()
     scaled_rotation.camera = get_camera_offset_matrix(c_div);
     real_rotation.camera = get_camera_offset_matrix(1);
 
-    bool nanp = is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation);
+    nanp |= is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation);
 
-    if (!nanp)
+    if (!tracking_started)
     {
-        if (!tracking_started)
-        {
-            using std::fabs;
+        using std::fabs;
 
-            for (int i = 0; i < 6; i++)
-                if (fabs(newpose(i)) != 0)
-                {
-                    tracking_started = true;
-                    break;
-                }
-        }
+        for (int i = 0; i < 6; i++)
+            if (fabs(newpose(i)) != 0)
+            {
+                tracking_started = true;
+                break;
+            }
 
-        if (get(f_center) && tracking_started)
-        {
-            set(f_center, false);
+        tracking_started &= !nanp;
+    }
 
-            if (libs.pFilter)
-                libs.pFilter->center();
+    if (get(f_center) && tracking_started)
+    {
+        set(f_center, false);
 
-            if (libs.pTracker->center())
-            {
-                scaled_rotation.rotation = scaled_rotation.camera.t();
-                real_rotation.rotation = real_rotation.camera.t();
-
-                scaled_rotation.rotation = rmat::eye();
-                real_rotation.rotation = rmat::eye();
-                scaled_rotation.center_roll = rmat::eye();
-                scaled_rotation.center_yaw = rmat::eye();
-                scaled_rotation.center_pitch = rmat::eye();
-            }
-            else
-            {
-                euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation),
-                                              scaled_rotation.center_roll,
-                                              scaled_rotation.center_pitch,
-                                              scaled_rotation.center_yaw);
+        if (libs.pFilter)
+            libs.pFilter->center();
+
+        if (libs.pTracker->center())
+        {
+            scaled_rotation.rotation = scaled_rotation.camera.t();
+            real_rotation.rotation = real_rotation.camera.t();
+
+            scaled_rotation.rotation = rmat::eye();
+            real_rotation.rotation = rmat::eye();
+            scaled_rotation.center_roll = rmat::eye();
+            scaled_rotation.center_yaw = rmat::eye();
+            scaled_rotation.center_pitch = rmat::eye();
+        }
+        else
+        {
+            euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation),
+                                          scaled_rotation.center_roll,
+                                          scaled_rotation.center_pitch,
+                                          scaled_rotation.center_yaw);
 #if 0
-                euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation),
-                                              real_rotation.center_roll,
-                                              real_rotation.center_pitch,
-                                              real_rotation.center_yaw);
+            euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation),
+                                          real_rotation.center_roll,
+                                          real_rotation.center_pitch,
+                                          real_rotation.center_yaw);
 #endif
-                real_rotation.rot_center = real_rotation.rotation.t();
-                scaled_rotation.rot_center = scaled_rotation.rotation.t();
-            }
-
-            t_center = euler_t(&value(TX));
+            real_rotation.rot_center = real_rotation.rotation.t();
+            scaled_rotation.rot_center = scaled_rotation.rotation.t();
         }
+
+        t_center = euler_t(&value(TX));
     }
 
     {
@@ -268,20 +266,37 @@ void Tracker::logic()
 
     logger.write_pose(value); // "corrected" - after various transformations to account for camera position
 
-    // whenever something can corrupt its internal state due to nan/inf, elide the call
-    if (is_nan(value))
-    {
-        nanp = true;
-        logger.write_pose(value); // "filtered"
-    }
-    else
+    nanp |= is_nan(value);
+
     {
-        Pose tmp(value);
+        {
+            Pose tmp(value);
 
-        if (libs.pFilter)
-            libs.pFilter->filter(tmp, value);
+            // nan/inf values will corrupt filter internal state
+            if (!nanp && libs.pFilter)
+                libs.pFilter->filter(tmp, value);
+
+            logger.write_pose(value); // "filtered"
+        }
+
+        euler_t neck, rel;
 
-        logger.write_pose(value); // "filtered"
+        {
+            double ny = s.neck_y, nz = -s.neck_z;
+
+            if (ny != 0 || nz != 0)
+            {
+                const rmat R = euler_to_rmat(
+                                   euler_t(value(Yaw)   * d2r,
+                                           value(Pitch) * d2r,
+                                           value(Roll)  * d2r));
+                euler_t xyz(0, ny, nz);
+                t_compensate(R, xyz, xyz, false, false, false);
+                neck(TX) = xyz(TX);
+                neck(TY) = xyz(TY) - ny;
+                neck(TZ) = xyz(TZ) - nz;
+            }
+        }
 
         // CAVEAT rotation only, due to tcomp
         for (int i = 3; i < 6; i++)
@@ -294,38 +309,45 @@ void Tracker::logic()
             for (int i = 0; i < 6; i++)
                 value(i) = 0;
 
-        if (is_nan(value))
-            nanp = true;
-    }
+        const bool reltrans = !get(f_tcomp_disabled);
 
-    if (s.tcomp_p && !get(f_tcomp_disabled))
-    {
-        const double tcomp_c[] =
+        if (s.tcomp_p && reltrans)
         {
-            double(!s.tcomp_disable_src_yaw),
-            double(!s.tcomp_disable_src_pitch),
-            double(!s.tcomp_disable_src_roll),
-        };
-        euler_t value_(value(TX), value(TY), value(TZ));
-        t_compensate(euler_to_rmat(
-                         euler_t(value(Yaw)   * d2r * tcomp_c[0],
-                                 value(Pitch) * d2r * tcomp_c[1],
-                                 value(Roll)  * d2r * tcomp_c[2])),
-                     value_,
-                     value_,
-                     s.tcomp_disable_tx,
-                     s.tcomp_disable_ty,
-                     s.tcomp_disable_tz);
-        if (is_nan(value_))
-            nanp = true;
+            const double tcomp_c[] =
+            {
+                double(!s.tcomp_disable_src_yaw),
+                double(!s.tcomp_disable_src_pitch),
+                double(!s.tcomp_disable_src_roll),
+            };
+            const rmat R = euler_to_rmat(
+                       euler_t(value(Yaw)   * d2r * tcomp_c[0],
+                               value(Pitch) * d2r * tcomp_c[1],
+                               value(Roll)  * d2r * tcomp_c[2]));
+            euler_t ret;
+            t_compensate(R,
+                         euler_t(value(TX), value(TY), value(TZ)),
+                         ret,
+                         s.tcomp_disable_tx,
+                         s.tcomp_disable_ty,
+                         s.tcomp_disable_tz);
+
+            for (int i = 0; i < 3; i++)
+                rel(i) = ret(i) - value(i);
+        }
+
+        // don't t_compensate existing compensated values
         for (int i = 0; i < 3; i++)
-            value(i) = value_(i);
+            value(i) += neck(i) + rel(i);
+
+        nanp |= is_nan(neck) | is_nan(rel) | is_nan(value);
     }
 
     // CAVEAT translation only, due to tcomp
     for (int i = 0; i < 3; i++)
         value(i) = map(value(i), m(i));
 
+    nanp |= is_nan(value);
+
     for (int i = 0; i < 6; i++)
         if (m(i).opts.invert)
             value(i) = -value(i);
@@ -334,17 +356,18 @@ void Tracker::logic()
 
     if (nanp)
     {
-        value = last_mapped;
+        QMutexLocker foo(&mtx);
+
+        value = output_pose;
+        raw = raw_6dof;
 
         // for widget last value display
         for (int i = 0; i < 6; i++)
-            (void) map(value(i), m(i));
+            (void) map(raw_6dof(i), m(i));
     }
 
-    libs.pProtocol->pose(value);
-
-    last_mapped = value;
-    last_raw = raw;
+    if (!nanp)
+        libs.pProtocol->pose(value);
 
     QMutexLocker foo(&mtx);
     output_pose = value;