blob: 7a6f48c18fd5816362a622c562d9d34bc0980fe1 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
|
#include "point-filter.hpp"
#include <QDebug>
namespace pt_point_filter_impl {
void point_filter::reset()
{
t = std::nullopt;
}
const PointOrder& point_filter::operator()(const PointOrder& input)
{
if (!s.enable_point_filter)
{
t = std::nullopt;
state_ = input;
return state_;
}
if (!t)
{
t.emplace();
state_ = input;
return state_;
}
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 dist[3], norm = 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;
}
if (norm < (f)1e-6)
return state_;
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 = 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} {}
} // ns pt_point_filter_impl
|
