diff --git a/src/xrt/auxiliary/math/m_relation_history.cpp b/src/xrt/auxiliary/math/m_relation_history.cpp index 686926501..5dbe02376 100644 --- a/src/xrt/auxiliary/math/m_relation_history.cpp +++ b/src/xrt/auxiliary/math/m_relation_history.cpp @@ -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 - * @ingroup drv_ht + * @author Ryan Pavlik + * @ingroup aux_math */ #include @@ -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(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(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(at_timestamp_ns) - predecessor.timestamp; + int64_t diff_after = static_cast(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; }