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

m/relation_history: Add motion estimation

Browse source
This commit is contained in:
Moses Turner 2022-06-18 21:49:56 +01:00
parent 329b5c05bc
commit 543a436c99
2 changed files with 63 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

47
src/xrt/auxiliary/math/m_relation_history.cpp
View file

@ -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,

16
src/xrt/auxiliary/math/m_relation_history.h
View file

@ -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.
*