summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--filter-accela-hamilton/accela_hamilton.cpp71
-rw-r--r--filter-accela-hamilton/accela_hamilton.h8
2 files changed, 40 insertions, 39 deletions
diff --git a/filter-accela-hamilton/accela_hamilton.cpp b/filter-accela-hamilton/accela_hamilton.cpp
index 5e7660a8..7fde90b8 100644
--- a/filter-accela-hamilton/accela_hamilton.cpp
+++ b/filter-accela-hamilton/accela_hamilton.cpp
@@ -6,14 +6,13 @@
* copyright notice and this permission notice appear in all copies.
*/
#include "accela_hamilton.h"
-#include "compat/math.hpp"
#include "api/plugin-api.hpp"
#include "opentrack/defs.hpp"
#include <algorithm>
-#include <QDebug>
-#include <QMutexLocker>
+#include "compat/math.hpp"
+#include "compat/hamilton-tools.h"
#include "compat/math-imports.hpp"
#include "compat/time.hpp"
@@ -26,10 +25,10 @@ accela_hamilton::accela_hamilton()
void accela_hamilton::filter(const double* input, double *output)
{
- constexpr float EPSILON = 1e-15F;
+ constexpr double EPSILON = 1e-15F;
- const QQuaternion current_rot = QQuaternion::fromEulerAngles(input[Pitch], input[Yaw], input[Roll]);
- const QVector3D current_pos(input[TX], input[TY], input[TZ]);
+ const tQuat current_rot = QuatFromYPR(input + Yaw);
+ const tVector current_pos(input[TX], input[TY], input[TZ]);
if (unlikely(first_run))
{
@@ -44,38 +43,38 @@ void accela_hamilton::filter(const double* input, double *output)
return;
}
- const float pos_thres{s.pos_smoothing};
- const float pos_dz{ s.pos_deadzone};
+ const double pos_thres{s.pos_smoothing};
+ const double pos_dz{ s.pos_deadzone};
- const float dt = t.elapsed_seconds();
+ const double dt = t.elapsed_seconds();
t.start();
// Position
{
- const QVector3D delta = current_pos - last_position;
- const float delta_len = delta.length();
- QVector3D delta_normed = delta_len>0.F ? delta/delta_len : QVector3D(); // Zero vector when length was zero.
- const float gain = dt*spline_pos.get_value_no_save(std::max(0.F, delta_len-pos_dz) / pos_thres);
- const QVector3D output_pos = last_position + gain * delta_normed;
- output[TX] = output_pos.x();
- output[TY] = output_pos.y();
- output[TZ] = output_pos.z();
+ 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 float zoomed_smoothing = [this](float output_z) {
+ 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 float max_zoomed_smoothing {s.max_zoomed_smoothing};
- const float max_z {s.max_z};
+ 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 float z = std::clamp(-output_z, 0.F, max_z);
+ const double z = std::clamp(-output_z, 0., max_z);
return max_zoomed_smoothing * z / (max_z + EPSILON);
}(output[TZ]);
- const float rot_dz{ s.rot_deadzone};
- const float rot_thres = float{s.rot_smoothing} + zoomed_smoothing;
+ const double rot_dz{ s.rot_deadzone};
+ const double rot_thres = double{s.rot_smoothing} + zoomed_smoothing;
// Rotation
{
@@ -85,19 +84,21 @@ void accela_hamilton::filter(const double* input, double *output)
// the new orientation, but that's fine.
// Compute rotation angle and axis which brings the previous orientation to the current rotation
- QVector3D axis;
- float angle;
- (last_rotation.conjugated() * current_rot).getAxisAndAngle(&axis, &angle);
- // Apply the Accela gain magic. Also need to multiply with dt here.
- angle = std::max(0.f, angle - std::copysign(rot_dz, angle)) / rot_thres;
- const float gain_angle = dt*spline_rot.get_value_no_save(std::abs(angle)) * signum(angle);
- // Rotate toward the measured orientation. We take the already computed axis. But the angle is now the accela gain.
- const QQuaternion output_rot = last_rotation * QQuaternion::fromAxisAndAngle(axis, gain_angle);
+ 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
- const QVector3D output_euler = output_rot.toEulerAngles();
- output[Pitch] = output_euler.x();
- output[Yaw] = output_euler.y();
- output[Roll] = output_euler.z();
+ QuatToYPR(output_rot, output + Yaw);
last_rotation = output_rot;
}
}
diff --git a/filter-accela-hamilton/accela_hamilton.h b/filter-accela-hamilton/accela_hamilton.h
index ccd74fbf..1afbfd8c 100644
--- a/filter-accela-hamilton/accela_hamilton.h
+++ b/filter-accela-hamilton/accela_hamilton.h
@@ -13,11 +13,11 @@
#include "api/plugin-api.hpp"
#include "compat/timer.hpp"
#include "compat/variance.hpp"
+#include "compat/hamilton-tools.h"
#include <QMutex>
#include <QTimer>
-#include <QQuaternion>
-#include <QVector3D>
+
//#define DEBUG_ACCELA
@@ -30,8 +30,8 @@ struct accela_hamilton : IFilter
module_status initialize() override { return status_ok(); }
private:
settings_accela_hamilton s;
- QVector3D last_position = {};
- QQuaternion last_rotation = {};
+ tVector last_position = {};
+ tQuat last_rotation = {};
Timer t;
#if defined DEBUG_ACCELA
Timer debug_timer;