1 files changed, 93 insertions, 39 deletions

diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index 36b5cad4..06a90305 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -8,10 +8,8 @@
 
 #pragma once
 
-#include <vector>
-
 #include "opentrack-compat/timer.hpp"
-#include "plugin-support.hpp"
+#include "opentrack/plugin-support.hpp"
 #include "mappings.hpp"
 #include "simple-mat.hpp"
 #include "selected-libraries.hpp"
@@ -23,32 +21,70 @@
 #include <QMutex>
 #include <QThread>
 
-class Pose {
-private:
-    static constexpr double pi = 3.141592653;
-    static constexpr double d2r = pi/180.0;
-    static constexpr double r2d = 180./pi;
-
-    double axes[6];
-public:
-    Pose() : axes {0,0,0, 0,0,0} {}
-
-    inline operator double*() { return axes; }
-    inline operator const double*() const { return axes; }
+#include <atomic>
+#include <vector>
 
-    inline double& operator()(int i) { return axes[i]; }
-    inline double operator()(int i) const { return axes[i]; }
+#include "export.hpp"
+
+using Pose = Mat<double, 6, 1>;
+
+struct bits
+{
+    enum flags {
+        f_center         = 1 << 0,
+        f_enabled        = 1 << 1,
+        f_zero           = 1 << 2,
+        f_tcomp_disabled = 1 << 3,
+        f_should_quit    = 1 << 4,
+    };
+
+    std::atomic<unsigned> b;
+
+    void set(flags flag_, bool val_)
+    {
+        unsigned b_(b);
+        const unsigned flag = unsigned(flag_);
+        const unsigned val = unsigned(!!val_);
+        while (!b.compare_exchange_weak(b_,
+                                        unsigned((b_ & ~flag) | (flag * val)),
+                                        std::memory_order_seq_cst,
+                                        std::memory_order_seq_cst))
+        { /* empty */ }
+    }
+
+    void negate(flags flag_)
+    {
+        unsigned b_(b);
+        const unsigned flag = unsigned(flag_);
+        while (!b.compare_exchange_weak(b_,
+                                        (b_ & ~flag) | (flag & ~b_),
+                                        std::memory_order_seq_cst,
+                                        std::memory_order_seq_cst))
+        { /* empty */ }
+    }
+
+    bool get(flags flag)
+    {
+        return !!(b & flag);
+    }
+
+    bits() : b(0u)
+    {
+        set(f_center, true);
+        set(f_enabled, true);
+        set(f_zero, false);
+        set(f_tcomp_disabled, false);
+        set(f_should_quit, false);
+    }
 };
 
-#ifdef BUILD_api
-#   include "opentrack-compat/export.hpp"
-#else
-#   include "opentrack-compat/import.hpp"
-#endif
-
-class OPENTRACK_EXPORT Tracker : private QThread {
+class OPENTRACK_API_EXPORT Tracker : private QThread, private bits
+{
     Q_OBJECT
 private:
+    using rmat = euler::rmat;
+    using euler_t = euler::euler_t;
+
     QMutex mtx;
     main_settings& s;
     Mappings& m;
@@ -56,31 +92,49 @@ private:
     Timer t;
     Pose output_pose, raw_6dof, last_mapped, last_raw;
 
-    double newpose[6];
-    volatile bool centerp;
-    volatile bool enabledp;
-    volatile bool zero_;
-    volatile bool should_quit;
+    Pose newpose;
     SelectedLibraries const& libs;
 
-    using rmat = dmat<3, 3>;
-    
-    dmat<3, 3> r_b;
-    double t_b[3];
+    struct state
+    {
+        rmat center_yaw, center_pitch, center_roll;
+        rmat rot_center;
+        rmat camera;
+        rmat rotation, ry, rp, rr;
+
+        state() : center_yaw(rmat::eye()), center_pitch(rmat::eye()), center_roll(rmat::eye()), rot_center(rmat::eye())
+        {}
+    };
+
+    state real_rotation, scaled_rotation;
+    euler_t t_center;
 
     double map(double pos, Mapping& axis);
     void logic();
-
-    void t_compensate(const dmat<3, 3>& rmat, const double* ypr, double* output, bool rz);
+    void t_compensate(const rmat& rmat, const euler_t& ypr, euler_t& output, bool rz);
     void run() override;
-    
+
+    static constexpr double pi = M_PI;
+    static constexpr double r2d = 180. / M_PI;
+    static constexpr double d2r = M_PI / 180.;
+
+    // note: float exponent base is 2
+    static constexpr double c_mult = 4;
+    static constexpr double c_div = 1./c_mult;
 public:
     Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs);
     ~Tracker();
 
+    rmat get_camera_offset_matrix(double c);
     void get_raw_and_mapped_poses(double* mapped, double* raw) const;
     void start() { QThread::start(); }
-    void toggle_enabled() { qDebug() << "toggle enabled"; enabledp = !enabledp; }
-    void center() { qDebug() << "toggle center"; centerp = !centerp; }
-    void zero() { qDebug() << "toggle zero"; zero_ = !zero_; }
+
+    void center() { set(f_center, true); }
+
+    void set_toggle(bool value) { set(f_enabled, value); }
+    void set_zero(bool value) { set(f_zero, value); }
+    void set_tcomp_disabled(bool x) { set(f_tcomp_disabled, x); }
+
+    void zero() { negate(f_zero); }
+    void toggle_enabled() { negate(f_enabled); }
 };