filter/accela: filter all rot/pos DOF together

Apply gain to Euclidean distance of rotation/position change,
rather to each DOF separately. Weight each DOF's separate length
and normalize it.

This makes diagonal movements less staircase-y.

You'll need smaller sensitivity values given Euclid of (x,y,z)
is smaller than |x|+|y|+|z|.

3 files changed, 120 insertions, 54 deletions

diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp
index 164d0ed5..24768dea 100644
--- a/filter-accela/ftnoir_filter_accela.cpp
+++ b/filter-accela/ftnoir_filter_accela.cpp
@@ -12,14 +12,14 @@
 #include "api/plugin-api.hpp"
 
 constexpr double settings_accela::rot_gains[16][2];
-constexpr double settings_accela::trans_gains[16][2];
+constexpr double settings_accela::pos_gains[16][2];
 
 accela::accela() : first_run(true)
 {
-    s.make_splines(rot, trans);
+    s.make_splines(spline_rot, spline_pos);
 }
 
-double accela::get_delta(double val, double prev, double& degen)
+double accela::get_rot_delta(double val, double prev, double& degen)
 {
     using std::fabs;
     using std::copysign;
@@ -45,6 +45,61 @@ double accela::get_delta(double val, double prev, double& degen)
     }
 }
 
+template <typename T>
+static inline constexpr T signum(T x)
+{
+    return T((T(0) < x) - (x < T(0)));
+}
+
+using std::fabs;
+using std::sqrt;
+using std::pow;
+using std::copysign;
+using std::max;
+
+template<int N = 3, typename F>
+static void do_deltas(const double* deltas, double* output, F&& fun)
+{
+    double norm[N];
+
+    const double dist = progn(
+        double ret = 0;
+        for (unsigned k = 0; k < N; k++)
+            ret += deltas[k]*deltas[k];
+        return sqrt(ret);
+    );
+
+    const double value = double(fun(dist));
+
+    for (unsigned k = 0; k < N; k++)
+    {
+        const double c = dist > 1e-6 ? clamp((fabs(deltas[k]) / dist), 0., 1.) : 0;
+        norm[k] = c;
+    }
+
+    progn(
+        double n = 0;
+        for (unsigned k = 0; k < N; k++)
+            n += norm[k];
+
+        if (n > 1e-6)
+        {
+            const double ret = 1./n;
+            for (unsigned k = 0; k < N; k++)
+                norm[k] *= ret;
+        }
+        else
+            for (unsigned k = 0; k < N; k++)
+                norm[k] = 0;
+    );
+
+    for (unsigned k = 0; k < N; k++)
+    {
+        const double d = norm[k] * value;
+        output[k] = signum(deltas[k]) * d;
+    }
+}
+
 void accela::filter(const double* input, double *output)
 {
     if (first_run)
@@ -61,47 +116,64 @@ void accela::filter(const double* input, double *output)
         return;
     }
 
-    const double rot_t = s.rot_sensitivity().cur();
-    const double trans_t = s.trans_sensitivity().cur();
+    const double rot_thres = s.rot_sensitivity.to<double>();
+    const double pos_thres = s.pos_sensitivity.to<double>();
 
     const double dt = t.elapsed_seconds();
     t.start();
 
-    const double RC = s.ewma().cur() / 1000.; // seconds
+    const double RC = s.ewma.to<double>() / 1000.; // seconds
     const double alpha = dt/(dt+RC);
-    const double rot_dz = s.rot_deadzone().cur();
-    const double trans_dz = s.trans_deadzone().cur();
-    const slider_value nl = s.rot_nonlinearity;
-
-    for (int i = 0; i < 6; i++)
-    {
-        spline& m = i >= 3 ? rot : trans;
+    const double rot_dz = s.rot_deadzone.to<double>();
+    const double pos_dz = s.pos_deadzone.to<double>();
+    const double nl = s.rot_nonlinearity.to<double>();
+    double deltas[6];
 
+    for (unsigned i = 0; i < 6; i++)
         smoothed_input[i] = smoothed_input[i] * (1-alpha) + input[i] * alpha;
 
-        const double in = smoothed_input[i];
+    // rot
 
+    for (unsigned i = 3; i < 6; i++)
+    {
         double degen;
-        const double vec_ = get_delta(in, last_output[i], degen);
-        const double dz = i >= 3 ? rot_dz : trans_dz;
-        const double vec = std::max(0., fabs(vec_) - dz);
-        const double thres = i >= 3 ? rot_t : trans_t;
-        const double out_ = vec / thres;
-        const double out = progn(
-            const bool should_apply_rot_nonlinearity =
-               i >= 3 &&
-               std::fabs(nl.cur() - 1) > 5e-3 &&
-               vec < nl.max();
+        double d = get_rot_delta(smoothed_input[i], last_output[i], degen);
+        d += copysign(rot_dz, -d);
+        deltas[i] = d / rot_thres;
+        last_output[i] += degen;
+    }
 
+    if (nl > 1.)
+    {
+        for (unsigned k = 3; k < 6; k++)
+        {
             static constexpr double nl_end = 1.5;
 
-            if (should_apply_rot_nonlinearity)
-               return std::pow(out_/nl_end, nl.cur()) * nl_end;
-            else
-               return out_;
-        );
-        const double val = double(m.get_value(out));
-        last_output[i] = output[i] = last_output[i] + signum(vec_) * dt * val + degen;
+            if (deltas[k] <= nl_end)
+                deltas[k] = copysign(pow(fabs(deltas[k]/nl_end), nl) * nl_end, deltas[k]);
+        }
+    }
+
+    do_deltas(&deltas[Yaw], &output[Yaw], [this](double x) { return spline_rot.get_value_no_save(x); });
+
+    // pos
+
+    for (unsigned i = 0; i < 3; i++)
+    {
+        double d = smoothed_input[i] - last_output[i];
+        d += copysign(pos_dz, -d);
+        deltas[i] = d / pos_thres;
+    }
+
+    do_deltas(&deltas[TX], &output[TX], [this](double x) { return spline_pos.get_value_no_save(x); });
+
+    // end
+
+    for (unsigned k = 0; k < 6; k++)
+    {
+        output[k] *= dt;
+        output[k] += last_output[k];
+        last_output[k] = output[k];
     }
 }
 
@@ -111,15 +183,15 @@ void settings_accela::make_splines(spline& rot, spline& trans)
     trans = spline();
 
     rot.set_max_input(rot_gains[0][0]);
-    trans.set_max_input(trans_gains[0][0]);
+    trans.set_max_input(pos_gains[0][0]);
     rot.set_max_output(rot_gains[0][1]);
-    trans.set_max_output(trans_gains[0][1]);
+    trans.set_max_output(pos_gains[0][1]);
 
     for (int i = 0; rot_gains[i][0] >= 0; i++)
         rot.add_point(QPointF(rot_gains[i][0], rot_gains[i][1]));
 
-    for (int i = 0; trans_gains[i][0] >= 0; i++)
-        trans.add_point(QPointF(trans_gains[i][0], trans_gains[i][1]));
+    for (int i = 0; pos_gains[i][0] >= 0; i++)
+        trans.add_point(QPointF(pos_gains[i][0], pos_gains[i][1]));
 }
 
 OPENTRACK_DECLARE_FILTER(accela, dialog_accela, accelaDll)
diff --git a/filter-accela/ftnoir_filter_accela.h b/filter-accela/ftnoir_filter_accela.h
index 0e558f1d..b23b8e88 100644
--- a/filter-accela/ftnoir_filter_accela.h
+++ b/filter-accela/ftnoir_filter_accela.h
@@ -20,7 +20,7 @@ public:
     accela();
     void filter(const double* input, double *output) override;
     void center() override { first_run = true; }
-    spline rot, trans;
+    spline spline_rot, spline_pos;
 private:
     settings_accela s;
     bool first_run;
@@ -28,13 +28,7 @@ private:
     double smoothed_input[6];
     Timer t;
 
-    static double get_delta(double val, double prev, double& degen);
-
-    template <typename T>
-    static inline int signum(T x)
-    {
-        return (T(0) < x) - (x < T(0));
-    }
+    static double get_rot_delta(double val, double prev, double& degen);
 };
 
 class dialog_accela: public IFilterDialog
@@ -53,9 +47,9 @@ private slots:
     void doCancel();
     void update_ewma_display(const slider_value& value);
     void update_rot_display(const slider_value& value);
-    void update_trans_display(const slider_value& value);
+    void update_pos_display(const slider_value& value);
     void update_rot_dz_display(const slider_value& value);
-    void update_trans_dz_display(const slider_value&);
+    void update_pos_dz_display(const slider_value&);
     void update_rot_nl_slider(const slider_value& sl);
 };
 
diff --git a/filter-accela/ftnoir_filter_accela_dialog.cpp b/filter-accela/ftnoir_filter_accela_dialog.cpp
index f0ee8391..51789f61 100644
--- a/filter-accela/ftnoir_filter_accela_dialog.cpp
+++ b/filter-accela/ftnoir_filter_accela_dialog.cpp
@@ -21,24 +21,24 @@ dialog_accela::dialog_accela()
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
 
     connect(&s.rot_sensitivity, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_rot_display(const slider_value&)));
-    connect(&s.trans_sensitivity, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_trans_display(const slider_value&)));
+    connect(&s.pos_sensitivity, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_pos_display(const slider_value&)));
     connect(&s.ewma, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_ewma_display(const slider_value&)));
     connect(&s.rot_deadzone, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_rot_dz_display(const slider_value&)));
-    connect(&s.trans_deadzone, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_trans_dz_display(const slider_value&)));
+    connect(&s.pos_deadzone, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_pos_dz_display(const slider_value&)));
     connect(&s.rot_nonlinearity, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_rot_nl_slider(const slider_value&)));
 
     tie_setting(s.rot_sensitivity, ui.rotation_slider);
-    tie_setting(s.trans_sensitivity, ui.translation_slider);
+    tie_setting(s.pos_sensitivity, ui.translation_slider);
     tie_setting(s.ewma, ui.ewma_slider);
     tie_setting(s.rot_deadzone, ui.rot_dz_slider);
-    tie_setting(s.trans_deadzone, ui.trans_dz_slider);
+    tie_setting(s.pos_deadzone, ui.trans_dz_slider);
     tie_setting(s.rot_nonlinearity, ui.rot_nl_slider);
 
     update_rot_display(s.rot_sensitivity);
-    update_trans_display(s.trans_sensitivity);
+    update_pos_display(s.pos_sensitivity);
     update_ewma_display(s.ewma);
     update_rot_dz_display(s.rot_deadzone);
-    update_trans_dz_display(s.trans_deadzone);
+    update_pos_dz_display(s.pos_deadzone);
     update_rot_nl_slider(s.rot_nonlinearity);
 
 //#define SPLINE_ROT_DEBUG
@@ -89,7 +89,7 @@ void dialog_accela::save()
     s.b->save();
 }
 
-#define FIELD(x, a) ((a).arg(((x).cur()), 0, 'g', 4))
+#define FIELD(x, a) ((a).arg(double((x)), 0, 'g', 4))
 #define LIT(x) QStringLiteral(x)
 
 void dialog_accela::update_rot_display(const slider_value& val)
@@ -98,7 +98,7 @@ void dialog_accela::update_rot_display(const slider_value& val)
     ui.rot_gain->setText(FIELD(val, str));
 }
 
-void dialog_accela::update_trans_display(const slider_value& val)
+void dialog_accela::update_pos_display(const slider_value& val)
 {
     ui.trans_gain->setText(FIELD(val, LIT("%1mm")));
 }
@@ -114,7 +114,7 @@ void dialog_accela::update_rot_dz_display(const slider_value& val)
     ui.rot_dz->setText(FIELD(val, str));
 }
 
-void dialog_accela::update_trans_dz_display(const slider_value& val)
+void dialog_accela::update_pos_dz_display(const slider_value& val)
 {
     ui.trans_dz->setText(FIELD(val, LIT("%1mm")));
 }