diff options
Diffstat (limited to 'tracker-pt/point-filter.cpp')
| -rw-r--r-- | tracker-pt/point-filter.cpp | 27 |
1 files changed, 13 insertions, 14 deletions
diff --git a/tracker-pt/point-filter.cpp b/tracker-pt/point-filter.cpp index 4217ac1c..7a6f48c1 100644 --- a/tracker-pt/point-filter.cpp +++ b/tracker-pt/point-filter.cpp @@ -24,39 +24,38 @@ const PointOrder& point_filter::operator()(const PointOrder& input) return state_; } - const f K = progn( - constexpr int C = 1'000'000; - f x = (f)*s.point_filter_coefficient; - f log10_pos = -2+(int)x, rest = 1+fmod(x, (f)1)*9; - return C * pow((f)10, (f)-log10_pos) * rest; + constexpr f E = (f)1.75; + 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 dt = (f)t->elapsed_seconds(); t->start(); f dist[3], norm = 0; for (unsigned i = 0; i < 3; i++) { vec2 tmp = input[i] - state_[i]; - dist[i] = sqrt(tmp.dot(tmp)); - norm += dist[i]; + f x = sqrt(tmp.dot(tmp)); + dist[i] = x; + norm += x; } if (norm < (f)1e-6) return state_; - // gain - float delta = std::clamp(norm * norm * K * dt, (f)0, (f)1); + f dt = (f)t->elapsed_seconds(); t->start(); + f delta = pow(norm, E) * C * dt; // gain for (unsigned i = 0; i < 3; i++) { - f x = delta * dist[i] / norm; + f x = std::clamp(delta * dist[i] / norm, (f)0, (f)1); state_[i] += x*(input[i] - state_[i]); } return state_; } -point_filter::point_filter(const pt_settings& s) : s{s} -{ -} +point_filter::point_filter(const pt_settings& s) : s{s} {} } // ns pt_point_filter_impl |