1 files changed, 25 insertions, 16 deletions
diff --git a/opentrack/tracker.h b/opentrack/tracker.h
index 462f4e50..ace9fa3c 100644
--- a/opentrack/tracker.h
+++ b/opentrack/tracker.h
@@ -1,16 +1,16 @@
 #pragma once
 
-#include <atomic>
 #include <vector>
 
-#include "./timer.hpp"
-#include "./plugin-support.h"
-#include "./mappings.hpp"
-#include "./pose.hpp"
+#include "timer.hpp"
+#include "plugin-support.h"
+#include "mappings.hpp"
+#include "pose.hpp"
+#include "simple-mat.hpp"
 
 #include "../qfunctionconfigurator/functionconfig.h"
-#include "./main-settings.hpp"
-#include "./options.hpp"
+#include "main-settings.hpp"
+#include "options.hpp"
 
 #include <QMutex>
 #include <QThread>
@@ -21,26 +21,35 @@ private:
     QMutex mtx;
     main_settings& s;
     Mappings& m;
-    
+
     Timer t;
-    Pose output_pose, raw_6dof, raw_center, final_raw;
+    Pose output_pose, raw_6dof;
+
     double newpose[6];
-    std::atomic<bool> centerp;
-    std::atomic<bool> enabledp;
-    std::atomic<bool> should_quit;
+    volatile bool centerp;
+    volatile bool enabledp;
+    volatile bool zero_;
+    volatile bool should_quit;
     SelectedLibraries const& libs;
+
+    using rmat = dmat<3, 3>;
     
+    dmat<3, 3> r_b;
+    double t_b[3];
+
     double map(double pos, Mapping& axis);
     void logic();
-    
-    static void t_compensate(const double* input, double* output, bool rz);
+
+    void t_compensate(const dmat<3, 3>& rmat, const double* ypr, double* output, bool rz);
     void run() override;
+    
 public:
     Tracker(main_settings& s, Mappings& m, SelectedLibraries& libs);
     ~Tracker();
 
     void get_raw_and_mapped_poses(double* mapped, double* raw) const;
     void start() { QThread::start(); }
-    void toggle_enabled() { enabledp.store(!enabledp.load()); }
-    void center() { centerp.store(!centerp.load()); }
+    void toggle_enabled() { enabledp = !enabledp; }
+    void center() { centerp = !centerp; }
+    void zero() { zero_ = !zero_; }
 };