summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-06-14 12:48:11 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-06-14 12:48:11 +0200
commitb10de6a3dbe1f16e288f049414508c6d84feecde (patch)
treec677763784cfcadfd1a86ab59ab4b9d98aa833bc
parent146540da2d1427a4172eb422cff94d89f03ce891 (diff)
lerp class to simplify filter flow
-rw-r--r--facetracknoir/lerp.hpp45
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];
+ }
+};