lerp class to simplify filter flow

1 files changed, 45 insertions, 0 deletions

diff --git a/facetracknoir/lerp.hpp b/facetracknoir/lerp.hpp
new file mode 100644
index 00000000..70d5995c
--- /dev/null
+++ b/facetracknoir/lerp.hpp
@@ -0,0 +1,45 @@
+#pragma once
+
+#include "facetracknoir/timer.hpp"
+#include <algorithm>
+
+class lerp {
+private:
+    double last[2][6], dt;
+    Timer t;
+public:
+    lerp() :
+        last { {0,0,0,0,0,0}, {0,0,0,0,0,0} }, dt(1)
+    {
+    }
+    bool idempotentp(const double* input)
+    {
+        for (int i = 0; i < 6; i++)
+            if (last[0][i] != input[i])
+                return false;
+        return true;
+    }
+
+    void write(const double* input, double* output)
+    {
+        const double q = dt;
+        dt = std::max(1, t.start());
+
+        const double c = std::max(std::min(1.0, q/(double)dt), 0.0);
+
+        for (int i = 0; i < 6; i++)
+        {
+            last[1][i] = last[0][i];
+            last[0][i] = input[i];
+        }
+
+        for (int i = 0; i < 6; i++)
+            output[i] = last[1][i] + (last[0][i] - last[1][i]) * c;
+    }
+
+    void get_state(double* state)
+    {
+        for (int i = 0; i < 6; i++)
+            state[i] = last[0][i];
+    }
+};