diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp index 7d5892bf1..e10737fb5 100644 --- a/src/xrt/auxiliary/math/m_relation_history.cpp +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -174,6 +174,53 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, } } +bool +m_relation_history_estimate_motion(struct m_relation_history *rh, + const struct xrt_space_relation *in_relation, + uint64_t timestamp, + struct xrt_space_relation *out_relation) +{ + + uint64_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 = 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); + + // Brevity + enum xrt_space_relation_flags &outf = out_relation->relation_flags; + + + 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); + + out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt; + } + + 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); + + outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + + math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt, + &out_relation->angular_velocity); + } + + out_relation->pose = in_relation->pose; + + return true; +} + bool m_relation_history_get_latest(struct m_relation_history *rh, uint64_t *out_time_ns, diff --git a/src/xrt/auxiliary/math/m_relation_history.h b/src/xrt/auxiliary/math/m_relation_history.h index 6eabcb5a8..3e6e84def 100644 --- a/src/xrt/auxiliary/math/m_relation_history.h +++ b/src/xrt/auxiliary/math/m_relation_history.h @@ -71,6 +71,22 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation); +/*! + * 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). + * + * 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. + * + * @public @memberof m_relation_history + */ +bool +m_relation_history_estimate_motion(struct m_relation_history *rh, + const struct xrt_space_relation *in_relation, + uint64_t timestamp, + struct xrt_space_relation *out_relation); + /*! * Get the latest report in the buffer, if any. *