From 6cba92a8ac9c66c01d484f4235e41752e751fa71 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Fri, 13 Jun 2014 07:50:56 +0200
Subject: accela: fix bugs introducing lack of idempotence

sq
---
 ftnoir_filter_accela/ftnoir_filter_accela.cpp | 29 ++++++---------------------
 1 file changed, 6 insertions(+), 23 deletions(-)

(limited to 'ftnoir_filter_accela/ftnoir_filter_accela.cpp')

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 31cb94bd..9fcf4b92 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -23,16 +23,6 @@ static inline double parabola(const double a, const double x, const double dz, c
     return a1 * pow(std::max<double>(fabs(x) - dz, 0), 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)
 {
@@ -46,8 +36,6 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
                 last_output[j][i] = target_camera_position[i];
         }
 
-        timer.start();
-        frame_delta = 1;
         first_run = false;
 		return;
 	}
@@ -63,19 +51,15 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
         }
     }
 
-    if (new_frame)
+    if (!new_frame)
     {
         for (int i = 0; i < 6; i++)
-            last_input[i] = target_camera_position[i];
-        frame_delta = timer.start();
-    } else {
-        auto d = timer.elapsed();
-        double c = clamp(0.0, 1.0, d / (double) frame_delta);
-        for (int i = 0; i < 6; i++)
-            new_camera_position[i] =
-                    last_output[1][i] + (last_output[0][i] - last_output[1][i]) * c;
+            new_camera_position[i] = last_output[0][i];
         return;
     }
+
+    for (int i = 0; i < 6; i++)
+        last_input[i] = target_camera_position[i];
     
     for (int i=0;i<6;i++)
 	{
@@ -93,10 +77,9 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
                 parabola(a3, vec3, deadzone, s.expt);
         const double result = last_output[0][i] + velocity;
         const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
-        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];
+        last_output[0][i] = new_camera_position[i] = done ? target_camera_position[i] : result;
 	}
 }
 
-- 
cgit v1.2.3