update accela, ewma to use lerp class

4 files changed, 50 insertions, 76 deletions

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 9fcf4b92..a82ff077 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -30,57 +30,40 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
 	{

         for (int i = 0; i < 6; i++)

         {

-            new_camera_position[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];

+            l.write(target_camera_position, new_camera_position);

         }

 

         first_run = false;

-		return;

-	}

-

-    bool new_frame = false;

+        return;

+    }

 

-    for (int i = 0; i < 6; i++)

+    if (!l.idempotentp(target_camera_position))

     {

-        if (target_camera_position[i] != last_input[i])

+        for (int i=0;i<6;i++)

         {

-            new_frame = true;

-            break;

+            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 ? s.rotation_alpha : s.translation_alpha;

+            const double a2 = a * s.second_order_alpha;

+            const double a3 = a * s.third_order_alpha;

+            const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone;

+            const double velocity =

+                    parabola(a, vec, deadzone, s.expt) +

+                    parabola(a2, vec2, deadzone, s.expt) +

+                    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];

+            last_output[2][i] = last_output[1][i];

+            last_output[1][i] = last_output[0][i];

+            last_output[0][i] = done ? target_camera_position[i] : result;

         }

     }

 

-    if (!new_frame)

-    {

-        for (int i = 0; i < 6; i++)

-            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++)

-	{

-        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 ? s.rotation_alpha : s.translation_alpha;

-        const double a2 = a * s.second_order_alpha;

-        const double a3 = a * s.third_order_alpha;

-        const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone;

-        const double velocity =

-                parabola(a, vec, deadzone, s.expt) +

-                parabola(a2, vec2, deadzone, s.expt) +

-                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];

-        last_output[2][i] = last_output[1][i];

-        last_output[1][i] = last_output[0][i];

-        last_output[0][i] = new_camera_position[i] = done ? target_camera_position[i] : result;

-	}

+    l.write(last_output[0], new_camera_position);

 }

 

 extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index 8cf7fe66..2187fd01 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -2,6 +2,7 @@
 #include "ftnoir_filter_base/ftnoir_filter_base.h"

 #include "ui_ftnoir_accela_filtercontrols.h"

 #include "facetracknoir/global-settings.h"

+#include "facetracknoir/lerp.hpp"

 #include <QMutex>

 

 #define ACCELA_SMOOTHING_ROTATION 60.0

@@ -44,11 +45,11 @@ public:
     void receiveSettings() {

         s.b->reload();

     }

-

 private:

+    lerp l;

     settings s;

 	bool first_run;

-    double last_input[6], last_output[3][6];

+    double last_output[3][6];

 };

 

 class FilterControls: public QWidget, public IFilterDialog

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index c073bfa4..dcc1475c 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -64,54 +64,43 @@ void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
 {
     double new_delta, new_noise, norm_noise;
     double alpha;
+    double pos[6];
 
     //On the first run, initialize to output=target and return.
     if (first_run==true) {
         for (int i=0;i<6;i++) {
-            new_camera_position[i] = target_camera_position[i];
-            current_camera_position[i] = target_camera_position[i];
             delta[i] = 0.0;
             noise[i] = 0.0;
+            l.write(target_camera_position, new_camera_position);
         }
         first_run=false;
         return;
     }
 
-    bool new_frame = false;
-
-    for (int i = 0; i < 6; i++)
+    if (!l.idempotentp(target_camera_position))
     {
-        if (target_camera_position[i] != current_camera_position[i])
-        {
-            new_frame = true;
-            break;
+        double cur[6];
+        l.get_state(cur);
+        // Calculate the new camera position.
+        for (int i=0;i<6;i++) {
+            // Calculate the current and smoothed delta.
+            new_delta = target_camera_position[i]-cur[i];
+            delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i];
+            // Calculate the current and smoothed noise variance.
+            new_noise = delta[i]*delta[i];
+            noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i];
+            // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs).
+            norm_noise = std::min<double>(new_noise/(9.0*noise[i]), 1.0);
+            // Calculate the alpha from the normalized noise.
+            // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise).
+            alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing));
+            // Update the current camera position to the new position.
+            double value = alpha*target_camera_position[i] + (1.0-alpha)*cur[i];
+            pos[i] = value;
         }
     }
 
-    if (!new_frame)
-    {
-        for (int i = 0; i < 6; i++)
-            new_camera_position[i] = current_camera_position[i];
-        return;
-    }
-
-    // Calculate the new camera position.
-    for (int i=0;i<6;i++) {
-        // Calculate the current and smoothed delta.
-        new_delta = target_camera_position[i]-current_camera_position[i];
-        delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i];
-        // Calculate the current and smoothed noise variance.
-        new_noise = delta[i]*delta[i];
-        noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[i];
-        // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs).
-        norm_noise = std::min<double>(new_noise/(9.0*noise[i]), 1.0);
-        // Calculate the alpha from the normalized noise.
-        // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise).
-        alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing));
-        // Update the current camera position to the new position.
-        double pos = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i];
-        new_camera_position[i] = current_camera_position[i] = pos;
-    }
+    l.write(pos, new_camera_position);
 }
 
 extern "C" FTNOIR_FILTER_BASE_EXPORT IFilter* CALLING_CONVENTION GetConstructor()
diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
index bde3e79c..ea25a135 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
@@ -32,6 +32,7 @@
 #include <QWidget>
 #include <QMutex>
 #include "facetracknoir/options.h"
+#include "facetracknoir/lerp.hpp"
 using namespace options;
 
 struct settings {
@@ -61,8 +62,8 @@ private:
     double noise_smoothing;
     double delta[6];
     double noise[6];
-    double current_camera_position[6];
     settings s;
+    lerp l;
 };
 
 class FilterControls: public QWidget, public IFilterDialog