diff options
| -rw-r--r-- | logic/pipeline.cpp | 51 | ||||
| -rw-r--r-- | logic/pipeline.hpp | 14 |
2 files changed, 12 insertions, 53 deletions
diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp index eb5ac17a..357b213c 100644 --- a/logic/pipeline.cpp +++ b/logic/pipeline.cpp @@ -292,26 +292,12 @@ void pipeline::set_center_pose(const Pose& value, bool own_center_logic) libs.pFilter->center(); if (own_center_logic) - { - scaled_state.inv_rot_center = rmat::eye(); - scaled_state.t_center = {}; - } + center = {}; else - { - scaled_state.inv_rot_center = scaled_state.rotation.t(); - scaled_state.t_center = (const double*)(value); - } + center = value; } } -void pipeline::store_tracker_pose(const Pose& value) -{ - // alas, this is poor man's gimbal lock "prevention" - // this is some kind of "canonical" representation, - // if we can even talk of one - scaled_state.rotation = euler_to_rmat(scale_inv_c * d2r * euler_t(&value[Yaw])); -} - Pose pipeline::clamp_value(Pose value) const { // hatire, udp, and freepie trackers can mess up here @@ -330,28 +316,14 @@ Pose pipeline::clamp_value(Pose value) const Pose pipeline::apply_center(Pose value) const { - euler_t T = euler_t(value) - scaled_state.t_center; - euler_t R = scale_c * rmat_to_euler(scaled_state.rotation * scaled_state.inv_rot_center); + // this is incorrect but people like it + value = value - center; - // XXX check these lines, it's been here forever - T = rel.rotate(euler_to_rmat(R), T, {}); - - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 6; i++) // don't invert after reltrans // inverting here doesn't break centering - - if (m(i+3).opts.invert) - R(i) = -R(i); if (m(i).opts.invert) - T(i) = -T(i); - } - - for (int i = 0; i < 3; i++) - { - value(i) = T(i); - value(i+3) = R(i) * r2d; - } + value(i) = -value(i); return value; } @@ -449,8 +421,6 @@ void pipeline::logic() value = clamp_value(value); { - store_tracker_pose(value); - maybe_enable_center_on_tracking_started(); set_center_pose(value, own_center_logic); value = apply_center(value); @@ -463,10 +433,11 @@ void pipeline::logic() ev.run_events(EV::ev_before_filter, value); // we must proceed with all the filtering since the filter // needs fresh values to prevent deconvergence - Pose tmp = maybe_apply_filter(value); - nan_check(tmp); - if (!center_ordered) - value = tmp; + if (center_ordered) + (void)maybe_apply_filter(value); + else + value = maybe_apply_filter(value); + nan_check(value); logger.write_pose(value); // "filtered" } diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp index 142a3844..9fdd6c32 100644 --- a/logic/pipeline.hpp +++ b/logic/pipeline.hpp @@ -31,16 +31,6 @@ using namespace time_units; using vec6_bool = Mat<bool, 6, 1>; using vec3_bool = Mat<bool, 6, 1>; -static constexpr inline double scale_c = 8; -static constexpr inline double scale_inv_c = 1/scale_c; - -struct state_ -{ - rmat inv_rot_center { rmat::eye() }; - rmat rotation { rmat::eye() }; - euler_t t_center; -}; - class reltrans { euler_t interp_pos; @@ -108,8 +98,7 @@ class OTR_LOGIC_EXPORT pipeline : private QThread reltrans rel; - //state_ state, scaled_state; - state_ scaled_state; + Pose center; ns backlog_time {}; @@ -120,7 +109,6 @@ class OTR_LOGIC_EXPORT pipeline : private QThread void run() override; bool maybe_enable_center_on_tracking_started(); void set_center_pose(const Pose& value, bool own_center_logic); - void store_tracker_pose(const Pose& value); Pose clamp_value(Pose value) const; Pose apply_center(Pose value) const; std::tuple<Pose, Pose, vec6_bool> get_selected_axis_values(const Pose& newpose) const; |