From 3f7f65c1bf67d0e1dc560ee6136ad9c6e82e73bb Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 1 Dec 2013 13:14:57 +0100 Subject: fix sign issues now that order > 1 Signed-off-by: Stanislaw Halik --- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'ftnoir_filter_accela/ftnoir_filter_accela.cpp') diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 1f9b493f..e2fd7cc0 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -68,8 +68,9 @@ void FTNoIR_Filter::receiveSettings(double rot, double trans, double zoom_fac, d static inline double parabola(const double a, const double x, const double dz, const double expt) { + const double sign = x > 0 ? 1 : -1; const double a1 = 1./a; - return a1 * pow(std::max(x - dz, 1e-3), expt); + return a1 * pow(std::max(fabs(x) - dz, 1e-3), expt) * sign; } void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, @@ -95,19 +96,18 @@ void FTNoIR_Filter::FilterHeadPoseData(const double* target_camera_position, 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 int sign = vec < 0 ? -1 : 1; - const double x = fabs(vec); const double a = i >= 3 ? rotation_alpha : translation_alpha; const double a2 = a * second_order_alpha; const double a3 = a * third_order_alpha; - const double x2 = fabs(target_camera_position[i] - current_camera_position_2[i]); - const double x3 = fabs(target_camera_position[i] - current_camera_position_3[i]); const double reduction = 1. / std::max(1., 1. + zoom_factor * -last_post_filter_values[TZ] / 1000); const double velocity = - parabola(a, x * scaling[i], deadzone, expt) * reduction + - parabola(a2, x2 * scaling[i], deadzone, expt) * reduction + - parabola(a3, x3 * scaling[i], deadzone, expt) * reduction; - const double result = current_camera_position[i] + velocity * sign; + 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 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]; -- cgit v1.2.3