1 files changed, 72 insertions, 0 deletions
diff --git a/tracker-pt/point-filter.cpp b/tracker-pt/point-filter.cpp
new file mode 100644
index 00000000..10448fe2
--- /dev/null
+++ b/tracker-pt/point-filter.cpp
@@ -0,0 +1,72 @@
+#include "point-filter.hpp"
+#include <algorithm>
+#include <cmath>
+#include <QDebug>
+
+namespace pt_point_filter_impl {
+
+void point_filter::reset()
+{
+    t = std::nullopt;
+}
+
+const PointOrder& point_filter::operator()(const PointOrder& input, f deadzone_amount)
+{
+    using std::fmod;
+    using std::sqrt;
+    using std::pow;
+    using std::clamp;
+
+    if (!s.enable_point_filter)
+    {
+        t = std::nullopt;
+        state_ = input;
+        return state_;
+    }
+
+    if (!t)
+    {
+        t.emplace();
+        state_ = input;
+        return state_;
+    }
+
+    constexpr auto E = (f)1.75;
+    const f limit = (f)*s.point_filter_limit;
+    const f C = progn(
+        constexpr int A = 1'000'000;
+        double K = *s.point_filter_coefficient;
+        f log10_pos = -2 + (int)K, rest = (f)(.999-fmod(K, 1.)*.9);
+        return A * pow((f)10, (f)-log10_pos) * rest;
+    );
+
+    f dist = 0, dz = deadzone_amount * (f)s.point_filter_deadzone / 800; // sqrt(640^2 + 480^2)
+
+    for (unsigned i = 0; i < 3; i++)
+    {
+        vec2 tmp = input[i] - state_[i];
+        f x = sqrt(tmp.dot(tmp));
+        x = std::max((f)0, x - dz);
+        dist = std::max(dist, x);
+    }
+
+    if (dist < (f)1e-6)
+        return state_;
+
+    f dt = (f)t->elapsed_seconds(); t->start();
+    f delta = pow(dist, E) * C * dt; // gain
+
+    //qDebug() << "gain" << std::min((f)1, delta);
+
+    for (unsigned i = 0; i < 3; i++)
+    {
+        f x = clamp(delta, (f)0, limit);
+        state_[i] += x*(input[i] - state_[i]);
+    }
+
+    return state_;
+}
+
+point_filter::point_filter(const pt_settings& s) : s{s} {}
+
+} // ns pt_point_filter_impl