1 files changed, 121 insertions, 0 deletions
diff --git a/filter-accela-hamilton/accela_hamilton.cpp b/filter-accela-hamilton/accela_hamilton.cpp
new file mode 100644
index 00000000..7fde90b8
--- /dev/null
+++ b/filter-accela-hamilton/accela_hamilton.cpp
@@ -0,0 +1,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)