tracker/pt: improve point filter somewhat

Moving LEDs at different speeds results in nonsensical poses so use the
deltas from the fastest moving point.

1 files changed, 11 insertions, 7 deletions

diff --git a/tracker-pt/point-filter.cpp b/tracker-pt/point-filter.cpp
index 7a6f48c1..85841993 100644
--- a/tracker-pt/point-filter.cpp
+++ b/tracker-pt/point-filter.cpp
@@ -1,4 +1,6 @@
 #include "point-filter.hpp"
+#include <algorithm>
+#include <cmath>
 #include <QDebug>
 
 namespace pt_point_filter_impl {
@@ -29,27 +31,29 @@ const PointOrder& point_filter::operator()(const PointOrder& input)
         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;
+        return A * std::pow((f)10, (f)-log10_pos) * rest;
     );
-    f dist[3], norm = 0;
+
+    f dist = 0;
 
     for (unsigned i = 0; i < 3; i++)
     {
         vec2 tmp = input[i] - state_[i];
         f x = sqrt(tmp.dot(tmp));
-        dist[i] = x;
-        norm += x;
+        dist = std::max((f)0, x);
     }
 
-    if (norm < (f)1e-6)
+    if (dist < (f)1e-6)
         return state_;
 
     f dt = (f)t->elapsed_seconds(); t->start();
-    f delta = pow(norm, E) * C * dt; // gain
+    f delta = std::pow(dist, E) * C * dt; // gain
+
+    //qDebug() << "gain" << std::min((f)1, delta);
 
     for (unsigned i = 0; i < 3; i++)
     {
-        f x = std::clamp(delta * dist[i] / norm, (f)0, (f)1);
+        f x = std::clamp(delta, (f)0, (f)1);
         state_[i] += x*(input[i] - state_[i]);
     }