ewma: don't filter twice with same input value

1 files changed, 18 insertions, 0 deletions

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index 8f20da13..1a09cee3 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -77,6 +77,24 @@ void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
         return;
     }
 
+    bool new_frame = false;
+
+    for (int i = 0; i < 6; i++)
+    {
+        if (target_camera_position[i] != current_camera_position[i])
+        {
+            new_frame = true;
+            break;
+        }
+    }
+
+    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.