From 61592edd7b953c35167fb95f1d26aecc4e5c594a Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 5 Apr 2019 20:28:02 +0200
Subject: logic/pipeline: kill improper center mode

Issue: #912, #925
---
 logic/pipeline.cpp | 22 ++++++++++++++++++----
 logic/pipeline.hpp |  5 ++++-
 2 files changed, 22 insertions(+), 5 deletions(-)

(limited to 'logic')

diff --git a/logic/pipeline.cpp b/logic/pipeline.cpp
index 2b3d9313..c540aa4e 100644
--- a/logic/pipeline.cpp
+++ b/logic/pipeline.cpp
@@ -281,16 +281,30 @@ void pipeline::maybe_set_center_pose(const Pose& value, bool own_center_logic)
             libs.pFilter->center();
 
         if (own_center_logic)
-            center = Pose();
+        {
+            center.inv_R = rmat::eye();
+            center.T = {};
+        }
         else
-            center = value;
+        {
+            center.inv_R = euler_to_rmat(euler_t(&value[Yaw]) * (M_PI / 180)).t();
+            center.T = euler_t(&value[TX]);
+        }
     }
 }
 
 Pose pipeline::apply_center(Pose value) const
 {
-    // this is incorrect but people like it
-    value = value - center;
+    {
+        for (unsigned k = 0; k < 3; k++)
+            value(k) -= center.T(k);
+
+        euler_t rot = rmat_to_euler(
+            euler_to_rmat(euler_t(&value[Yaw]) * (M_PI / 180)) * center.inv_R
+        );
+        for (unsigned k = 0; k < 3; k++)
+            value(k+3) = rot(k) * 180 / M_PI;
+    }
 
     for (int i = 0; i < 6; i++)
         // don't invert after reltrans
diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 1471b310..cf87dfca 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -97,7 +97,10 @@ class OTR_LOGIC_EXPORT pipeline : private QThread
 
     reltrans rel;
 
-    Pose center;
+    struct {
+        rmat inv_R = rmat::eye();
+        euler_t T;
+    } center;
 
     time_units::ns backlog_time {};
 
-- 
cgit v1.2.3