logic/pipeline: center through arithmetic only

This is incorrect but people like it.

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;