accela: simplify, don't filter empty updates

Signed-off-by: Stanislaw Halik <sthalik@misaki.pl>

1 files changed, 51 insertions, 11 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];

 	}

 }