diff options
Diffstat (limited to 'logic')
| -rw-r--r-- | logic/main-settings.hpp | 10 | ||||
| -rw-r--r-- | logic/pipeline.cpp | 72 | ||||
| -rw-r--r-- | logic/pipeline.hpp | 10 |
3 files changed, 70 insertions, 22 deletions
diff --git a/logic/main-settings.hpp b/logic/main-settings.hpp index 8fef7ea7..0795bca4 100644 --- a/logic/main-settings.hpp +++ b/logic/main-settings.hpp @@ -22,6 +22,14 @@ enum reltrans_state reltrans_non_center = 2, }; +enum centering_state +{ + center_disabled = 0, + center_point = 1, + center_vr360 = 2, + center_roll_compensated = 3, +}; + namespace main_settings_impl { using namespace options; @@ -65,7 +73,7 @@ struct OTR_LOGIC_EXPORT main_settings final value<bool> tray_start { b, "start-in-tray", false }; value<bool> center_at_startup { b, "center-at-startup", true }; - //value<int> center_method; + value<centering_state> centering_mode { b, "centering-mode", center_roll_compensated };; value<int> neck_z { b, "neck-depth", 0 }; value<bool> neck_enable { b, "neck-enable", false }; diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp index 39c75e99..f8827b7e 100644 --- a/logic/pipeline.cpp +++ b/logic/pipeline.cpp @@ -272,7 +272,7 @@ bool pipeline::maybe_enable_center_on_tracking_started() return false; } -void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) +void pipeline::maybe_set_center_pose(const centering_state mode, const Pose& value, bool own_center_logic) { if (b.get(f_center | f_held_center)) { @@ -283,28 +283,66 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic) if (own_center_logic) { - center.inv_R = rmat::eye(); - center.T = {}; + center.P = {}; + center.QC = QQuaternion(1,0,0,0); + center.QR = QQuaternion(1,0,0,0); } else { - center.inv_R = euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)).t(); - center.T = Pose_(&value[TX]); + if (mode) + { + center.P = value; + center.QC = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]).conjugated(); + center.QR = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], 0).conjugated(); + } + else + { + // To reset the centering coordinates + // just select "Centering method [Disabled]" and then press [Center] button + center.P = {}; + center.QC = QQuaternion(1,0,0,0); + center.QR = QQuaternion(1,0,0,0); + } } } } -Pose pipeline::apply_center(Pose value) const +Pose pipeline::apply_center(const centering_state mode, Pose value) const { - { - for (unsigned k = 0; k < 3; k++) - value(k) -= center.T(k); - - Pose_ rot = rmat_to_euler( - euler_to_rmat(Pose_(&value[Yaw]) * (M_PI / 180)) * center.inv_R - ); - for (unsigned k = 0; k < 3; k++) - value(k+3) = rot(k) * 180 / M_PI; + if (mode != center_disabled) + { + for (unsigned k = TX; k <= TZ; k++) + value(k) -= center.P(k); + + QQuaternion q; + QVector3D v; + + switch (mode) + { + case center_point: + for (unsigned k = Yaw; k <= Roll; k++) + { + value(k) -= center.P(k); + if (fabs(value[k]) > 180) value[k] -= copysign(360, value[k]); + } + break; + case center_vr360: + q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], -value[Roll]); + q = center.QC * q; + v = q.toEulerAngles(); + value[Pitch] = v.x(); + value[Yaw] = v.y(); + value[Roll] = -v.z(); + break; + case center_roll_compensated: + q = QQuaternion::fromEulerAngles(value[Pitch], value[Yaw], center.P[Roll] - value[Roll]); + q = center.QR * q; + v = q.toEulerAngles(); + value[Pitch] = v.x(); + value[Yaw] = v.y(); + value[Roll] = -v.z(); + break; + } } for (int i = 0; i < 6; i++) @@ -407,8 +445,8 @@ void pipeline::logic() { maybe_enable_center_on_tracking_started(); - maybe_set_center_pose(value, own_center_logic); - value = apply_center(value); + maybe_set_center_pose(s.centering_mode, value, own_center_logic); + value = apply_center(s.centering_mode, value); // "corrected" - after various transformations to account for camera position logger.write_pose(value); diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp index fc1f2060..80d4d8a7 100644 --- a/logic/pipeline.hpp +++ b/logic/pipeline.hpp @@ -20,6 +20,7 @@ #include <atomic> #include <cmath> +#include <QQuaternion> #include "export.hpp" @@ -99,8 +100,9 @@ class OTR_LOGIC_EXPORT pipeline : private QThread reltrans rel; struct { - rmat inv_R = rmat::eye(); - Pose_ T; + Pose P; + QQuaternion QC = QQuaternion(1,0,0,0); + QQuaternion QR = QQuaternion(1,0,0,0); } center; time_units::ms backlog_time {}; @@ -111,8 +113,8 @@ class OTR_LOGIC_EXPORT pipeline : private QThread void logic(); void run() override; bool maybe_enable_center_on_tracking_started(); - void maybe_set_center_pose(const Pose& value, bool own_center_logic); - Pose apply_center(Pose value) const; + void maybe_set_center_pose(const centering_state mode, const Pose& value, bool own_center_logic); + Pose apply_center(const centering_state mode, Pose value) const; std::tuple<Pose, Pose, vec6_bool> get_selected_axis_values(const Pose& newpose) const; Pose maybe_apply_filter(const Pose& value) const; Pose apply_reltrans(Pose value, vec6_bool disabled, bool centerp); |