diff options
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.cpp | 62 | ||||
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.h | 8 |
2 files changed, 56 insertions, 14 deletions
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index e2fd7cc0..fd47f18d 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -73,6 +73,16 @@ static inline double parabola(const double a, const double x, const double dz, c return a1 * pow(std::max<double>(fabs(x) - dz, 1e-3), expt) * sign;
}
+template<typename T>
+static inline T clamp(const T min, const T max, const T value)
+{
+ if (value < min)
+ return min;
+ if (value > max)
+ return max;
+ return value;
+}
+
void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position,
double *new_camera_position,
const double* last_post_filter_values)
@@ -82,22 +92,51 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, for (int i = 0; i < 6; i++)
{
new_camera_position[i] = target_camera_position[i];
- current_camera_position[i] = target_camera_position[i];
- current_camera_position_2[i] = target_camera_position[i];
- current_camera_position_3[i] = target_camera_position[i];
+ last_input[i] = target_camera_position[i];
+ for (int j = 0; j < 3; j++)
+ last_output[j][i] = target_camera_position[i];
}
- first_run = false;
+ timer.start();
+ frame_delta = 1;
+ first_run = false;
return;
}
+
+ bool new_frame = false;
+
+ for (int i = 0; i < 6; i++)
+ {
+ if (target_camera_position[i] != last_input[i])
+ {
+ new_frame = true;
+ break;
+ }
+ }
+
+ if (new_frame)
+ {
+ for (int i = 0; i < 6; i++)
+ last_input[i] = target_camera_position[i];
+ frame_delta = timer.isValid() ? timer.elapsed() : 1;
+ timer.start();
+ } else {
+ auto d = timer.elapsed();
+ double c = clamp(0.0, 1.0, d / (double) frame_delta);
+ QMutexLocker foo(&mutex);
+ for (int i = 0; i < 6; i++)
+ new_camera_position[i] =
+ last_output[1][i] + (last_output[0][i] - last_output[1][i]) * c;
+ return;
+ }
QMutexLocker foo(&mutex);
for (int i=0;i<6;i++)
{
- const double vec = target_camera_position[i] - current_camera_position[i];
- const double vec2 = target_camera_position[i] - current_camera_position_2[i];
- const double vec3 = target_camera_position[i] - current_camera_position_3[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 ? rotation_alpha : translation_alpha;
const double a2 = a * second_order_alpha;
@@ -107,11 +146,12 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, parabola(a, vec * scaling[i], deadzone, expt) * reduction +
parabola(a2, vec2 * scaling[i], deadzone, expt) * reduction +
parabola(a3, vec3 * scaling[i], deadzone, expt) * reduction;
- const double result = current_camera_position[i] + velocity;
+ const double result = last_output[0][i] + velocity;
const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
- current_camera_position_3[i] = current_camera_position_2[i];
- current_camera_position_2[i] = current_camera_position[i];
- new_camera_position[i] = current_camera_position[i] = done ? target_camera_position[i] : result;
+ new_camera_position[i] = done ? target_camera_position[i] : result;
+ last_output[2][i] = last_output[1][i];
+ last_output[1][i] = last_output[0][i];
+ last_output[0][i] = new_camera_position[i];
}
}
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h index f5c0b8bc..c92e28df 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.h +++ b/ftnoir_filter_accela/ftnoir_filter_accela.h @@ -30,6 +30,7 @@ #include "ui_ftnoir_accela_filtercontrols.h"
#include "facetracknoir/global-settings.h"
#include <QMutex>
+#include <QElapsedTimer>
#define ACCELA_SMOOTHING_ROTATION 60.0
#define ACCELA_SMOOTHING_TRANSLATION 40.0
@@ -56,12 +57,13 @@ private: bool first_run;
double rotation_alpha, translation_alpha, zoom_factor;
double second_order_alpha, third_order_alpha;
- double current_camera_position[6];
- double current_camera_position_2[6];
- double current_camera_position_3[6];
double scaling[6];
double deadzone;
double expt;
+ double last_input[6];
+ double last_output[3][6];
+ QElapsedTimer timer;
+ qint64 frame_delta;
};
//*******************************************************************************************************
|