1 files changed, 24 insertions, 25 deletions

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 8a98e528..370d19db 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -12,24 +12,24 @@
 #include "opentrack/plugin-api.hpp"
 using namespace std;
 
-FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0, 0,0,0 }
+FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0 }
 {
 }
 
-static inline double parabola(const double a, const double x, const double dz, const double expt)
+static inline double dz(double x, double dz)
 {
-    const double sign = x > 0 ? 1 : -1;
-    const double a1 = 1./a;
-    return a1 * pow(std::max(0., fabs(x) - dz), expt) * sign;
+    return std::max(0., fabs(x) - dz) * (x < 0. ? -1. : 1.);
 }
 
 void FTNoIR_Filter::filter(const double* input, double *output)
 {
     if (first_run)
     {
+        for (int i = 0; i < 3; i++)
+            fast_state[i] = 0;
+
         for (int i = 0; i < 6; i++)
         {
-            fast_state[i] = 0;
             output[i] = input[i];
             for (int j = 0; j < 3; j++)
                 last_output[i] = input[i];
@@ -38,46 +38,45 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         return;
     }
 
-    const double fast_c = s.fast_alpha / 100.;
     const double rot_dz = s.rot_deadzone;
     const double trans_dz = s.trans_deadzone;
-    const double rot_a = s.rotation_alpha;
-    const double trans_a = s.translation_alpha;
-    const double expt = s.expt;
+
+    const int s_rot_plus = s.rot_plus, s_rot_minus = s.rot_minus;
+
+    const double a_rot_plus = s_rot_plus/100.;
+    const double a_rot_minus = s_rot_minus/100. * a_rot_plus;
+    const double a_trans = s.trans_smoothing/100.;
 
     static constexpr double fast_alpha = Hz/(Hz + fast_alpha_seconds);
 
-    for (int i=0;i<6;i++)
+    for (int i = 0; i < 6; i++)
     {
         const double vec = input[i] - last_output[i];
-        const double a = i >= 3 ? rot_a : trans_a;
-        const double deadzone = i >= 3 ? rot_dz : trans_dz;
-
-        double datum;
+        double datum = Hz * 16;
 
         if (i >= 3)
         {
-            const double cur_fast = std::abs(vec) * fast_alpha + fast_state[i]*(1. - fast_alpha);
-            fast_state[i] = cur_fast;
-            const double how_fast = std::max(0., fast_c * (cur_fast - max_slow_delta));
-            datum = parabola(a, vec * (1.-damping + (1.+damping)*how_fast), deadzone, s.expt);
+            int k = i - 3;
+            const double vec_ = dz(vec, rot_dz);
+            const double cur_fast = fabs(vec_) * fast_alpha + fast_state[k]*(1. - fast_alpha);
+            fast_state[k] = cur_fast;
+            const double c = cur_fast > max_slow_delta ? a_rot_plus : a_rot_minus;
+            datum *= vec_ * c;
         }
         else
-            datum = parabola(a, vec, deadzone, expt);
+            datum *= dz(vec, trans_dz) * a_trans;
 
         const double result = last_output[i] + datum;
         const bool negp = vec < 0.;
         const bool done = negp ? result <= input[i] : result >= input[i];
 
-        last_output[i] = last_output[i];
-        last_output[i] = last_output[i];
         const double ret = done ? input[i] : result;
         last_output[i] = output[i] = ret;
     }
 
-    state.y = output[Yaw] - input[Yaw];
-    state.p = output[Pitch] - input[Pitch];
-    state.r = output[Roll] - input[Roll];
+    state.y = fast_state[0];
+    state.p = fast_state[1];
+    state.r = fast_state[2];
 }
 
 extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()