mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Add m_filter_euro_quat

Browse source
This commit is contained in:
Mateo de Mayo 2021-12-30 17:33:40 -03:00
parent 06597a0655
commit 8820a3e99c
2 changed files with 58 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

41
src/xrt/auxiliary/math/m_filter_one_euro.c
View file

@ -53,6 +53,14 @@ exp_smooth_vec3(double alpha, struct xrt_vec3 y, struct xrt_vec3 prev_y)
return m_vec3_add(scaled_prev, scaled_new);
}
static inline struct xrt_quat
exp_smooth_quat(double alpha, struct xrt_quat y, struct xrt_quat prev_y)
{
struct xrt_quat result;
math_quat_slerp(&prev_y, &y, alpha, &result);
return result;
}
static void
filter_one_euro_init(struct m_filter_one_euro_base *f, double fc_min, double beta, double fc_min_d)
{
@ -239,3 +247,36 @@ m_filter_euro_vec3_run(struct m_filter_euro_vec3 *f, uint64_t ts, const struct x
f->prev_y = exp_smooth_vec3(alpha, *in_y, f->prev_y);
*out_y = f->prev_y;
}
void
m_filter_euro_quat_init(struct m_filter_euro_quat *f, double fc_min, double beta, double fc_min_d)
{
filter_one_euro_init(&f->base, fc_min, beta, fc_min_d);
}
void
m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct xrt_quat *in_y, struct xrt_quat *out_y)
{
if (filter_one_euro_handle_first_sample(&f->base, ts, true)) {
/* First sample - no filtering yet */
f->prev_dy = (struct xrt_quat)XRT_QUAT_IDENTITY;
f->prev_y = *in_y;
*out_y = *in_y;
return;
}
double dt = 0;
double alpha_d = filter_one_euro_compute_alpha_d(&f->base, &dt, ts, true);
struct xrt_quat dy;
math_quat_unrotate(&f->prev_y, in_y, &dy);
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);
/* 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);
*out_y = f->prev_y;
}

17
src/xrt/auxiliary/math/m_filter_one_euro.h
View file

@ -89,6 +89,18 @@ struct m_filter_euro_vec3
struct xrt_vec3 prev_dy;
};
struct m_filter_euro_quat
{
/** Base/common data */
struct m_filter_one_euro_base base;
/** The previous sample */
struct xrt_quat prev_y;
/** The previous sample derivative */
struct xrt_quat prev_dy;
};
void
m_filter_euro_f32_init(struct m_filter_euro_f32 *f, double fc_min, double beta, double fc_min_d);
void
@ -109,6 +121,11 @@ m_filter_euro_vec3_init(struct m_filter_euro_vec3 *f, double fc_min, double beta
void
m_filter_euro_vec3_run(struct m_filter_euro_vec3 *f, uint64_t ts, const struct xrt_vec3 *in_y, struct xrt_vec3 *out_y);
void
m_filter_euro_quat_init(struct m_filter_euro_quat *f, double fc_min, double beta, double fc_min_d);
void
m_filter_euro_quat_run(struct m_filter_euro_quat *f, uint64_t ts, const struct xrt_quat *in_y, struct xrt_quat *out_y);
#ifdef __cplusplus
}
#endif