mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-19 13:18:32 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

m/space: Make sure to use valid poses in linear and angular calculations

Browse source
This commit is contained in:
Jakob Bornecrantz 2022-12-24 01:05:49 +00:00
parent f7118d7a93
commit c7fdaacd02
1 changed files with 10 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
src/xrt/auxiliary/math/m_space.cpp
View file

@ -161,9 +161,11 @@ apply_relation(const struct xrt_space_relation *a,
if (af.has_linear_velocity) { if (af.has_linear_velocity) {
nf.has_linear_velocity = true; nf.has_linear_velocity = true;
struct xrt_vec3 tmp = XRT_VEC3_ZERO; struct xrt_vec3 tmp = XRT_VEC3_ZERO;
math_quat_rotate_vec3(&b->pose.orientation, // Base rotation
&a->linear_velocity, // In base space math_quat_rotate_vec3(&base_pose.orientation, // Base rotation
&tmp); // Output &a->linear_velocity, // In base space
&tmp); // Output
linear_velocity += tmp; linear_velocity += tmp;
} }
@ -181,9 +183,9 @@ apply_relation(const struct xrt_space_relation *a,
nf.has_angular_velocity = true; nf.has_angular_velocity = true;
struct xrt_vec3 tmp = XRT_VEC3_ZERO; struct xrt_vec3 tmp = XRT_VEC3_ZERO;
math_quat_rotate_derivative(&b->pose.orientation, // Base rotation math_quat_rotate_derivative(&base_pose.orientation, // Base rotation
&a->angular_velocity, // In base space &a->angular_velocity, // In base space
&tmp); // Output &tmp); // Output
angular_velocity += tmp; angular_velocity += tmp;
} }
@ -198,8 +200,8 @@ apply_relation(const struct xrt_space_relation *a,
struct xrt_quat orientation = XRT_QUAT_IDENTITY; struct xrt_quat orientation = XRT_QUAT_IDENTITY;
struct xrt_vec3 tangental_velocity = XRT_VEC3_ZERO; struct xrt_vec3 tangental_velocity = XRT_VEC3_ZERO;
position = a->pose.position; // In the base space position = body_pose.position; // In the base space
orientation = b->pose.orientation; // Base space orientation = base_pose.orientation; // Base space
math_quat_rotate_vec3(&orientation, // Rotation math_quat_rotate_vec3(&orientation, // Rotation
&position, // Vector &position, // Vector