diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp index d7bf2769e..6c588124a 100644 --- a/src/xrt/auxiliary/math/m_relation_history.cpp +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -183,51 +183,55 @@ m_relation_history_get(const struct m_relation_history *rh, } } -bool -m_relation_history_estimate_motion(struct m_relation_history *rh, - const struct xrt_space_relation *in_relation, - int64_t timestamp, - struct xrt_space_relation *out_relation) +static void +m_relation_history_estimate_motion(struct xrt_space_relation const &old_relation, + struct xrt_space_relation const &new_relation, + float dt, + struct xrt_vec3 &out_linear_velocity, + struct xrt_vec3 &out_angular_velocity, + enum xrt_space_relation_flags &out_flags) { + assert(dt != 0.0f); - int64_t last_time_ns; - struct xrt_space_relation last_relation; - if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) { - return false; - }; - - float dt = (float)time_ns_to_s(timestamp - last_time_ns); - - // Used to find out what values are valid in both the old relation and the new relation - enum xrt_space_relation_flags tmp_flags = - (enum xrt_space_relation_flags)(last_relation.relation_flags & in_relation->relation_flags); + enum xrt_space_relation_flags shared_flags = + (enum xrt_space_relation_flags)(old_relation.relation_flags & new_relation.relation_flags); - // Brevity - enum xrt_space_relation_flags &outf = out_relation->relation_flags; + // If both relations have position data, estimate linear velocity + if (shared_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { + out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + out_linear_velocity = (new_relation.pose.position - old_relation.pose.position) / dt; + } - if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); - - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + // If both relations have orientation data, estimate angular velocity + if (shared_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { + out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); - out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt; + math_quat_finite_difference(&old_relation.pose.orientation, &new_relation.pose.orientation, dt, + &out_angular_velocity); } +} - if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); +bool +m_relation_history_push_with_motion_estimation(struct m_relation_history *rh, + struct xrt_space_relation const *in_relation, + int64_t timestamp) +{ + assert((in_relation->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) == 0); + assert((in_relation->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) == 0); - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + struct xrt_space_relation final_relation = *in_relation; - math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt, - &out_relation->angular_velocity); - } + int64_t last_time_ns; + struct xrt_space_relation last_relation; + if (m_relation_history_get_latest(rh, &last_time_ns, &last_relation) && timestamp > last_time_ns) { + float dt = (float)time_ns_to_s(timestamp - last_time_ns); - out_relation->pose = in_relation->pose; + m_relation_history_estimate_motion(last_relation, *in_relation, dt, final_relation.linear_velocity, + final_relation.angular_velocity, final_relation.relation_flags); + } - return true; + return m_relation_history_push(rh, &final_relation, timestamp); } bool diff --git a/src/xrt/auxiliary/math/m_relation_history.h b/src/xrt/auxiliary/math/m_relation_history.h index 2faee0cb0..11c7d5ef3 100644 --- a/src/xrt/auxiliary/math/m_relation_history.h +++ b/src/xrt/auxiliary/math/m_relation_history.h @@ -63,33 +63,31 @@ bool m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation const *in_relation, int64_t timestamp); /*! - * Interpolates or extrapolates to the desired timestamp. + * Pushes a new pose to the history, estimating linear and angular velocity based on the previous entry. + * + * If the history is full, it will also pop a pose out of the other side of the buffer. * - * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often as - * you want. + * @return false if the timestamp is earlier than the most recent timestamp already recorded * * @public @memberof m_relation_history */ -enum m_relation_history_result -m_relation_history_get(const struct m_relation_history *rh, - int64_t at_timestamp_ns, - struct xrt_space_relation *out_relation); +bool +m_relation_history_push_with_motion_estimation(struct m_relation_history *rh, + struct xrt_space_relation const *in_relation, + int64_t timestamp); /*! - * Estimates the movement (velocity and angular velocity) of a new relation based on - * the latest relation found in the buffer (as returned by m_relation_history_get_latest). + * Interpolates or extrapolates to the desired timestamp. * - * Read-only on m_relation_history and in_relation. - * Copies in_relation->pose to out_relation->pose, and writes new flags and linear/angular velocities to - * out_relation->pose. OK to alias in_relation and out_relation. + * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often + * as you want. * * @public @memberof m_relation_history */ -bool -m_relation_history_estimate_motion(struct m_relation_history *rh, - const struct xrt_space_relation *in_relation, - int64_t timestamp, - struct xrt_space_relation *out_relation); +enum m_relation_history_result +m_relation_history_get(const struct m_relation_history *rh, + int64_t at_timestamp_ns, + struct xrt_space_relation *out_relation); /*! * Get the latest report in the buffer, if any. diff --git a/src/xrt/drivers/hydra/hydra_driver.c b/src/xrt/drivers/hydra/hydra_driver.c index c30afd9a4..62e3cf703 100644 --- a/src/xrt/drivers/hydra/hydra_driver.c +++ b/src/xrt/drivers/hydra/hydra_driver.c @@ -345,9 +345,7 @@ hydra_device_parse_controller(struct hydra_device *hd, uint8_t *buf, int64_t now (XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) | (XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT); - m_relation_history_estimate_motion(state->relation_history, &space_relation, now, &space_relation); - - m_relation_history_push(state->relation_history, &space_relation, now); + m_relation_history_push_with_motion_estimation(state->relation_history, &space_relation, now); state->buttons = hydra_read_uint8(&buf); diff --git a/src/xrt/tracking/hand/t_hand_tracking_async.c b/src/xrt/tracking/hand/t_hand_tracking_async.c index 0102a7589..dae7a0f6c 100644 --- a/src/xrt/tracking/hand/t_hand_tracking_async.c +++ b/src/xrt/tracking/hand/t_hand_tracking_async.c @@ -137,16 +137,10 @@ ht_async_mainloop(void *ptr) struct xrt_space_relation wrist_rel = hta->working.hands[i].values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation; - m_relation_history_estimate_motion( // - hta->present.relation_hist[i], // - &wrist_rel, // - hta->working.timestamp, // - &wrist_rel); // - - m_relation_history_push( // - hta->present.relation_hist[i], // - &wrist_rel, // - hta->working.timestamp); // + m_relation_history_push_with_motion_estimation( // + hta->present.relation_hist[i], // + &wrist_rel, // + hta->working.timestamp); // } hta->hand_tracking_work_active = false;