52 final.timestamp += delta_ns;
63 final.timestamp = to_ns;
65 int64_t diff_before = to_ns - this->timestamp;
66 int64_t diff_after = successor.timestamp - to_ns;
68 float amount_to_lerp = (float)diff_before / (
float)(diff_before + diff_after);
70 auto &result_relation =
final.relation;
73 result_relation.relation_flags =
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);
81 if (result_relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
83 math_quat_slerp(&this->relation.pose.orientation, &successor.relation.pose.orientation,
84 amount_to_lerp, &result_relation.pose.orientation);
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);
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);
107 const auto &old_relation = this->relation;
108 const auto &new_relation = in_out_end.relation;
110 xrt_vec3 &out_linear_velocity = in_out_end.relation.linear_velocity;
111 xrt_vec3 &out_angular_velocity = in_out_end.relation.angular_velocity;
114 double dt =
time_ns_to_s(in_out_end.timestamp - this->timestamp);
122 if (new_relation.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) {
125 out_linear_velocity = new_relation.linear_velocity;
128 else if (shared_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
132 out_linear_velocity = (new_relation.pose.position - old_relation.pose.position) / dt;
136 if (new_relation.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) {
139 out_angular_velocity = new_relation.angular_velocity;
142 else if (shared_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
146 math_quat_finite_difference(&old_relation.pose.orientation, &new_relation.pose.orientation, dt,
147 &out_angular_velocity);
160 static constexpr size_t kBufLen = 4096;
164 mutable os::Mutex mutex;
176 if (this->impl.
empty() || rhe.timestamp > this->impl.back().timestamp) {
191 for (
auto it = this->impl.
end(); it != this->impl.
begin();) {
196 if (rhe.timestamp > item.timestamp) {
201 if (item.timestamp == rhe.timestamp) {
218 if (this->impl.
empty()) {
222 return this->impl.
back();
233 if (this->impl.
empty() || timestamp_ns == 0) {
239 const auto b = this->impl.
begin();
240 const auto e = this->impl.
end();
244 const auto it = std::lower_bound(
245 b, e, timestamp_ns, [](
const T &rhe, int64_t timestamp) {
return rhe.timestamp < timestamp; });
255 U_LOG_T(
"Extrapolating %f s past the back of the buffer!", delta_s);
257 out_entry = this->impl.
back().predict(delta_ns);
260 if (timestamp_ns == it->timestamp) {
263 U_LOG_T(
"Exact match in the buffer!");
274 U_LOG_T(
"Extrapolating %f s before the front of the buffer!", delta_s);
275 out_entry = this->impl.
front().predict(delta_ns);
279 U_LOG_T(
"Interpolating within buffer!");
283 const auto &predecessor = *(it - 1);
284 const auto &successor = *it;
287 out_entry = predecessor.interpolate(timestamp_ns, successor);
299 if (this->impl.
empty()) {
303 const auto b = this->impl.
begin();
304 const auto e = this->impl.
end();
306 const auto it = std::lower_bound(
307 b, e, timestamp_ns, [](
const T &rhe, int64_t timestamp) {
return rhe.timestamp < timestamp; });
309 if (it == e || it->timestamp != timestamp_ns) {
327 if (this->impl.
empty() || max_slop_ns < 0) {
331 const auto b = this->impl.
begin();
332 const auto e = this->impl.
end();
335 const auto it = std::lower_bound(
336 b, e, timestamp_ns, [](
const T &rhe, int64_t timestamp) {
return rhe.timestamp < timestamp; });
338 std::optional<T> best = std::nullopt;
342 const auto &before = *(it - 1);
344 if (diff <= best_diff) {
351 const auto &after = *it;
354 if (diff < best_diff || (!best.has_value() && diff <= best_diff)) {
371 std::unique_lock<os::Mutex> lock(this->mutex);
383 std::unique_lock<os::Mutex> lock(this->mutex);
395 std::unique_lock<os::Mutex> lock(this->mutex);
398 T end_entry = new_entry;
399 if (begin_entry.has_value() && end_entry.timestamp > begin_entry->timestamp) {
400 begin_entry->estimateMotion(end_entry);
414 std::unique_lock<os::Mutex> lock(this->mutex);
415 return static_cast<uint32_t
>(this->impl.
size());
426 std::unique_lock<os::Mutex> lock(this->mutex);
438 std::unique_lock<os::Mutex> lock(this->mutex);
439 return this->
getLocked(timestamp_ns, out_entry);
450 std::unique_lock<os::Mutex> lock(this->mutex);
462 std::unique_lock<os::Mutex> lock(this->mutex);
467using RelationHistory = RelationHistoryBase<RelationHistoryEntry>;
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++-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.