diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 104939bb3..280ea5cc5 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -36,6 +36,8 @@ set(MATH_SOURCE_FILES math/m_predict.c math/m_predict.h math/m_quatexpmap.cpp + math/m_relation_history.h + math/m_relation_history.cpp math/m_space.cpp math/m_space.h math/m_vec2.h diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp new file mode 100644 index 000000000..f97859a6f --- /dev/null +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -0,0 +1,224 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Small utility for keeping track of the history of an xrt_space_relation, ie. for knowing where a HMD or + * controller was in the past. + * @author Moses Turner + * @ingroup drv_ht + */ + +#include +#include +#include +#include +#include +#include +#include "math/m_api.h" +#include "math/m_predict.h" +#include "math/m_vec3.h" +#include "os/os_time.h" +#include "util/u_logging.h" +#include "util/u_trace_marker.h" +#include "xrt/xrt_defines.h" +#include "os/os_threading.h" +#include "util/u_template_historybuf.hpp" + +#include "m_relation_history.h" + + +using namespace xrt::auxiliary::util; + +struct relation_history_entry +{ + struct xrt_space_relation relation; + uint64_t timestamp; +}; + +#define leng 4096 +#define power2 12 +#undef RH_DEBUG + +struct m_relation_history +{ + HistoryBuffer impl; + bool has_first_sample; + struct os_mutex mutex; +}; + + +extern "C" { +void +m_relation_history_create(struct m_relation_history **rh_ptr) +{ + *rh_ptr = U_TYPED_CALLOC(struct m_relation_history); + struct m_relation_history *rh = *rh_ptr; + + rh->impl.topIdx = 0; + rh->impl.length = 0; + rh->has_first_sample = false; + os_mutex_init(&rh->mutex); +#if 0 + struct xrt_space_relation first_relation = {}; + first_relation.pose.orientation.w = 1.0f; // Everything else, including tracked flags, is 0. + m_relation_history_push(rh, &first_relation, os_monotonic_get_ns()); +#endif +} + +void +m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation *in_relation, uint64_t timestamp) +{ + XRT_TRACE_MARKER(); + struct relation_history_entry rhe; + rhe.relation = *in_relation; + rhe.timestamp = timestamp; + os_mutex_lock(&rh->mutex); + // Don't evaluate the second condition if the length is 0 - rh->impl[0] will be NULL! + if ((!rh->has_first_sample) || (rhe.timestamp > rh->impl[0]->timestamp)) { + // Everything explodes if the timestamps in relation_history aren't monotonically increasing. If we get + // a timestamp that's before the most recent timestamp in the buffer, just don't put it in the history. + rh->impl.push(rhe); + } + rh->has_first_sample = true; + os_mutex_unlock(&rh->mutex); +} + +void +m_relation_history_get(struct m_relation_history *rh, struct xrt_space_relation *out_relation, uint64_t at_timestamp_ns) +{ + XRT_TRACE_MARKER(); + os_mutex_lock(&rh->mutex); + if (rh->has_first_sample == 0) { + // Do nothing. You push nothing to the buffer you get nothing from the buffer. + goto end; + } + + { + uint64_t oldest_in_buffer = rh->impl[rh->impl.length - 1]->timestamp; + uint64_t newest_in_buffer = rh->impl[0]->timestamp; + + if (at_timestamp_ns > newest_in_buffer) { + // The desired timestamp is after what our buffer contains. + // Aka pose-prediction. + int64_t diff_prediction_ns = 0; + diff_prediction_ns = at_timestamp_ns - newest_in_buffer; + double delta_s = time_ns_to_s(diff_prediction_ns); +#ifdef RH_DEBUG + U_LOG_E("Extrapolating %f s after the head of the buffer!", delta_s); +#endif + m_predict_relation(&rh->impl[0]->relation, delta_s, out_relation); + goto end; + + } else if (at_timestamp_ns < oldest_in_buffer) { + // The desired timestamp is before what our buffer contains. + // Aka a weird edge case where somebody asks for a really old pose and we do our best. + int64_t diff_prediction_ns = 0; + diff_prediction_ns = at_timestamp_ns - oldest_in_buffer; + double delta_s = time_ns_to_s(diff_prediction_ns); +#ifdef RH_DEBUG + U_LOG_E("Extrapolating %f s before the tail of the buffer!", delta_s); +#endif + m_predict_relation(&rh->impl[rh->impl.length - 1]->relation, delta_s, out_relation); + goto end; + } +#ifdef RH_DEBUG + U_LOG_E("Interpolating within buffer!"); +#endif +#if 0 + // Very slow - O(n) - but easier to read + int idx = 0; + + for (int i = 0; i < rh->impl.length; i++) { + if (rh->impl[i]->timestamp < at_timestamp_ns) { + // If the entry we're looking at is before the input time + idx = i; + break; + } + } + U_LOG_E("Correct answer is %i", idx); +#else + + // Fast - O(log(n)) - but hard to read + int idx = leng / 2; // 2048 + int step = idx; + + for (int i = power2 - 2; i >= -1; i--) { + uint64_t ts_after = rh->impl[idx - 1]->timestamp; + uint64_t ts_before = rh->impl[idx]->timestamp; + + // This is a little hack because any power of two looks like 0b0001000 (with the 1 in a + // different place for each power). Bit-shift it and it either doubles or halves. In our case it + // halves. step should always be equivalent to pow(2,i). If it's not that's very very bad. + step = step >> 1; +#ifdef RH_DEBUG + assert(step == (int)pow(2, i)); +#endif + + if (idx >= rh->impl.length) { + // We'd be looking at an uninitialized value. Go back closer to the head of the buffer. + idx -= step; + continue; + } + if ((ts_before < at_timestamp_ns) && (ts_after > at_timestamp_ns)) { + // Found what we're looking for - at_timestamp_ns is between the reading before us and + // the reading after us. Break out of the loop + break; + } + + // This would mean you did the math very wrong. Doesn't happen. + assert(i != -1); + + if (ts_after > at_timestamp_ns) { + // the reading we're looking at is after the reading we want; go closer to the tail of + // the buffer + idx += step; + } else { + // the reading we're looking at is before the reading we want; go closer to the head of + // the buffer + idx -= step; + // Random note: some day, stop using pow(). it's slow, you can do + } + } +#endif + + // Do the thing. + struct xrt_space_relation before = rh->impl[idx]->relation; + struct xrt_space_relation after = rh->impl[idx - 1]->relation; + int64_t diff_before, diff_after = 0; + diff_before = at_timestamp_ns - rh->impl[idx]->timestamp; + diff_after = rh->impl[idx - 1]->timestamp - at_timestamp_ns; + + float amount_to_lerp = (float)diff_before / (float)(diff_before + diff_after); + + // Copy relation flags + out_relation->relation_flags = + (enum xrt_space_relation_flags)(before.relation_flags & after.relation_flags); + + // First-order implementation - just lerp between the before and after + out_relation->pose.position = m_vec3_lerp(before.pose.position, after.pose.position, amount_to_lerp); + math_quat_slerp(&before.pose.orientation, &after.pose.orientation, amount_to_lerp, + &out_relation->pose.orientation); + + //! @todo Does this make any sense? + out_relation->angular_velocity = + m_vec3_lerp(before.angular_velocity, after.angular_velocity, amount_to_lerp); + out_relation->linear_velocity = + m_vec3_lerp(before.linear_velocity, after.linear_velocity, amount_to_lerp); + } +end: + os_mutex_unlock(&rh->mutex); +} + +void +m_relation_history_destroy(struct m_relation_history **rh_ptr) +{ + struct m_relation_history *rh = *rh_ptr; + if (rh == NULL) { + // Do nothing, it's likely already been destroyed + return; + } + os_mutex_destroy(&rh->mutex); + free(rh); + *rh_ptr = NULL; +} +} diff --git a/src/xrt/auxiliary/math/m_relation_history.h b/src/xrt/auxiliary/math/m_relation_history.h new file mode 100644 index 000000000..0ae3be083 --- /dev/null +++ b/src/xrt/auxiliary/math/m_relation_history.h @@ -0,0 +1,54 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Small utility for keeping track of the history of an xrt_space_relation, ie. for knowing where a HMD or + * controller was in the past + * @author Moses Turner + * @ingroup drv_ht + */ + +#include "xrt/xrt_defines.h" +struct m_relation_history; + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + * Creates an opaque relation_history object. + * + * @ingroup aux_util + */ +void +m_relation_history_create(struct m_relation_history **rh); + +/*! + * Pushes a new pose to the history - if the history is full, it will also pop a pose out of the other side of the + * buffer. + * + * @ingroup aux_util + */ +void +m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation *in_relation, uint64_t ts); + +/*! + * Interpolates or extrapolates to the desired timestamp. Read-only operation - doesn't remove anything from the buffer + * or anything like that - you can call this as often as you want. + * + * @ingroup aux_util + */ +void +m_relation_history_get(struct m_relation_history *rh, struct xrt_space_relation *out_relation, uint64_t at_time_ns); + +/*! + * Destroys an opaque relation_history object. + * + * @ingroup aux_util + */ +void +m_relation_history_destroy(struct m_relation_history **rh); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index 3d89bd2c3..dced71d83 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -174,6 +174,8 @@ lib_aux_math = static_library( 'math/m_predict.c', 'math/m_predict.h', 'math/m_quatexpmap.cpp', + 'math/m_relation_history.h', + 'math/m_relation_history.cpp', 'math/m_space.cpp', 'math/m_space.h', 'math/m_vec2.h',