From 74248c79e4ffb9b29a528c184f8ead4613afd3a2 Mon Sep 17 00:00:00 2001 From: MatrixFurry Date: Fri, 21 Nov 2025 18:03:49 -0600 Subject: [PATCH] xrt: enable acceleration components for xrt_space_relation Signed-off-by: MatrixFurry --- src/xrt/auxiliary/math/m_predict.c | 4 +--- src/xrt/auxiliary/math/m_space.cpp | 12 ++++++++++++ src/xrt/auxiliary/math/m_space.h | 6 ++++-- src/xrt/include/xrt/xrt_defines.h | 22 ++++++++++++++-------- 4 files changed, 31 insertions(+), 13 deletions(-) diff --git a/src/xrt/auxiliary/math/m_predict.c b/src/xrt/auxiliary/math/m_predict.c index 3a84179d5..cf3c093db 100644 --- a/src/xrt/auxiliary/math/m_predict.c +++ b/src/xrt/auxiliary/math/m_predict.c @@ -28,6 +28,7 @@ do_orientation(const struct xrt_space_relation *rel, struct xrt_vec3 accum = {0}; bool valid_orientation = (flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; bool valid_angular_velocity = (flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0; + bool valid_angular_acceleration = (flags & XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) != 0; if (valid_angular_velocity) { // angular velocity needs to be in body space for prediction @@ -43,14 +44,11 @@ do_orientation(const struct xrt_space_relation *rel, accum.z += ang_vel_body_space.z; } - // We don't want the angular acceleration, it's way too noisy. -#if 0 if (valid_angular_acceleration) { accum.x += delta_s / 2 * rel->angular_acceleration.x; accum.y += delta_s / 2 * rel->angular_acceleration.y; accum.z += delta_s / 2 * rel->angular_acceleration.z; } -#endif if (valid_orientation) { math_quat_integrate_velocity(&rel->pose.orientation, // Old orientation diff --git a/src/xrt/auxiliary/math/m_space.cpp b/src/xrt/auxiliary/math/m_space.cpp index 8a2be7840..a5d06ceb5 100644 --- a/src/xrt/auxiliary/math/m_space.cpp +++ b/src/xrt/auxiliary/math/m_space.cpp @@ -42,10 +42,22 @@ dump_relation(const struct xrt_space_relation *r) fprintf(stderr, " LV{%f %f %f}", r->linear_velocity.x, r->linear_velocity.y, r->linear_velocity.z); } + if (r->relation_flags & + XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT) { + fprintf(stderr, " LA{%f %f %f}", r->linear_acceleration.x, + r->linear_acceleration.y, r->linear_acceleration.z); + } + if (r->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) { fprintf(stderr, " AV{%f %f %f}", r->angular_velocity.x, r->angular_velocity.y, r->angular_velocity.z); } + if (r->relation_flags & + XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) { + fprintf(stderr, " AA{%f %f %f}", r->angular_acceleration.x, + r->angular_acceleration.y, r->angular_acceleration.z); + } + fprintf(stderr, "\n"); } diff --git a/src/xrt/auxiliary/math/m_space.h b/src/xrt/auxiliary/math/m_space.h index 3af9a58bc..8bfb40853 100644 --- a/src/xrt/auxiliary/math/m_space.h +++ b/src/xrt/auxiliary/math/m_space.h @@ -66,8 +66,10 @@ m_space_relation_from_pose(const struct xrt_pose *pose, bool set_tracked, struct XRT_SPACE_RELATION_POSITION_VALID_BIT); struct xrt_space_relation relation = { - flags, - *pose, + flags, + *pose, + XRT_VEC3_ZERO, + XRT_VEC3_ZERO, XRT_VEC3_ZERO, XRT_VEC3_ZERO, }; diff --git a/src/xrt/include/xrt/xrt_defines.h b/src/xrt/include/xrt/xrt_defines.h index 8fb7038d1..ab2040406 100644 --- a/src/xrt/include/xrt/xrt_defines.h +++ b/src/xrt/include/xrt/xrt_defines.h @@ -645,14 +645,18 @@ enum xrt_space_relation_flags XRT_SPACE_RELATION_POSITION_VALID_BIT = (1u << 1u), XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT = (1u << 2u), XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT = (1u << 3u), - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT = (1u << 4u), - XRT_SPACE_RELATION_POSITION_TRACKED_BIT = (1u << 5u), + XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT = (1u << 4u), + XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT = (1u << 5u), + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT = (1u << 6u), + XRT_SPACE_RELATION_POSITION_TRACKED_BIT = (1u << 7u), // clang-format on - XRT_SPACE_RELATION_BITMASK_ALL = (uint32_t)XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | // - (uint32_t)XRT_SPACE_RELATION_POSITION_VALID_BIT | // - (uint32_t)XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | // - (uint32_t)XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | // - (uint32_t)XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | // + XRT_SPACE_RELATION_BITMASK_ALL = (uint32_t)XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_POSITION_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT | // + (uint32_t)XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | // (uint32_t)XRT_SPACE_RELATION_POSITION_TRACKED_BIT, XRT_SPACE_RELATION_BITMASK_NONE = 0, }; @@ -672,6 +676,8 @@ struct xrt_space_relation struct xrt_pose pose; struct xrt_vec3 linear_velocity; struct xrt_vec3 angular_velocity; + struct xrt_vec3 linear_acceleration; + struct xrt_vec3 angular_acceleration; }; /*! @@ -684,7 +690,7 @@ struct xrt_space_relation * @relates xrt_space_relation */ // clang-format off -#define XRT_SPACE_RELATION_ZERO {XRT_SPACE_RELATION_BITMASK_NONE, XRT_POSE_IDENTITY, XRT_VEC3_ZERO, XRT_VEC3_ZERO} +#define XRT_SPACE_RELATION_ZERO {XRT_SPACE_RELATION_BITMASK_NONE, XRT_POSE_IDENTITY, XRT_VEC3_ZERO, XRT_VEC3_ZERO, XRT_VEC3_ZERO, XRT_VEC3_ZERO} // clang-format on /*! -- 2.51.2