summaryrefslogtreecommitdiffhomepage
path: root/filter-accela
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2017-03-27 00:22:15 +0200
committerStanislaw Halik <sthalik@misaki.pl>2017-03-27 00:22:15 +0200
commit9f8c137bf602f6625aab115cad73cc1d8308d928 (patch)
treefd20520975ea658c8f56a60b8cd4265ff055dfd5 /filter-accela
parentb78bed514b844e2be7aa85384e578faaa12f287e (diff)
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|.
Diffstat (limited to 'filter-accela')
-rw-r--r--filter-accela/ftnoir_filter_accela.cpp142
-rw-r--r--filter-accela/ftnoir_filter_accela.h14
-rw-r--r--filter-accela/ftnoir_filter_accela_dialog.cpp18
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")));
}