diff options
| -rw-r--r-- | filter-accela/ftnoir_filter_accela.cpp | 142 | ||||
| -rw-r--r-- | filter-accela/ftnoir_filter_accela.h | 14 | ||||
| -rw-r--r-- | filter-accela/ftnoir_filter_accela_dialog.cpp | 18 |
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"))); } |