diff options
Diffstat (limited to 'tracker-pt')
| -rw-r--r-- | tracker-pt/point-filter.cpp | 18 |
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]); } |