The open source OpenXR runtime
m/relation_history: Add motion estimation
+63
+47
src/xrt/auxiliary/math/m_relation_history.cpp
+47
src/xrt/auxiliary/math/m_relation_history.cpp
···
175
175
}
176
176
177
177
bool
178
+
m_relation_history_estimate_motion(struct m_relation_history *rh,
179
+
const struct xrt_space_relation *in_relation,
180
+
uint64_t timestamp,
181
+
struct xrt_space_relation *out_relation)
182
+
{
183
+
184
+
uint64_t last_time_ns;
185
+
struct xrt_space_relation last_relation;
186
+
if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
187
+
return false;
188
+
};
189
+
190
+
float dt = time_ns_to_s(timestamp - last_time_ns);
191
+
192
+
// Used to find out what values are valid in both the old relation and the new relation
193
+
enum xrt_space_relation_flags tmp_flags =
194
+
(enum xrt_space_relation_flags)(last_relation.relation_flags & in_relation->relation_flags);
195
+
196
+
// Brevity
197
+
enum xrt_space_relation_flags &outf = out_relation->relation_flags;
198
+
199
+
200
+
if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
201
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
202
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
203
+
204
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
205
+
206
+
out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
207
+
}
208
+
209
+
if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
210
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
211
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
212
+
213
+
outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
214
+
215
+
math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
216
+
&out_relation->angular_velocity);
217
+
}
218
+
219
+
out_relation->pose = in_relation->pose;
220
+
221
+
return true;
222
+
}
223
+
224
+
bool
178
225
m_relation_history_get_latest(struct m_relation_history *rh,
179
226
uint64_t *out_time_ns,
180
227
struct xrt_space_relation *out_relation)
+16
src/xrt/auxiliary/math/m_relation_history.h
+16
src/xrt/auxiliary/math/m_relation_history.h
···
72
72
struct xrt_space_relation *out_relation);
73
73
74
74
/*!
75
+
* Estimates the movement (velocity and angular velocity) of a new relation based on
76
+
* the latest relation found in the buffer (as returned by m_relation_history_get_latest).
77
+
*
78
+
* Read-only on m_relation_history and in_relation.
79
+
* Copies in_relation->pose to out_relation->pose, and writes new flags and linear/angular velocities to
80
+
* out_relation->pose. OK to alias in_relation and out_relation.
81
+
*
82
+
* @public @memberof m_relation_history
83
+
*/
84
+
bool
85
+
m_relation_history_estimate_motion(struct m_relation_history *rh,
86
+
const struct xrt_space_relation *in_relation,
87
+
uint64_t timestamp,
88
+
struct xrt_space_relation *out_relation);
89
+
90
+
/*!
75
91
* Get the latest report in the buffer, if any.
76
92
*
77
93
* @param rh self