blob: 4217ac1c14bde213b8e5387fe4e88f4d9c957ad9 (
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
62
|
#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_;
}
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;
);
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];
}
if (norm < (f)1e-6)
return state_;
// gain
float delta = std::clamp(norm * norm * K * dt, (f)0, (f)1);
for (unsigned i = 0; i < 3; i++)
{
f x = delta * dist[i] / norm;
state_[i] += x*(input[i] - state_[i]);
}
return state_;
}
point_filter::point_filter(const pt_settings& s) : s{s}
{
}
} // ns pt_point_filter_impl
|
