Combine Accela Filter with Hamilton Filter

filter-accela-hamilton/CMakeLists.txt

@@ -0,0 +1,2 @@
+otr_module(filter-accela-hamilton)
+target_link_libraries(opentrack-filter-accela-hamilton opentrack-spline)

filter-accela-hamilton/accela_hamilton.cpp

@@ -0,0 +1,120 @@
+/* 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 "compat/math.hpp"
+#include "api/plugin-api.hpp"
+#include "opentrack/defs.hpp"
+
+#include <algorithm>
+#include <QDebug>
+#include <QMutexLocker>
+
+#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 float 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]);
+
+    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 float pos_thres{s.pos_smoothing};
+    const float pos_dz{ s.pos_deadzone};
+
+    const float 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();
+        last_position = output_pos;
+    }
+
+    // Zoom smoothing:
+    const float zoomed_smoothing = [this](float 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};
+        // 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);
+        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;
+
+    // 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
+        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);
+        // 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();
+        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)

filter-accela-hamilton/accela_hamilton.h

@@ -0,0 +1,69 @@
+/* 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.
+ */
+#pragma once
+
+#include "ui_accela_hamilton_filtercontrols.h"
+
+#include "accela_hamilton_settings.hpp"
+#include "api/plugin-api.hpp"
+#include "compat/timer.hpp"
+#include "compat/variance.hpp"
+
+#include <QMutex>
+#include <QTimer>
+#include <QQuaternion>
+#include <QVector3D>
+
+//#define DEBUG_ACCELA
+
+struct accela_hamilton : IFilter
+{
+    accela_hamilton();
+    void filter(const double* input, double *output) override;
+    void center() override { first_run = true; }
+    spline spline_rot, spline_pos;
+    module_status initialize() override { return status_ok(); }
+private:
+    settings_accela_hamilton s;
+    QVector3D last_position = {};
+    QQuaternion last_rotation = {};
+    Timer t;
+#if defined DEBUG_ACCELA
+    Timer debug_timer;
+    double debug_max;
+    variance var;
+#endif
+    bool first_run = true;
+};
+
+class dialog_accela_hamilton : public IFilterDialog
+{
+    Q_OBJECT
+public:
+    dialog_accela_hamilton();
+    void register_filter(IFilter*) override {}
+    void unregister_filter() override {}
+    void save() override;
+    void reload() override;
+    bool embeddable() noexcept override { return true; }
+    void set_buttons_visible(bool x) override;
+private:
+    Ui::AccelaUICdialog_accela ui;
+    settings_accela_hamilton s;
+private slots:
+    void doOK();
+    void doCancel();
+};
+
+class accela_hamiltonDll : public Metadata
+{
+    Q_OBJECT
+
+    QString name() override { return tr("AccelaHamilton"); }
+    QIcon icon() override { return QIcon(":/images/filter-16.png"); }
+};

filter-accela-hamilton/accela_hamilton_dialog.cpp

@@ -0,0 +1,99 @@
+/* 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 <cmath>
+#include <QDebug>
+#include <algorithm>
+#include <QDoubleSpinBox>
+#include "api/plugin-api.hpp"
+#include "spline/spline-widget.hpp"
+#include <QDialog>
+
+using namespace options;
+
+dialog_accela_hamilton::dialog_accela_hamilton()
+{
+    ui.setupUi(this);
+
+    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+    connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+
+    tie_setting(s.rot_smoothing, ui.rotation_slider);
+    tie_setting(s.pos_smoothing, ui.translation_slider);
+    tie_setting(s.rot_deadzone, ui.rot_dz_slider);
+    tie_setting(s.pos_deadzone, ui.trans_dz_slider);
+
+    tie_setting(s.rot_smoothing, ui.rot_gain, [](const slider_value& s) { return tr("%1°").arg(s, 0, 'g', 4); });
+    tie_setting(s.pos_smoothing, ui.trans_gain, [](const slider_value& s) { return tr("%1mm").arg(s, 0, 'g', 4); });
+    tie_setting(s.rot_deadzone, ui.rot_dz, [](const slider_value& s) { return tr("%1°").arg(s, 0, 'g', 4); });
+    tie_setting(s.pos_deadzone, ui.trans_dz, [](const slider_value& s) { return tr("%1mm").arg(s); });
+
+    tie_setting(s.max_zoomed_smoothing, ui.max_zoomed_smoothing);
+    tie_setting(s.max_zoomed_smoothing, ui.lb_max_zoomed_smoothing, [](double x)
+      { return tr("%1°").arg(x, 0, 'g', 3);});
+
+    tie_setting(s.max_z, ui.max_z);
+    tie_setting(s.max_z, ui.lb_max_z, [](double x)
+      { return tr("%1mm").arg(x, 0, 'g', 3);});
+
+//#define SPLINE_ROT_DEBUG
+//#define SPLINE_TRANS_DEBUG
+
+#if defined SPLINE_ROT_DEBUG || defined SPLINE_TRANS_DEBUG
+    {
+        spline rot, pos;
+        s.make_splines(rot, pos);
+
+#ifdef SPLINE_ROT_DEBUG
+        QDialog dr;
+        spline_widget r(&dr);
+        dr.setWindowTitle("Accela rotation gain"); r.set_preview_only(true); r.setEnabled(true);
+        r.setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); r.set_config(&rot);
+        r.setFixedSize(1024, 600);
+        dr.show();
+        dr.exec();
+#endif
+
+#ifdef SPLINE_TRANS_DEBUG
+        QDialog dt;
+        spline_widget t(&dt);
+        dt.setWindowTitle("Accela translation gain"); t.set_preview_only(true); t.setEnabled(true);
+        dt.setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); t.set_config(&pos);
+        t.setFixedSize(1024, 600);
+        dt.show();
+        dt.exec();
+#endif
+    }
+#endif
+}
+
+void dialog_accela_hamilton::doOK()
+{
+    save();
+    close();
+}
+
+void dialog_accela_hamilton::doCancel()
+{
+    close();
+}
+
+void dialog_accela_hamilton::save()
+{
+    s.b->save();
+}
+
+void dialog_accela_hamilton::reload()
+{
+    s.b->reload();
+}
+
+void dialog_accela_hamilton::set_buttons_visible(bool x)
+{
+    ui.buttonBox->setVisible(x);
+}