From 326402da2accbc65b0685b261990959628efe1f6 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Mon, 13 Jun 2022 16:59:50 -0300 Subject: [PATCH] a/math: Use angle as magnitude of change in m_filter_euro_quat Previously this was using math_quat_len which was always 1 for these unit quaternions. This commit assumes math_quat_ln works properly which is not exactly true at the moment and the returned angle will be twice as large. --- src/xrt/auxiliary/math/m_filter_one_euro.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/xrt/auxiliary/math/m_filter_one_euro.c b/src/xrt/auxiliary/math/m_filter_one_euro.c index 07d6a419c..b32f68ebd 100644 --- a/src/xrt/auxiliary/math/m_filter_one_euro.c +++ b/src/xrt/auxiliary/math/m_filter_one_euro.c @@ -282,8 +282,12 @@ m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct x f->prev_dy = exp_smooth_quat(alpha_d, dy, f->prev_dy); - double dy_mag = math_quat_len(&f->prev_dy); - double alpha = filter_one_euro_compute_alpha(&f->base, dt, dy_mag); + // The magnitud of the smoothed dy (f->prev_dy) is its rotation angle in radians + struct xrt_vec3 smooth_dy_aa; + math_quat_ln(&f->prev_dy, &smooth_dy_aa); + double smooth_dy_mag = m_vec3_len(smooth_dy_aa); + + double alpha = filter_one_euro_compute_alpha(&f->base, dt, smooth_dy_mag); /* Smooth the dy values and use them to calculate the frequency cutoff for the main filter */ f->prev_y = exp_smooth_quat(alpha, *in_y, f->prev_y);