Monado OpenXR Runtime
Loading...
Searching...
No Matches
m_relation_history.hpp
Go to the documentation of this file.
1// Copyright 2021, Collabora, Ltd.
2// Copyright 2026, Beyley Cardellio
3// SPDX-License-Identifier: BSL-1.0
4/*!
5 * @file
6 * @brief Small utility for keeping track of the history of an xrt_space_relation, ie. for knowing where a HMD or
7 * controller was in the past, and predicting where it will be.
8 * @author Moshi Turner <moshiturner@protonmail.com>
9 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
10 * @author Korcan Hussein <korcan.hussein@collabora.com>
11 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
12 * @ingroup aux_math
13 */
14
15#pragma once
16
17#include "xrt/xrt_defines.h"
18
19#include "os/os_threading.h"
20
22#include "util/u_logging.h"
23#include "util/u_time.h"
24
25#include "math/m_api.h"
26#include "math/m_vec3.h"
27#include "math/m_predict.h"
28
29#include "m_relation_history.h"
30
31#include <algorithm>
32#include <mutex>
33#include <optional>
34#include <assert.h>
35#include <stdint.h>
36
37
38namespace xrt::auxiliary::math {
39
41{
42 xrt_space_relation relation XRT_SPACE_RELATION_ZERO;
43 int64_t timestamp{0};
44
45 //! Predicts the relation history entry to the specified delta, returning the new one.
47 predict(time_duration_ns delta_ns) const
48 {
49 double delta_s = time_ns_to_s(delta_ns);
50
51 RelationHistoryEntry final = *this;
52 final.timestamp += delta_ns;
53 m_predict_relation(&this->relation, delta_s, &final.relation);
54
55 return final;
56 }
57
58 //! Interpolates between the two relation history entries, returning a new one.
61 {
62 RelationHistoryEntry final = *this;
63 final.timestamp = to_ns;
64
65 int64_t diff_before = to_ns - this->timestamp;
66 int64_t diff_after = successor.timestamp - to_ns;
67
68 float amount_to_lerp = (float)diff_before / (float)(diff_before + diff_after);
69
70 auto &result_relation = final.relation;
71
72 // Copy intersection of relation flags
73 result_relation.relation_flags =
74 (enum xrt_space_relation_flags)(this->relation.relation_flags & successor.relation.relation_flags);
75
76 // First-order implementation - lerp between the before and after
77 if (result_relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
78 result_relation.pose.position =
79 m_vec3_lerp(this->relation.pose.position, successor.relation.pose.position, amount_to_lerp);
80 }
81 if (result_relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
82
83 math_quat_slerp(&this->relation.pose.orientation, &successor.relation.pose.orientation,
84 amount_to_lerp, &result_relation.pose.orientation);
85 }
86
87 //! @todo Does interpolating the velocities make any sense?
88 if (result_relation.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
89 result_relation.angular_velocity = m_vec3_lerp(
90 this->relation.angular_velocity, successor.relation.angular_velocity, amount_to_lerp);
91 }
92 if (result_relation.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
93 result_relation.linear_velocity = m_vec3_lerp(
94 this->relation.linear_velocity, successor.relation.linear_velocity, amount_to_lerp);
95 }
96
97 return final;
98 }
99
100 /*!
101 * Estimates motion from this relation history to the end entry, modifying the end entry with the new
102 * velocities.
103 */
104 void
106 {
107 const auto &old_relation = this->relation;
108 const auto &new_relation = in_out_end.relation;
109
110 xrt_vec3 &out_linear_velocity = in_out_end.relation.linear_velocity;
111 xrt_vec3 &out_angular_velocity = in_out_end.relation.angular_velocity;
112 enum xrt_space_relation_flags &out_flags = in_out_end.relation.relation_flags;
113
114 double dt = time_ns_to_s(in_out_end.timestamp - this->timestamp);
115
116 assert(dt != 0.0f);
117
118 enum xrt_space_relation_flags shared_flags =
119 (enum xrt_space_relation_flags)(old_relation.relation_flags & new_relation.relation_flags);
120
121 // Don't do anything if the new relation already has linear velocity
122 if (new_relation.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
123 out_flags =
124 (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
125 out_linear_velocity = new_relation.linear_velocity;
126 }
127 // If both relations have position data, estimate linear velocity
128 else if (shared_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
129 out_flags =
130 (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
131
132 out_linear_velocity = (new_relation.pose.position - old_relation.pose.position) / dt;
133 }
134
135 // Don't do anything if the new relation already has angular velocity
136 if (new_relation.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
137 out_flags =
138 (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
139 out_angular_velocity = new_relation.angular_velocity;
140 }
141 // If both relations have orientation data, estimate angular velocity
142 else if (shared_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
143 out_flags =
144 (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
145
146 math_quat_finite_difference(&old_relation.pose.orientation, &new_relation.pose.orientation, dt,
147 &out_angular_velocity);
148 }
149 }
150};
151
152/*!
153 * The base type for a relation history.
154 *
155 * T must be a type compatible with @ref RelationHistoryEntry (fields & functions).
156 */
157template <typename T> struct RelationHistoryBase
158{
159public: // Constants
160 static constexpr size_t kBufLen = 4096;
161
162public: // Fields
164 mutable os::Mutex mutex;
165
166public: // Methods (thread-unsafe)
167 /*!
168 * Pushes a new entry (if newest) or updates an old one if allowed.
169 *
170 * Call with @ref mutex locked.
171 */
172 bool
173 pushOrUpdateLocked(const T &rhe, bool allow_update)
174 {
175 // if we aren't empty, we can compare against the latest timestamp.
176 if (this->impl.empty() || rhe.timestamp > this->impl.back().timestamp) {
177 /*
178 * Everything explodes if the timestamps in relation_history aren't monotonically increasing. If
179 * we get a timestamp that's before the most recent timestamp in the buffer, don't put it
180 * in the history.
181 */
182 this->impl.push_back(rhe);
183 return true;
184 }
185
186 // Don't do any updating if it's not enabled
187 if (!allow_update) {
188 return false;
189 }
190
191 for (auto it = this->impl.end(); it != this->impl.begin();) {
192 --it;
193 auto &item = *it;
194
195 // Poses are monotonic, so if we're newer than this item than it will never reach
196 if (rhe.timestamp > item.timestamp) {
197 return false;
198 }
199
200 // If that timestamp is in here, update the pose
201 if (item.timestamp == rhe.timestamp) {
202 item = rhe;
203 return true;
204 }
205 }
206
207 return false;
208 }
209
210 /*!
211 * Gets the latest entry, or nullopt if none exists.
212 *
213 * Call with @ref mutex locked.
214 */
215 std::optional<T>
217 {
218 if (this->impl.empty()) {
219 return std::nullopt;
220 }
221
222 return this->impl.back();
223 }
224
225 /*!
226 * Gets the device's relation at the specified timestamp. May predict or interpolate to get the closest value.
227 *
228 * Call with @ref mutex held.
229 */
231 getLocked(timepoint_ns timestamp_ns, T &out_entry) const
232 {
233 if (this->impl.empty() || timestamp_ns == 0) {
234 // Do nothing. You push nothing to the buffer you get nothing from the buffer.
235 out_entry = {};
237 }
238
239 const auto b = this->impl.begin();
240 const auto e = this->impl.end();
241
242 // Find the first element *not less than* our value. the lambda we pass is the comparison
243 // function, to compare against timestamps.
244 const auto it = std::lower_bound(
245 b, e, timestamp_ns, [](const T &rhe, int64_t timestamp) { return rhe.timestamp < timestamp; });
246
247 if (it == e) {
248 // lower bound is at the end:
249 // The desired timestamp is after what our buffer contains.
250 // (pose-prediction)
251 // Output flags match the most recent buffer entry.
252 time_duration_ns delta_ns = timestamp_ns - this->impl.back().timestamp;
253 double delta_s = time_ns_to_s(delta_ns);
254
255 U_LOG_T("Extrapolating %f s past the back of the buffer!", delta_s);
256
257 out_entry = this->impl.back().predict(delta_ns);
259 }
260 if (timestamp_ns == it->timestamp) {
261 // exact match:
262 // Flags copied directly along with everything else.
263 U_LOG_T("Exact match in the buffer!");
264 out_entry = *it;
266 }
267 if (it == b) {
268 // lower bound is at the beginning (and it's not an exact match):
269 // The desired timestamp is before what our buffer contains.
270 // (an edge case where somebody asks for a really old pose and we do our best)
271 // Output flags are the same as the input flags for the history entry we use
272 time_duration_ns delta_ns = timestamp_ns - this->impl.front().timestamp;
273 double delta_s = time_ns_to_s(delta_ns);
274 U_LOG_T("Extrapolating %f s before the front of the buffer!", delta_s);
275 out_entry = this->impl.front().predict(delta_ns);
276
278 }
279 U_LOG_T("Interpolating within buffer!");
280
281 // We precede *it and follow *(it - 1) (which we know exists because we already handled
282 // the it = begin() case)
283 const auto &predecessor = *(it - 1);
284 const auto &successor = *it;
285
286 // Do the thing.
287 out_entry = predecessor.interpolate(timestamp_ns, successor);
289 }
290
291 /*!
292 * Gets an exact entry, if it exists.
293 *
294 * Call with @ref mutex held.
295 */
296 std::optional<T>
297 getExactLocked(timepoint_ns timestamp_ns) const
298 {
299 if (this->impl.empty()) {
300 return std::nullopt;
301 }
302
303 const auto b = this->impl.begin();
304 const auto e = this->impl.end();
305
306 const auto it = std::lower_bound(
307 b, e, timestamp_ns, [](const T &rhe, int64_t timestamp) { return rhe.timestamp < timestamp; });
308
309 if (it == e || it->timestamp != timestamp_ns) {
310 return std::nullopt;
311 }
312
313 return *it;
314 }
315
316 /*!
317 * Gets the stored entry nearest to the given timestamp, if one exists within @p max_slop_ns of it.
318 *
319 * Never interpolates or extrapolates, the returned entry is exactly as stored (check its timestamp to see
320 * how far off it is). If two entries are equally close, the older one is returned.
321 *
322 * Call with @ref mutex held.
323 */
324 std::optional<T>
325 getNearestLocked(timepoint_ns timestamp_ns, time_duration_ns max_slop_ns) const
326 {
327 if (this->impl.empty() || max_slop_ns < 0) {
328 return std::nullopt;
329 }
330
331 const auto b = this->impl.begin();
332 const auto e = this->impl.end();
333
334 // First element not less than our timestamp, the nearest is either it or its predecessor.
335 const auto it = std::lower_bound(
336 b, e, timestamp_ns, [](const T &rhe, int64_t timestamp) { return rhe.timestamp < timestamp; });
337
338 std::optional<T> best = std::nullopt;
339 time_duration_ns best_diff = max_slop_ns;
340
341 if (it != b) {
342 const auto &before = *(it - 1);
343 time_duration_ns diff = timestamp_ns - before.timestamp;
344 if (diff <= best_diff) {
345 best = before;
346 best_diff = diff;
347 }
348 }
349
350 if (it != e) {
351 const auto &after = *it;
352 time_duration_ns diff = after.timestamp - timestamp_ns;
353 // Strictly less, so ties go to the older entry.
354 if (diff < best_diff || (!best.has_value() && diff <= best_diff)) {
355 best = after;
356 }
357 }
358
359 return best;
360 }
361
362public: // Methods (thread-safe)
363 /*!
364 * @copydoc pushOrUpdateLocked
365 *
366 * Thread safe, holds @ref mutex.
367 */
368 bool
369 pushOrUpdate(const T &entry, bool allow_update)
370 {
371 std::unique_lock<os::Mutex> lock(this->mutex);
372 return this->pushOrUpdateLocked(entry, allow_update);
373 }
374
375 /*!
376 * @copydoc getLatestLocked
377 *
378 * Thread safe, holds @ref mutex.
379 */
380 std::optional<T>
381 getLatest() const
382 {
383 std::unique_lock<os::Mutex> lock(this->mutex);
384 return this->getLatestLocked();
385 }
386
387 /*!
388 * Pushes a new entry, doing motion estimation if required.
389 *
390 * Thread safe, holds @ref mutex.
391 */
392 bool
393 pushWithMotionEstimation(const T &new_entry, bool allow_update)
394 {
395 std::unique_lock<os::Mutex> lock(this->mutex);
396 const auto begin_entry = this->getLatestLocked();
397
398 T end_entry = new_entry;
399 if (begin_entry.has_value() && end_entry.timestamp > begin_entry->timestamp) {
400 begin_entry->estimateMotion(end_entry);
401 }
402
403 return this->pushOrUpdateLocked(end_entry, allow_update);
404 }
405
406 /*!
407 * Gets the amount of entries contained within the history.
408 *
409 * Thread safe, holds @ref mutex.
410 */
411 uint32_t
412 size() const
413 {
414 std::unique_lock<os::Mutex> lock(this->mutex);
415 return static_cast<uint32_t>(this->impl.size());
416 }
417
418 /*!
419 * Clears the relation history.
420 *
421 * Thread safe, holds @ref mutex.
422 */
423 void
425 {
426 std::unique_lock<os::Mutex> lock(this->mutex);
427 this->impl.clear();
428 }
429
430 /*!
431 * @copydoc getLocked
432 *
433 * Thread safe, holds @ref mutex.
434 */
436 get(timepoint_ns timestamp_ns, T &out_entry) const
437 {
438 std::unique_lock<os::Mutex> lock(this->mutex);
439 return this->getLocked(timestamp_ns, out_entry);
440 }
441
442 /*!
443 * @copydoc getExactLocked
444 *
445 * Thread safe, holds @ref mutex.
446 */
447 std::optional<T>
448 getExact(timepoint_ns timestamp_ns) const
449 {
450 std::unique_lock<os::Mutex> lock(this->mutex);
451 return this->getExactLocked(timestamp_ns);
452 }
453
454 /*!
455 * @copydoc getNearestLocked
456 *
457 * Thread safe, holds @ref mutex.
458 */
459 std::optional<T>
460 getNearest(timepoint_ns timestamp_ns, time_duration_ns max_slop_ns) const
461 {
462 std::unique_lock<os::Mutex> lock(this->mutex);
463 return this->getNearestLocked(timestamp_ns, max_slop_ns);
464 }
465};
466
467using RelationHistory = RelationHistoryBase<RelationHistoryEntry>;
468
469}; // namespace xrt::auxiliary::math
Stores some number of values in a ring buffer, overwriting the earliest-pushed-remaining element if o...
Definition u_template_historybuf.hpp:38
bool empty() const noexcept
Is the buffer empty?
Definition u_template_historybuf.hpp:206
const_iterator end() const noexcept
Get a "past the end" (past the newest) const iterator.
Definition u_template_historybuf_const_iterator.inl:251
T & back()
Gets a reference to the back (newest) element in the buffer.
Definition u_template_historybuf.hpp:291
size_t size() const noexcept
How many elements are in the buffer?
Definition u_template_historybuf.hpp:213
const_iterator begin() const noexcept
Get a const iterator for the oldest element.
Definition u_template_historybuf_const_iterator.inl:244
void push_back(const T &element)
Put something at the back, overwriting whatever was at the front if necessary.
Definition u_template_historybuf.hpp:220
T & front()
Gets a reference to the front (oldest) element in the buffer.
Definition u_template_historybuf.hpp:271
#define U_LOG_T(...)
Log a message at U_LOGGING_TRACE level, conditional on the global log level.
Definition u_logging.h:430
void m_predict_relation(const struct xrt_space_relation *rel, double delta_s, struct xrt_space_relation *out_rel)
Using the given xrt_space_relation predicts a new xrt_space_relation delta_s into the future.
Definition m_predict.c:106
int64_t timepoint_ns
Integer timestamp type.
Definition u_time.h:77
static double time_ns_to_s(time_duration_ns ns)
Convert nanoseconds duration to double seconds.
Definition u_time.h:97
int64_t time_duration_ns
Integer duration type in nanoseconds.
Definition u_time.h:88
xrt_space_relation_flags
Flags of which components of a xrt_space_relation is valid.
Definition xrt_defines.h:685
C interface to math library.
Simple function to predict a new pose from a given pose.
Small utility for keeping track of the history of an xrt_space_relation, ie.
m_relation_history_result
Describes how the resulting space relation for the desired time stamp was generated.
Definition m_relation_history.h:37
@ M_RELATION_HISTORY_RESULT_INTERPOLATED
The desired timestamp was between two entries.
Definition m_relation_history.h:40
@ M_RELATION_HISTORY_RESULT_PREDICTED
The desired timestamp was newer than the most recent entry.
Definition m_relation_history.h:41
@ M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED
The desired timestamp was older than the oldest entry.
Definition m_relation_history.h:42
@ M_RELATION_HISTORY_RESULT_INVALID
The supplied timestamp was invalid (0) or buffer was empty.
Definition m_relation_history.h:38
@ M_RELATION_HISTORY_RESULT_EXACT
The exact desired timestamp was found.
Definition m_relation_history.h:39
C vec3 math library.
C++-only functionality in the Math helper library.
Definition m_documentation.hpp:15
Wrapper around OS threading native functions.
The base type for a relation history.
Definition m_relation_history.hpp:158
m_relation_history_result getLocked(timepoint_ns timestamp_ns, T &out_entry) const
Gets the device's relation at the specified timestamp.
Definition m_relation_history.hpp:231
bool pushOrUpdate(const T &entry, bool allow_update)
Pushes a new entry (if newest) or updates an old one if allowed.
Definition m_relation_history.hpp:369
std::optional< T > getNearest(timepoint_ns timestamp_ns, time_duration_ns max_slop_ns) const
Gets the stored entry nearest to the given timestamp, if one exists within max_slop_ns of it.
Definition m_relation_history.hpp:460
std::optional< T > getNearestLocked(timepoint_ns timestamp_ns, time_duration_ns max_slop_ns) const
Gets the stored entry nearest to the given timestamp, if one exists within max_slop_ns of it.
Definition m_relation_history.hpp:325
std::optional< T > getLatestLocked() const
Gets the latest entry, or nullopt if none exists.
Definition m_relation_history.hpp:216
std::optional< T > getExactLocked(timepoint_ns timestamp_ns) const
Gets an exact entry, if it exists.
Definition m_relation_history.hpp:297
std::optional< T > getExact(timepoint_ns timestamp_ns) const
Gets an exact entry, if it exists.
Definition m_relation_history.hpp:448
void clear()
Clears the relation history.
Definition m_relation_history.hpp:424
bool pushWithMotionEstimation(const T &new_entry, bool allow_update)
Pushes a new entry, doing motion estimation if required.
Definition m_relation_history.hpp:393
bool pushOrUpdateLocked(const T &rhe, bool allow_update)
Pushes a new entry (if newest) or updates an old one if allowed.
Definition m_relation_history.hpp:173
std::optional< T > getLatest() const
Gets the latest entry, or nullopt if none exists.
Definition m_relation_history.hpp:381
uint32_t size() const
Gets the amount of entries contained within the history.
Definition m_relation_history.hpp:412
m_relation_history_result get(timepoint_ns timestamp_ns, T &out_entry) const
Gets the device's relation at the specified timestamp.
Definition m_relation_history.hpp:436
Definition m_relation_history.hpp:41
RelationHistoryEntry predict(time_duration_ns delta_ns) const
Predicts the relation history entry to the specified delta, returning the new one.
Definition m_relation_history.hpp:47
void estimateMotion(RelationHistoryEntry &in_out_end) const
Estimates motion from this relation history to the end entry, modifying the end entry with the new ve...
Definition m_relation_history.hpp:105
RelationHistoryEntry interpolate(timepoint_ns to_ns, RelationHistoryEntry successor) const
Interpolates between the two relation history entries, returning a new one.
Definition m_relation_history.hpp:60
A relation with two spaces, includes velocity and acceleration.
Definition xrt_defines.h:713
A 3 element vector with single floats.
Definition xrt_defines.h:310
Basic logging functionality.
Ringbuffer implementation for keeping track of the past state of things.
Time-keeping: a clock that is steady, convertible to system time, and ideally high-resolution.
Common defines and enums for XRT.