mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

a/math: Simplify m_history_relation using standard algorithms

Browse source
This commit is contained in:
Ryan Pavlik 2021-11-23 17:28:50 -06:00 committed by Jakob Bornecrantz
parent 614e0e58ae
commit 1c183a9eeb
1 changed files with 68 additions and 124 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

192
src/xrt/auxiliary/math/m_relation_history.cpp
View file

@ -5,7 +5,8 @@
* @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 <moses@collabora.com>
* @ingroup drv_ht
* @author Ryan Pavlik <ryan.pavlik@collabora.com>
* @ingroup aux_math
*/
#include <algorithm>
@ -91,146 +92,89 @@ m_relation_history_get(struct m_relation_history *rh, uint64_t at_timestamp_ns,
m_relation_history_result ret = M_RELATION_HISTORY_RESULT_INVALID;
if (rh->impl.empty() || at_timestamp_ns == 0) {
// Do nothing. You push nothing to the buffer you get nothing from the buffer.
goto end;
}
*out_relation = {};
} else {
const auto b = rh->impl.begin();
const auto e = rh->impl.end();
{
uint64_t oldest_in_buffer = rh->impl.front().timestamp;
uint64_t newest_in_buffer = rh->impl.back().timestamp;
// find the first element *not less than* our value. the lambda we pass is the comparison function, to
// compare against timestamps.
const auto it =
std::lower_bound(b, e, at_timestamp_ns, [](const relation_history_entry &rhe, uint64_t timestamp) {
return rhe.timestamp < timestamp;
});
if (at_timestamp_ns > newest_in_buffer) {
if (it == e) {
// lower bound is at the end:
// 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;
int64_t diff_prediction_ns = static_cast<int64_t>(at_timestamp_ns) - rh->impl.back().timestamp;
double delta_s = time_ns_to_s(diff_prediction_ns);
U_LOG_T("Extrapolating %f s after the head of the buffer!", delta_s);
U_LOG_T("Extrapolating %f s past the back of the buffer!", delta_s);
m_predict_relation(&rh->impl.back().relation, delta_s, out_relation);
ret = M_RELATION_HISTORY_RESULT_PREDICTED;
goto end;
} else if (at_timestamp_ns < oldest_in_buffer) {
} else if (at_timestamp_ns == it->timestamp) {
// exact match
U_LOG_T("Exact match in the buffer!");
*out_relation = it->relation;
ret = M_RELATION_HISTORY_RESULT_EXACT;
} else if (it == b) {
// lower bound is at the beginning:
// 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;
int64_t diff_prediction_ns = static_cast<int64_t>(at_timestamp_ns) - rh->impl.front().timestamp;
double delta_s = time_ns_to_s(diff_prediction_ns);
U_LOG_T("Extrapolating %f s before the tail of the buffer!", delta_s);
U_LOG_T("Extrapolating %f s before the front of the buffer!", delta_s);
m_predict_relation(&rh->impl.front().relation, delta_s, out_relation);
ret = M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED;
goto end;
} else {
U_LOG_T("Interpolating within buffer!");
// We precede *it and follow *(it - 1) (which we know exists because we already handled the it =
// begin() case)
auto predecessor = *(it - 1);
auto successor = *it;
// Do the thing.
int64_t diff_before = static_cast<int64_t>(at_timestamp_ns) - predecessor.timestamp;
int64_t diff_after = static_cast<int64_t>(successor.timestamp) - at_timestamp_ns;
float amount_to_lerp = (float)diff_before / (float)(diff_before + diff_after);
// Copy relation flags
xrt_space_relation result{};
result.relation_flags = (enum xrt_space_relation_flags)(predecessor.relation.relation_flags &
successor.relation.relation_flags);
// First-order implementation - just lerp between the before and after
if (0 != (result.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT)) {
result.pose.position = m_vec3_lerp(predecessor.relation.pose.position,
successor.relation.pose.position, amount_to_lerp);
}
if (0 != (result.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) {
math_quat_slerp(&predecessor.relation.pose.orientation,
&successor.relation.pose.orientation, amount_to_lerp,
&result.pose.orientation);
}
//! @todo Does this make any sense?
if (0 != (result.relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT)) {
result.angular_velocity =
m_vec3_lerp(predecessor.relation.angular_velocity,
successor.relation.angular_velocity, amount_to_lerp);
}
if (0 != (result.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT)) {
result.linear_velocity =
m_vec3_lerp(predecessor.relation.linear_velocity,
successor.relation.linear_velocity, amount_to_lerp);
}
*out_relation = result;
ret = M_RELATION_HISTORY_RESULT_INTERPOLATED;
}
U_LOG_T("Interpolating within buffer!");
#if 0
// Very slow - O(n) - but easier to read
size_t idx = 0;
for (size_t i = 0; i < rh->impl.size(); i++) {
auto ts = rh->impl.get_at_age(i)->timestamp;
if (ts == at_timestamp_ns) {
*out_relation = rh->impl.get_at_age(i)->relation;
ret = M_RELATION_HISTORY_RESULT_EXACT;
goto end;
}
if (ts < at_timestamp_ns) {
// If the entry we're looking at is before the input time
idx = i;
break;
}
}
U_LOG_T("Correct answer is %li", idx);
#else
// Fast - O(log(n)) - but hard to read
int idx = BufLen / 2; // 2048
int step = idx;
for (int i = power2 - 2; i >= -1; i--) {
// 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;
assert(step == (int)pow(2, i));
if (idx >= (int)rh->impl.size()) {
// We'd be looking at an uninitialized value. Go back closer to the head of the buffer.
idx -= step;
continue;
}
assert(idx > 0);
uint64_t ts_after = rh->impl.get_at_age(idx - 1)->timestamp;
uint64_t ts_before = rh->impl.get_at_age(idx)->timestamp;
if (ts_before == at_timestamp_ns || ts_after == at_timestamp_ns) {
// exact match
break;
}
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.get_at_age(idx)->relation;
struct xrt_space_relation after = rh->impl.get_at_age(idx - 1)->relation;
if (rh->impl.get_at_age(idx)->timestamp == at_timestamp_ns) {
// exact match: before
*out_relation = before;
ret = M_RELATION_HISTORY_RESULT_EXACT;
goto end;
}
if (rh->impl.get_at_age(idx - 1)->timestamp == at_timestamp_ns) {
// exact match: after
*out_relation = after;
ret = M_RELATION_HISTORY_RESULT_EXACT;
goto end;
}
int64_t diff_before, diff_after = 0;
diff_before = at_timestamp_ns - rh->impl.get_at_age(idx)->timestamp;
diff_after = rh->impl.get_at_age(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);
ret = M_RELATION_HISTORY_RESULT_INTERPOLATED;
}
end:
os_mutex_unlock(&rh->mutex);
return ret;
}