diff options
| -rw-r--r-- | gui/options-dialog.cpp | 3 | ||||
| -rw-r--r-- | gui/options-dialog.ui | 522 | ||||
| -rw-r--r-- | logic/main-settings.hpp | 3 | ||||
| -rw-r--r-- | logic/tracker.cpp | 193 |
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; |