1 files changed, 52 insertions, 66 deletions

diff --git a/logic/pipeline.hpp b/logic/pipeline.hpp
index 82a339a3..a539525d 100644
--- a/logic/pipeline.hpp
+++ b/logic/pipeline.hpp
@@ -1,11 +1,3 @@
-/* Copyright (c) 2014-2015, Stanislaw Halik <sthalik@misaki.pl>
-
- * Permission to use, copy, modify, and/or distribute this
- * software for any purpose with or without fee is hereby granted,
- * provided that the above copyright notice and this permission
- * notice appear in all copies.
- */
-
 #pragma once
 
 #include <vector>
@@ -14,8 +6,8 @@
 #include "api/plugin-support.hpp"
 #include "mappings.hpp"
 #include "compat/euler.hpp"
+#include "compat/enum-operators.hpp"
 #include "runtime-libraries.hpp"
-#include "extensions.hpp"
 
 #include "spline/spline.hpp"
 #include "main-settings.hpp"
@@ -27,74 +19,74 @@
 
 #include <atomic>
 #include <cmath>
+#include <QQuaternion>
 
 #include "export.hpp"
 
-namespace gui_tracker_impl {
+namespace pipeline_impl {
 
-using rmat = euler::rmat;
-using euler_t = euler::euler_t;
+using namespace euler;
+using namespace time_units;
 
 using vec6_bool = Mat<bool, 6, 1>;
 using vec3_bool = Mat<bool, 6, 1>;
 
 class reltrans
 {
-    euler_t interp_pos, last_value;
+    Pose_ interp_pos;
     Timer interp_timer;
 
     // this implements smooth transition into reltrans mode
     // once not aiming anymore. see `apply_pipeline()'.
     Timer interp_phase_timer;
-    unsigned RC_phase;
+    unsigned RC_stage = 0;
 
-    bool cur = false;
+    bool moving_to_reltans = false;
     bool in_zone = false;
 
 public:
     reltrans();
 
-    warn_result_unused
-    euler_t rotate(const rmat& rmat, const euler_t& in, vec3_bool disable) const;
+    void on_center();
 
-    warn_result_unused
+    Pose_ rotate(const rmat& rmat, const Pose_& in, vec3_bool disable) const;
+    Pose_ apply_neck(const rmat& R, int nz, bool disable_tz) const;
     Pose apply_pipeline(reltrans_state state, const Pose& value,
                         const vec6_bool& disable, bool neck_enable, int neck_z);
-
-    warn_result_unused
-    euler_t apply_neck(const Pose& value, int nz) const;
 };
 
-using namespace time_units;
+enum bit_flags : unsigned {
+    f_none           = 0,
+    f_center         = 1 << 0,
+    f_held_center    = 1 << 1,
+    f_enabled_h      = 1 << 2,
+    f_enabled_p      = 1 << 3,
+    f_zero           = 1 << 4,
+};
 
 struct OTR_LOGIC_EXPORT bits
 {
-    enum flags : unsigned {
-        f_center         = 1 << 0,
-        f_enabled_h      = 1 << 1,
-        f_enabled_p      = 1 << 2,
-        f_zero           = 1 << 3,
-    };
-
-    std::atomic<unsigned> b;
-
-    void set(flags flag_, bool val_);
-    void negate(flags flag_);
-    bool get(flags flag);
+    bit_flags flags{0};
+    QMutex lock;
+
+    void set(bit_flags flag, bool val);
+    void negate(bit_flags flag);
+    bool get(bit_flags flag);
     bits();
 };
 
-class OTR_LOGIC_EXPORT pipeline : private QThread, private bits
+DEFINE_ENUM_OPERATORS(bit_flags);
+
+class OTR_LOGIC_EXPORT pipeline : private QThread
 {
     Q_OBJECT
-private:
-    QMutex mtx;
+
+    mutable QMutex mtx;
     main_settings s;
-    Mappings& m;
-    event_handler& ev;
+    const Mappings& m;
 
     Timer t;
-    Pose output_pose, raw_6dof, last_mapped, last_raw;
+    Pose output_pose, raw_6dof, last_value;
 
     Pose newpose;
     runtime_libraries const& libs;
@@ -103,42 +95,36 @@ private:
     // the logger while the tracker is running.
     TrackLogger& logger;
 
-    struct state
-    {
-        rmat inv_rot_center;
-        rmat rotation;
-
-        state() : inv_rot_center(rmat::eye())
-        {}
-    };
-
     reltrans rel;
 
-    state rotation;
-    euler_t t_center;
+    struct {
+		Pose P;
+		QQuaternion QC = QQuaternion(1,0,0,0);
+		QQuaternion QR = QQuaternion(1,0,0,0);
+    } center;
 
-    ns backlog_time = ns(0);
+    time_units::ms backlog_time {};
 
     bool tracking_started = false;
 
-    double map(double pos, Map& axis);
+    static double map(double pos, const Map& axis);
     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 clamp_value(Pose value) const;
-    Pose apply_center(Pose value) const;
-    std::tuple<Pose, Pose, vec6_bool> get_selected_axis_value(const Pose& newpose) 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);
+    Pose apply_reltrans(Pose value, vec6_bool disabled, bool centerp);
     Pose apply_zero_pos(Pose value) const;
 
-    // reminder: float exponent base is 2
-    //static constexpr inline double c_mult = 16;
-    //static constexpr inline double c_div = 1./c_mult;
+    void set_center(bool x);
+
+    bits b;
+
 public:
-    pipeline(Mappings& m, runtime_libraries& libs, event_handler& ev, TrackLogger& logger);
-    ~pipeline();
+    pipeline(const Mappings& m, const runtime_libraries& libs, TrackLogger& logger);
+    ~pipeline() override;
 
     void raw_and_mapped_pose(double* mapped, double* raw) const;
     void start() { QThread::start(QThread::HighPriority); }
@@ -146,11 +132,11 @@ public:
     void toggle_zero();
     void toggle_enabled();
 
-    void set_center();
+    void set_held_center(bool value);
     void set_enabled(bool value);
     void set_zero(bool value);
 };
 
-} // ns impl
+} // ns pipeline_impl
 
-using gui_tracker_impl::pipeline;
+using pipeline = pipeline_impl::pipeline;