2 files changed, 60 insertions, 49 deletions

diff --git a/facetracknoir/tracker.cpp b/facetracknoir/tracker.cpp
index 4a80c722..f17b257f 100644
--- a/facetracknoir/tracker.cpp
+++ b/facetracknoir/tracker.cpp
@@ -36,14 +36,13 @@ Tracker::~Tracker()
     wait();
 }
 
-void Tracker::get_curve(double pos, double& out, Mapping& axis) {
+double Tracker::map(double pos, Mapping& axis) {
     bool altp = (pos < 0) && axis.opts.altp;
     axis.curve.setTrackingActive( !altp );
     axis.curveAlt.setTrackingActive( altp );
     auto& fc = altp ? axis.curveAlt : axis.curve;
-    out = (axis.opts.invert ? -1 : 1) * fc.getValue(pos);
-
-    out += axis.opts.zero;
+    double invert = axis.opts.invert ? -1 : 1;
+    return invert * (fc.getValue(pos) + axis.opts.zero);
 }
 
 void Tracker::t_compensate(const double* input, double* output, bool rz)
@@ -82,24 +81,14 @@ void Tracker::t_compensate(const double* input, double* output, bool rz)
         output[i] = ret(i);
 }
 
-void Tracker::run() {
-    Pose pose_offset, unstopped_pose;
-
-    double newpose[6] = {0};
-    const int sleep_ms = 3;
-
-#if defined(_WIN32)
-    (void) timeBeginPeriod(1);
-#endif
+void Tracker::logic()
+{
+    Libraries->pTracker->GetHeadPoseData(newpose);
+    
+    Pose final_raw;
 
-    while (!should_quit)
+    if (enabledp)
     {
-        t.start();
-
-        Libraries->pTracker->GetHeadPoseData(newpose);
-
-        Pose final_raw, filtered;
-
         for (int i = 0; i < 6; i++)
         {
             auto& axis = m(i);
@@ -109,37 +98,57 @@ void Tracker::run() {
             // not really raw, after axis remap -sh
             final_raw(i) = newpose[k];
         }
+        unstopped_raw = final_raw;
+    }
+    
+    Pose filtered_pose;
+    
+    if (Libraries->pFilter)
+        Libraries->pFilter->FilterHeadPoseData(final_raw, filtered_pose);
+    else
+        filtered_pose = final_raw;
+    
+    if (centerp)
+    {
+        centerp = false;
+        raw_center = final_raw;
+    }
+    
+    Pose raw_centered = filtered_pose & raw_center;
+    
+    Pose mapped_pose_precomp;
+    
+    for (int i = 0; i < 6; i++)
+        mapped_pose_precomp(i) = map(raw_centered(i), m(i));
+    
+    Pose mapped_pose;
+    
+    if (s.tcomp_p)
+        t_compensate(mapped_pose_precomp, mapped_pose, s.tcomp_tz);
+    else
+        mapped_pose = mapped_pose_precomp;
 
-        {
-            if (enabledp)
-                unstopped_pose = final_raw;
-
-            if (Libraries->pFilter)
-                Libraries->pFilter->FilterHeadPoseData(unstopped_pose, filtered);
-            else
-                filtered = unstopped_pose;
-            
-            if (centerp)  {
-                centerp = false;
-                pose_offset = filtered;
-            }
-
-            filtered = filtered & pose_offset;
-
-            for (int i = 0; i < 6; i++)
-                get_curve(filtered(i), filtered(i), m(i));
-        }
+    Libraries->pProtocol->sendHeadposeToGame(mapped_pose);
 
-        if (s.tcomp_p)
-            t_compensate(filtered, filtered, s.tcomp_tz);
+    {
+        QMutexLocker foo(&mtx);
+        output_pose = mapped_pose;
+        raw_6dof = unstopped_raw;
+    }
+}
 
-        Libraries->pProtocol->sendHeadposeToGame(filtered);
+void Tracker::run() {
+    const int sleep_ms = 3;
 
-        {
-            QMutexLocker foo(&mtx);
-            output_pose = filtered;
-            raw_6dof = final_raw;
-        }
+#if defined(_WIN32)
+    (void) timeBeginPeriod(1);
+#endif
+
+    while (!should_quit)
+    {
+        t.start();
+
+        logic();
 
         const long q = 1000L * std::max(0L, sleep_ms - t.elapsed_ms());
         usleep(q);
diff --git a/facetracknoir/tracker.h b/facetracknoir/tracker.h
index d65e1cf1..afb86802 100644
--- a/facetracknoir/tracker.h
+++ b/facetracknoir/tracker.h
@@ -23,12 +23,14 @@ private:
     // XXX can be const-cast when functionconfig const-correct -sh 20141004
     Mappings& m;
     Timer t;
-    Pose output_pose, raw_6dof;
+    Pose output_pose, raw_6dof, raw_center, unstopped_raw;
+    double newpose[6];
     std::atomic<bool> centerp;
     std::atomic<bool> enabledp;
     std::atomic<bool> should_quit;
 
-    static void get_curve(double pos, double& out, Mapping& axis);
+    double map(double pos, Mapping& axis);
+    void logic();
     static void t_compensate(const double* input, double* output, bool rz);
 protected:
     void run() override;