From 16d0d632ea10f25845407b4fb483a5d8c83260d4 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Mon, 2 Dec 2013 09:30:19 +0100
Subject: accela: simplify, don't filter empty updates

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>
---
 ftnoir_filter_accela/ftnoir_filter_accela.cpp | 62 ++++++++++++++++++++++-----
 ftnoir_filter_accela/ftnoir_filter_accela.h   |  8 ++--
 2 files changed, 56 insertions(+), 14 deletions(-)

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index e2fd7cc0..fd47f18d 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -73,6 +73,16 @@ static inline double parabola(const double a, const double x, const double dz, c
     return a1 * pow(std::max<double>(fabs(x) - dz, 1e-3), expt) * sign;
 }
 
+template<typename T>
+static inline T clamp(const T min, const T max, const T value)
+{
+    if (value < min)
+        return min;
+    if (value > max)
+        return max;
+    return value;
+}
+
 void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
                                        double *new_camera_position,
                                        const double* last_post_filter_values)
@@ -82,22 +92,51 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
         for (int i = 0; i < 6; i++)
         {
             new_camera_position[i] = target_camera_position[i];
-            current_camera_position[i] = target_camera_position[i];
-            current_camera_position_2[i] = target_camera_position[i];
-            current_camera_position_3[i] = target_camera_position[i];
+            last_input[i] = target_camera_position[i];
+            for (int j = 0; j < 3; j++)
+                last_output[j][i] = target_camera_position[i];
         }
 
-		first_run = false;
+        timer.start();
+        frame_delta = 1;
+        first_run = false;
 		return;
 	}
+
+    bool new_frame = false;
+
+    for (int i = 0; i < 6; i++)
+    {
+        if (target_camera_position[i] != last_input[i])
+        {
+            new_frame = true;
+            break;
+        }
+    }
+
+    if (new_frame)
+    {
+        for (int i = 0; i < 6; i++)
+            last_input[i] = target_camera_position[i];
+        frame_delta = timer.isValid() ? timer.elapsed() : 1;
+        timer.start();
+    } else {
+        auto d = timer.elapsed();
+        double c = clamp(0.0, 1.0, d / (double) frame_delta);
+        QMutexLocker foo(&mutex);
+        for (int i = 0; i < 6; i++)
+            new_camera_position[i] =
+                    last_output[1][i] + (last_output[0][i] - last_output[1][i]) * c;
+        return;
+    }
     
     QMutexLocker foo(&mutex);
 
     for (int i=0;i<6;i++)
 	{
-        const double vec = target_camera_position[i] - current_camera_position[i];
-        const double vec2 = target_camera_position[i] - current_camera_position_2[i];
-        const double vec3 = target_camera_position[i] - current_camera_position_3[i];
+        const double vec = target_camera_position[i] - last_output[0][i];
+        const double vec2 = target_camera_position[i] - last_output[1][i];
+        const double vec3 = target_camera_position[i] - last_output[2][i];
 		const int sign = vec < 0 ? -1 : 1;
         const double a = i >= 3 ? rotation_alpha : translation_alpha;
         const double a2 = a * second_order_alpha;
@@ -107,11 +146,12 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
                 parabola(a, vec * scaling[i], deadzone, expt) * reduction +
                 parabola(a2, vec2 * scaling[i], deadzone, expt) * reduction +
                 parabola(a3, vec3 * scaling[i], deadzone, expt) * reduction;
-        const double result = current_camera_position[i] + velocity;
+        const double result = last_output[0][i] + velocity;
         const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
-        current_camera_position_3[i] = current_camera_position_2[i];
-        current_camera_position_2[i] = current_camera_position[i];
-        new_camera_position[i] = current_camera_position[i] = done ? target_camera_position[i] : result;
+        new_camera_position[i] = done ? target_camera_position[i] : result;
+        last_output[2][i] = last_output[1][i];
+        last_output[1][i] = last_output[0][i];
+        last_output[0][i] = new_camera_position[i];
 	}
 }
 
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index f5c0b8bc..c92e28df 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -30,6 +30,7 @@
 #include "ui_ftnoir_accela_filtercontrols.h"
 #include "facetracknoir/global-settings.h"
 #include <QMutex>
+#include <QElapsedTimer>
 
 #define ACCELA_SMOOTHING_ROTATION 60.0
 #define ACCELA_SMOOTHING_TRANSLATION 40.0
@@ -56,12 +57,13 @@ private:
 	bool first_run;
     double rotation_alpha, translation_alpha, zoom_factor;
     double second_order_alpha, third_order_alpha;
-    double current_camera_position[6];
-    double current_camera_position_2[6];
-    double current_camera_position_3[6];
     double scaling[6];
     double deadzone;
     double expt;
+    double last_input[6];
+    double last_output[3][6];
+    QElapsedTimer timer;
+    qint64 frame_delta;
 };
 
 //*******************************************************************************************************
-- 
cgit v1.2.3