1 files changed, 24 insertions, 21 deletions

diff --git a/logic/tracker.cpp b/logic/tracker.cpp
index 8757e98e..a5892dd0 100644
--- a/logic/tracker.cpp
+++ b/logic/tracker.cpp
@@ -26,6 +26,7 @@
 #endif
 
 using namespace euler;
+using namespace gui_tracker_impl;
 
 constexpr double Tracker::r2d;
 constexpr double Tracker::d2r;
@@ -34,7 +35,8 @@ Tracker::Tracker(Mappings& m, SelectedLibraries& libs, TrackLogger& logger) :
     m(m),
     libs(libs),
     logger(logger),
-    backlog_time(0)
+    backlog_time(0),
+    tracking_started(false)
 {
     set(f_center, s.center_at_startup);
 }
@@ -169,28 +171,23 @@ void Tracker::logic()
     scaled_rotation.camera = get_camera_offset_matrix(c_div);
     real_rotation.camera = get_camera_offset_matrix(1);
 
-    scaled_rotation.rotation = scaled_rotation.camera * scaled_rotation.rotation;
-    real_rotation.rotation = real_rotation.camera * real_rotation.rotation;
-
     bool nanp = is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation);
 
     if (!nanp)
     {
-        bool can_center = false;
-
-        if (get(f_center))
+        if (!tracking_started)
         {
             using std::fabs;
 
             for (int i = 0; i < 6; i++)
                 if (fabs(newpose(i)) != 0)
                 {
-                    can_center = true;
+                    tracking_started = true;
                     break;
                 }
         }
 
-        if (can_center)
+        if (get(f_center) && tracking_started)
         {
             set(f_center, false);
 
@@ -260,10 +257,7 @@ void Tracker::logic()
         const euler_t rot = r2d * c_mult * rmat_to_euler(rotation);
         euler_t pos = euler_t(&value[TX]) - t_center;
 
-        if (s.use_camera_offset_from_centering)
-            t_compensate((real_rotation.camera * real_rotation.rot_center).t(), pos, pos, false, false, false);
-        else
-            t_compensate(real_rotation.camera.t(), pos, pos, false, false, false);
+        t_compensate(real_rotation.camera.t(), pos, pos, false, false, false);
 
         for (int i = 0; i < 3; i++)
         {
@@ -452,25 +446,34 @@ void Tracker::get_raw_and_mapped_poses(double* mapped, double* raw) const
 
 void bits::set(bits::flags flag_, bool val_)
 {
-    unsigned b_(b);
     const unsigned flag = unsigned(flag_);
-    const unsigned val = unsigned(!!val_);
-    while (!b.compare_exchange_weak(b_,
+    const unsigned val = unsigned(val_);
+
+    for (;;)
+    {
+        unsigned b_(b);
+        if (b.compare_exchange_weak(b_,
                                     unsigned((b_ & ~flag) | (flag * val)),
                                     std::memory_order_seq_cst,
                                     std::memory_order_seq_cst))
-    { /* empty */ }
+            break;
+    }
 }
 
 void bits::negate(bits::flags flag_)
 {
-    unsigned b_(b);
     const unsigned flag = unsigned(flag_);
-    while (!b.compare_exchange_weak(b_,
-                                    (b_ & ~flag) | (flag & ~b_),
+
+    for (;;)
+    {
+        unsigned b_(b);
+
+        if (b.compare_exchange_weak(b_,
+                                    b_ ^ flag,
                                     std::memory_order_seq_cst,
                                     std::memory_order_seq_cst))
-    { /* empty */ }
+            break;
+    }
 }
 
 bool bits::get(bits::flags flag)