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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
|
/* Copyright (c) 2012-2015 Stanislaw Halik
* Copyright (c) 2023-2024 Michael Welter
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*/
#include "accela_hamilton.h"
#include "api/plugin-api.hpp"
#include "opentrack/defs.hpp"
#include <algorithm>
#include "compat/math.hpp"
#include "compat/hamilton-tools.h"
#include "compat/math-imports.hpp"
#include "compat/time.hpp"
accela_hamilton::accela_hamilton()
{
s.make_splines(spline_rot, spline_pos);
}
void accela_hamilton::filter(const double* input, double *output)
{
constexpr double EPSILON = 1e-15F;
const tQuat current_rot = QuatFromYPR(input + Yaw);
const tVector current_pos(input[TX], input[TY], input[TZ]);
if (unlikely(first_run))
{
first_run = false;
last_rotation = current_rot;
last_position = current_pos;
t.start();
#if defined DEBUG_ACCELA
debug_max = 0;
debug_timer.start();
#endif
return;
}
const double pos_thres{s.pos_smoothing};
const double pos_dz{ s.pos_deadzone};
const double dt = t.elapsed_seconds();
t.start();
// Position
{
const tVector delta = current_pos - last_position;
const double delta_len = VectorLength(delta);
const tVector delta_normed = delta_len>0. ? delta/delta_len : tVector(); // Zero vector when length was zero.
const double gain = dt*spline_pos.get_value_no_save(std::max(0., delta_len-pos_dz) / pos_thres);
const tVector output_pos = last_position + (delta_normed * gain);
output[TX] = output_pos.v[0];
output[TY] = output_pos.v[1];
output[TZ] = output_pos.v[2];
last_position = output_pos;
}
// Zoom smoothing:
const double zoomed_smoothing = [this](double output_z) {
// Local copies because accessing settings involves thread synchronization
// and I don't like this in the middle of math.
const double max_zoomed_smoothing {s.max_zoomed_smoothing};
const double max_z {s.max_z};
// Movement toward the monitor is negative. Negate and clamp it to get a positive value
const double z = std::clamp(-output_z, 0., max_z);
return max_zoomed_smoothing * z / (max_z + EPSILON);
}(output[TZ]);
const double rot_dz{ s.rot_deadzone};
const double rot_thres = double{s.rot_smoothing} + zoomed_smoothing;
// Rotation
{
// Inter/extrapolates along the arc between the old and new orientation.
// It's basically a quaternion spherical linear interpolation, where the
// accela gain x dt is the blending parameter. Might actually overshoot
// the new orientation, but that's fine.
// Compute rotation angle and axis which brings the previous orientation to the current rotation
const double angle = AngleBetween(last_rotation, current_rot);
// Apply the Accela gain magic. The "gain_angle" is the desired rotation from the old orientation
// towards the current. Then alpha is the blending factor for the SLerp operation. It is normalized
// to the range [0,1] where 1 corresponds to the current orientation, i.e. it is the fractional
// rotation relative to the "gain_angle". EPSILON is added to prevent division by zero.
// Additionally we use std::min(1., ...) to clamp the blending. alpha>1 would probably not make much
// sense since it would mean extrapolation beyond the current orientation. And it would be a rare
// edge case. Secondly idk if Slerp supports alpha>1.
const double normalized_angle = std::max(0., angle - rot_dz) / rot_thres;
const double gain_angle = dt*spline_rot.get_value_no_save(std::abs(normalized_angle));
const double alpha = std::min(1., gain_angle / (angle + EPSILON));
// Rotate toward the measured orientation.
const tQuat output_rot = Slerp(last_rotation, current_rot, alpha);
// And back to Euler angles
QuatToYPR(output_rot, output + Yaw);
last_rotation = output_rot;
}
}
namespace detail::accela_hamilton {
void settings_accela_hamilton::make_splines(spline& rot, spline& pos)
{
rot.clear(); pos.clear();
for (const auto& val : rot_gains)
rot.add_point({ val.x, val.y });
for (const auto& val : pos_gains)
pos.add_point({ val.x, val.y });
}
} // ns detail::accela_hamilton
OPENTRACK_DECLARE_FILTER(accela_hamilton, dialog_accela_hamilton, accela_hamiltonDll)
|
