1 files changed, 49 insertions, 35 deletions

diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 37bd8891..dc754b69 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2013 Stanislaw Halik
+/* Copyright (c) 2012-2015 Stanislaw Halik
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -16,52 +16,66 @@ FTNoIR_Filter::FTNoIR_Filter() : first_run(true)
 {
 }
 
-static inline double parabola(const double a, const double x, const double dz, const double expt)
+double FTNoIR_Filter::f(double vec, double thres)
 {
-    const double sign = x > 0 ? 1 : -1;
-    const double a1 = 1./a;
-    return a1 * pow(std::max<double>(fabs(x) - dz, 0), expt) * sign;
+    if (vec > thres*high_thres_c)
+        return (vec - thres*high_thres_c) * high_thres_out + thres*high_thres_c;
+    if (vec > thres)
+        return (vec - thres) * low_thres_mult + thres;
+    return pow(vec, 2.0) / thres;
 }
 
-void FTNoIR_Filter::filter(const double* target_camera_position,
-                                       double *new_camera_position)
+void FTNoIR_Filter::filter(const double* input, double *output)
 {
-	if (first_run)
-	{
+    if (first_run)
+    {
         for (int i = 0; i < 6; i++)
         {
-            new_camera_position[i] = target_camera_position[i];
-            for (int j = 0; j < 3; j++)
-                last_output[j][i] = target_camera_position[i];
+            output[i] = input[i];
+            last_output[i] = input[i];
+            smoothed_input[i] = input[i];
         }
-
         first_run = false;
-		return;
-	}
+        t.start();
+        return;
+    }
+
+    const double rot_t = 7. * (1+s.rot_threshold) / 100.;
+    const double trans_t = 5. * (1+s.trans_threshold) / 100.;
+    
+    const double dt = t.elapsed() * 1e-9;
+    t.start();
 
-    for (int i=0;i<6;i++)
-	{
-        const double vec = target_camera_position[i] - last_output[0][i];
-        const double vec2 = target_camera_position[i] - last_output[1][i];
-        const double vec3 = target_camera_position[i] - last_output[2][i];
-		const int sign = vec < 0 ? -1 : 1;
-        const double a = i >= 3 ? s.rotation_alpha : s.translation_alpha;
-        const double a2 = a * s.second_order_alpha;
-        const double a3 = a * s.third_order_alpha;
-        const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone;
-        const double velocity =
-                parabola(a, vec, deadzone, s.expt) +
-                parabola(a2, vec2, deadzone, s.expt) +
-                parabola(a3, vec3, deadzone, s.expt);
-        const double result = last_output[0][i] + velocity;
-        const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
-        last_output[2][i] = last_output[1][i];
-        last_output[1][i] = last_output[0][i];
-        last_output[0][i] = new_camera_position[i] = done ? target_camera_position[i] : result;
-	}
+    const double RC = 2 * s.ewma / 1000.; // seconds
+    const double alpha = dt/(dt+RC);
+    
+    for (int i = 0; i < 6; i++)
+    {
+        smoothed_input[i] = smoothed_input[i] * (1.-alpha) + input[i] * alpha;
+        
+        const double in = smoothed_input[i];
+        
+        const double vec = in - last_output[i];
+        const double vec_ = fabs(vec);
+        const double t = i >= 3 ? rot_t : trans_t;
+        const double val = f(vec_, t);
+        const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val;
+        const bool negp = vec < 0.;
+        const bool done = negp
+            ? result <= in
+            : result >= in;
+        const double ret = done ? in : result;
+        
+        last_output[i] = output[i] = ret;
+    }
 }
 
 extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()
 {
     return new FTNoIR_Filter;
 }
+
+extern "C" OPENTRACK_EXPORT Metadata* GetMetadata()
+{
+    return new FTNoIR_FilterDll;
+}