matrixfurry.com
The open source OpenXR runtime
1// Copyright 2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Small utility for keeping track of the history of an xrt_space_relation, ie. for knowing where a HMD or
6 * controller was in the past.
7 * @author Moshi Turner <moshiturner@protonmail.com>
8 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
9 * @author Korcan Hussein <korcan.hussein@collabora.com>
10 * @ingroup aux_math
11 */
12
13#include "m_relation_history.h"
14
15#include "xrt/xrt_defines.h"
16
17#include "math/m_api.h"
18#include "math/m_predict.h"
19#include "math/m_vec3.h"
20
21#include "os/os_time.h"
22#include "os/os_threading.h"
23
24#include "util/u_logging.h"
25#include "util/u_trace_marker.h"
26#include "util/u_template_historybuf.hpp"
27
28#include <memory>
29#include <algorithm>
30#include <cstring>
31#include <stdio.h>
32#include <stdlib.h>
33#include <stdint.h>
34#include <assert.h>
35#include <mutex>
36
37using namespace xrt::auxiliary::util;
38namespace os = xrt::auxiliary::os;
39
40struct relation_history_entry
41{
42 struct xrt_space_relation relation;
43 int64_t timestamp;
44};
45
46static constexpr size_t BufLen = 4096;
47
48struct m_relation_history
49{
50 HistoryBuffer<struct relation_history_entry, BufLen> impl;
51 mutable os::Mutex mutex;
52};
53
54
55void
56m_relation_history_create(struct m_relation_history **rh_ptr)
57{
58 auto ret = std::make_unique<m_relation_history>();
59 *rh_ptr = ret.release();
60}
61
62bool
63m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation const *in_relation, int64_t timestamp)
64{
65 XRT_TRACE_MARKER();
66 struct relation_history_entry rhe;
67 rhe.relation = *in_relation;
68 rhe.timestamp = timestamp;
69 bool ret = false;
70 std::unique_lock<os::Mutex> lock(rh->mutex);
71 try {
72 // if we aren't empty, we can compare against the latest timestamp.
73 if (rh->impl.empty() || rhe.timestamp > rh->impl.back().timestamp) {
74 // Everything explodes if the timestamps in relation_history aren't monotonically increasing. If
75 // we get a timestamp that's before the most recent timestamp in the buffer, don't put it
76 // in the history.
77 rh->impl.push_back(rhe);
78 ret = true;
79 }
80 } catch (std::exception const &e) {
81 U_LOG_E("Caught exception: %s", e.what());
82 }
83 return ret;
84}
85
86enum m_relation_history_result
87m_relation_history_get(const struct m_relation_history *rh,
88 int64_t at_timestamp_ns,
89 struct xrt_space_relation *out_relation)
90{
91 XRT_TRACE_MARKER();
92 std::unique_lock<os::Mutex> lock(rh->mutex);
93 try {
94 if (rh->impl.empty() || at_timestamp_ns == 0) {
95 // Do nothing. You push nothing to the buffer you get nothing from the buffer.
96 *out_relation = {};
97 return M_RELATION_HISTORY_RESULT_INVALID;
98 }
99 const auto b = rh->impl.begin();
100 const auto e = rh->impl.end();
101
102 // Find the first element *not less than* our value. the lambda we pass is the comparison
103 // function, to compare against timestamps.
104 const auto it =
105 std::lower_bound(b, e, at_timestamp_ns, [](const relation_history_entry &rhe, int64_t timestamp) {
106 return rhe.timestamp < timestamp;
107 });
108
109 if (it == e) {
110 // lower bound is at the end:
111 // The desired timestamp is after what our buffer contains.
112 // (pose-prediction)
113 // Output flags match the most recent buffer entry.
114 int64_t diff_prediction_ns = static_cast<int64_t>(at_timestamp_ns) - rh->impl.back().timestamp;
115 double delta_s = time_ns_to_s(diff_prediction_ns);
116
117 U_LOG_T("Extrapolating %f s past the back of the buffer!", delta_s);
118
119 m_predict_relation(&rh->impl.back().relation, delta_s, out_relation);
120 return M_RELATION_HISTORY_RESULT_PREDICTED;
121 }
122 if (at_timestamp_ns == it->timestamp) {
123 // exact match:
124 // Flags copied directly along with everything else.
125 U_LOG_T("Exact match in the buffer!");
126 *out_relation = it->relation;
127 return M_RELATION_HISTORY_RESULT_EXACT;
128 }
129 if (it == b) {
130 // lower bound is at the beginning (and it's not an exact match):
131 // The desired timestamp is before what our buffer contains.
132 // (an edge case where somebody asks for a really old pose and we do our best)
133 // Output flags are the same as the input flags for the history entry we use
134 int64_t diff_prediction_ns = static_cast<int64_t>(at_timestamp_ns) - rh->impl.front().timestamp;
135 double delta_s = time_ns_to_s(diff_prediction_ns);
136 U_LOG_T("Extrapolating %f s before the front of the buffer!", delta_s);
137 m_predict_relation(&rh->impl.front().relation, delta_s, out_relation);
138 return M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED;
139 }
140 U_LOG_T("Interpolating within buffer!");
141
142 // We precede *it and follow *(it - 1) (which we know exists because we already handled
143 // the it = begin() case)
144 const auto &predecessor = *(it - 1);
145 const auto &successor = *it;
146
147 // Do the thing.
148 int64_t diff_before = static_cast<int64_t>(at_timestamp_ns) - predecessor.timestamp;
149 int64_t diff_after = static_cast<int64_t>(successor.timestamp) - at_timestamp_ns;
150
151 float amount_to_lerp = (float)diff_before / (float)(diff_before + diff_after);
152
153 // Copy intersection of relation flags
154 xrt_space_relation result{};
155 result.relation_flags = (enum xrt_space_relation_flags)(predecessor.relation.relation_flags &
156 successor.relation.relation_flags);
157 // First-order implementation - lerp between the before and after
158 if (0 != (result.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT)) {
159 result.pose.position = m_vec3_lerp(predecessor.relation.pose.position,
160 successor.relation.pose.position, amount_to_lerp);
161 }
162 if (0 != (result.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) {
163
164 math_quat_slerp(&predecessor.relation.pose.orientation, &successor.relation.pose.orientation,
165 amount_to_lerp, &result.pose.orientation);
166 }
167
168 //! @todo Does interpolating the velocities make any sense?
169 if (0 != (result.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT)) {
170 result.angular_velocity = m_vec3_lerp(predecessor.relation.angular_velocity,
171 successor.relation.angular_velocity, amount_to_lerp);
172 }
173 if (0 != (result.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT)) {
174 result.linear_velocity = m_vec3_lerp(predecessor.relation.linear_velocity,
175 successor.relation.linear_velocity, amount_to_lerp);
176 }
177 *out_relation = result;
178 return M_RELATION_HISTORY_RESULT_INTERPOLATED;
179
180 } catch (std::exception const &e) {
181 U_LOG_E("Caught exception: %s", e.what());
182 return M_RELATION_HISTORY_RESULT_INVALID;
183 }
184}
185
186bool
187m_relation_history_estimate_motion(struct m_relation_history *rh,
188 const struct xrt_space_relation *in_relation,
189 int64_t timestamp,
190 struct xrt_space_relation *out_relation)
191{
192
193 int64_t last_time_ns;
194 struct xrt_space_relation last_relation;
195 if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) {
196 return false;
197 };
198
199 float dt = (float)time_ns_to_s(timestamp - last_time_ns);
200
201 // Used to find out what values are valid in both the old relation and the new relation
202 enum xrt_space_relation_flags tmp_flags =
203 (enum xrt_space_relation_flags)(last_relation.relation_flags & in_relation->relation_flags);
204
205 // Brevity
206 enum xrt_space_relation_flags &outf = out_relation->relation_flags;
207
208
209 if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
210 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT);
211 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
212
213 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT);
214
215 out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt;
216 }
217
218 if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
219 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
220 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
221
222 outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT);
223
224 math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt,
225 &out_relation->angular_velocity);
226 }
227
228 out_relation->pose = in_relation->pose;
229
230 return true;
231}
232
233bool
234m_relation_history_get_latest(const struct m_relation_history *rh,
235 int64_t *out_time_ns,
236 struct xrt_space_relation *out_relation)
237{
238 std::unique_lock<os::Mutex> lock(rh->mutex);
239 if (rh->impl.empty()) {
240 return false;
241 }
242 *out_relation = rh->impl.back().relation;
243 *out_time_ns = rh->impl.back().timestamp;
244 return true;
245}
246
247uint32_t
248m_relation_history_get_size(const struct m_relation_history *rh)
249{
250 std::unique_lock<os::Mutex> lock(rh->mutex);
251 return (uint32_t)rh->impl.size();
252}
253
254void
255m_relation_history_clear(struct m_relation_history *rh)
256{
257 std::unique_lock<os::Mutex> lock(rh->mutex);
258 rh->impl.clear();
259}
260
261void
262m_relation_history_destroy(struct m_relation_history **rh_ptr)
263{
264 struct m_relation_history *rh = *rh_ptr;
265 if (rh == NULL) {
266 // Do nothing, it's likely already been destroyed
267 return;
268 }
269 try {
270 delete rh;
271 } catch (std::exception const &e) {
272 U_LOG_E("Caught exception: %s", e.what());
273 }
274 *rh_ptr = NULL;
275}