mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2024-12-26 17:37:34 +00:00
429 lines
13 KiB
C++
429 lines
13 KiB
C++
// Copyright 2021, Collabora, Ltd.
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
/*!
|
|
* @file
|
|
* @brief HistoryBuffer collection tests.
|
|
* @author Rylie Pavlik <rylie.pavlik@collabora.com>
|
|
*/
|
|
|
|
#include <math/m_relation_history.h>
|
|
#include <util/u_time.h>
|
|
#include <util/u_template_historybuf.hpp>
|
|
#include <iostream>
|
|
|
|
|
|
using xrt::auxiliary::util::HistoryBuffer;
|
|
|
|
template <typename Container>
|
|
static inline std::ostream &
|
|
operator<<(std::ostream &os, const xrt::auxiliary::util::RandomAccessIteratorBase<Container> &iter_base)
|
|
{
|
|
os << "Iterator@[" << iter_base.index() << "]";
|
|
return os;
|
|
}
|
|
|
|
|
|
#include "catch_amalgamated.hpp"
|
|
|
|
|
|
|
|
TEST_CASE("m_relation_history")
|
|
{
|
|
m_relation_history *rh = nullptr;
|
|
|
|
m_relation_history_create(&rh);
|
|
SECTION("empty buffer")
|
|
{
|
|
xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO;
|
|
CHECK(m_relation_history_get(rh, 0, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
CHECK(m_relation_history_get(rh, 1, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
}
|
|
SECTION("populated buffer")
|
|
{
|
|
xrt_space_relation relation = XRT_SPACE_RELATION_ZERO;
|
|
relation.relation_flags = (xrt_space_relation_flags)( //
|
|
XRT_SPACE_RELATION_POSITION_TRACKED_BIT | //
|
|
XRT_SPACE_RELATION_POSITION_VALID_BIT | //
|
|
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | //
|
|
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | //
|
|
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); //
|
|
relation.linear_velocity.x = 1.f;
|
|
|
|
// arbitrary value
|
|
constexpr auto T0 = 20 * (uint64_t)U_TIME_1S_IN_NS;
|
|
// one second after T0
|
|
constexpr auto T1 = T0 + (uint64_t)U_TIME_1S_IN_NS;
|
|
// two seconds after T0
|
|
constexpr auto T2 = T1 + (uint64_t)U_TIME_1S_IN_NS;
|
|
|
|
xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO;
|
|
int64_t out_time = 0;
|
|
|
|
CHECK(m_relation_history_get_size(rh) == 0);
|
|
CHECK_FALSE(m_relation_history_get_latest(rh, &out_time, &out_relation));
|
|
|
|
|
|
CHECK(m_relation_history_push(rh, &relation, T0));
|
|
CHECK(m_relation_history_get_size(rh) == 1);
|
|
CHECK(m_relation_history_get_latest(rh, &out_time, &out_relation));
|
|
CHECK(out_time == T0);
|
|
|
|
relation.pose.position.x = 1.f;
|
|
CHECK(m_relation_history_push(rh, &relation, T1));
|
|
CHECK(m_relation_history_get_size(rh) == 2);
|
|
CHECK(m_relation_history_get_latest(rh, &out_time, &out_relation));
|
|
CHECK(out_time == T1);
|
|
|
|
relation.pose.position.x = 2.f;
|
|
CHECK(m_relation_history_push(rh, &relation, T2));
|
|
CHECK(m_relation_history_get_size(rh) == 3);
|
|
CHECK(m_relation_history_get_latest(rh, &out_time, &out_relation));
|
|
CHECK(out_time == T2);
|
|
|
|
// Try going back in time: should fail to push, leave state the same
|
|
CHECK_FALSE(m_relation_history_push(rh, &relation, T1));
|
|
CHECK(m_relation_history_get_size(rh) == 3);
|
|
CHECK(m_relation_history_get_latest(rh, &out_time, &out_relation));
|
|
CHECK(out_time == T2);
|
|
|
|
CHECK(m_relation_history_get(rh, 0, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
|
|
CHECK(m_relation_history_get(rh, T0, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 0.f);
|
|
|
|
CHECK(m_relation_history_get(rh, T1, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 1.f);
|
|
|
|
CHECK(m_relation_history_get(rh, T2, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 2.f);
|
|
|
|
|
|
CHECK(m_relation_history_get(rh, T0 - (uint64_t)U_TIME_1S_IN_NS, &out_relation) ==
|
|
M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED);
|
|
CHECK(out_relation.pose.position.x < 0.f);
|
|
|
|
CHECK(m_relation_history_get(rh, (T0 + T1) / 2, &out_relation) ==
|
|
M_RELATION_HISTORY_RESULT_INTERPOLATED);
|
|
CHECK(out_relation.pose.position.x > 0.f);
|
|
CHECK(out_relation.pose.position.x < 1.f);
|
|
|
|
CHECK(m_relation_history_get(rh, (T1 + T2) / 2, &out_relation) ==
|
|
M_RELATION_HISTORY_RESULT_INTERPOLATED);
|
|
CHECK(out_relation.pose.position.x > 1.f);
|
|
CHECK(out_relation.pose.position.x < 2.f);
|
|
|
|
CHECK(m_relation_history_get(rh, T2 + (uint64_t)U_TIME_1S_IN_NS, &out_relation) ==
|
|
M_RELATION_HISTORY_RESULT_PREDICTED);
|
|
CHECK(out_relation.pose.position.x > 2.f);
|
|
}
|
|
|
|
|
|
m_relation_history_destroy(&rh);
|
|
}
|
|
|
|
|
|
TEST_CASE("RelationHistory")
|
|
{
|
|
using xrt::auxiliary::math::RelationHistory;
|
|
RelationHistory rh;
|
|
|
|
SECTION("empty buffer")
|
|
{
|
|
xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO;
|
|
CHECK(rh.get(0, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
CHECK(rh.get(1, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
}
|
|
SECTION("populated buffer")
|
|
{
|
|
xrt_space_relation relation = XRT_SPACE_RELATION_ZERO;
|
|
relation.relation_flags = (xrt_space_relation_flags)( //
|
|
XRT_SPACE_RELATION_POSITION_TRACKED_BIT | //
|
|
XRT_SPACE_RELATION_POSITION_VALID_BIT | //
|
|
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | //
|
|
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | //
|
|
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); //
|
|
relation.linear_velocity.x = 1.f;
|
|
|
|
// arbitrary value
|
|
constexpr auto T0 = 20 * (uint64_t)U_TIME_1S_IN_NS;
|
|
// one second after T0
|
|
constexpr auto T1 = T0 + (uint64_t)U_TIME_1S_IN_NS;
|
|
// two seconds after T0
|
|
constexpr auto T2 = T1 + (uint64_t)U_TIME_1S_IN_NS;
|
|
|
|
xrt_space_relation out_relation = XRT_SPACE_RELATION_ZERO;
|
|
int64_t out_time = 0;
|
|
|
|
CHECK(rh.size() == 0);
|
|
CHECK_FALSE(rh.get_latest(&out_time, &out_relation));
|
|
|
|
|
|
CHECK(rh.push(relation, T0));
|
|
CHECK(rh.size() == 1);
|
|
CHECK(rh.get_latest(&out_time, &out_relation));
|
|
CHECK(out_time == T0);
|
|
|
|
relation.pose.position.x = 1.f;
|
|
CHECK(rh.push(relation, T1));
|
|
CHECK(rh.size() == 2);
|
|
CHECK(rh.get_latest(&out_time, &out_relation));
|
|
CHECK(out_time == T1);
|
|
|
|
relation.pose.position.x = 2.f;
|
|
CHECK(rh.push(relation, T2));
|
|
CHECK(rh.size() == 3);
|
|
CHECK(rh.get_latest(&out_time, &out_relation));
|
|
CHECK(out_time == T2);
|
|
|
|
// Try going back in time: should fail to push, leave state the same
|
|
CHECK_FALSE(rh.push(relation, T1));
|
|
CHECK(rh.size() == 3);
|
|
CHECK(rh.get_latest(&out_time, &out_relation));
|
|
CHECK(out_time == T2);
|
|
|
|
CHECK(rh.get(0, &out_relation) == M_RELATION_HISTORY_RESULT_INVALID);
|
|
|
|
CHECK(rh.get(T0, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 0.f);
|
|
|
|
CHECK(rh.get(T1, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 1.f);
|
|
|
|
CHECK(rh.get(T2, &out_relation) == M_RELATION_HISTORY_RESULT_EXACT);
|
|
CHECK(out_relation.pose.position.x == 2.f);
|
|
|
|
|
|
CHECK(rh.get(T0 - (uint64_t)U_TIME_1S_IN_NS, &out_relation) ==
|
|
M_RELATION_HISTORY_RESULT_REVERSE_PREDICTED);
|
|
CHECK(out_relation.pose.position.x < 0.f);
|
|
|
|
CHECK(rh.get((T0 + T1) / 2, &out_relation) == M_RELATION_HISTORY_RESULT_INTERPOLATED);
|
|
CHECK(out_relation.pose.position.x > 0.f);
|
|
CHECK(out_relation.pose.position.x < 1.f);
|
|
|
|
CHECK(rh.get((T1 + T2) / 2, &out_relation) == M_RELATION_HISTORY_RESULT_INTERPOLATED);
|
|
CHECK(out_relation.pose.position.x > 1.f);
|
|
CHECK(out_relation.pose.position.x < 2.f);
|
|
|
|
CHECK(rh.get(T2 + (uint64_t)U_TIME_1S_IN_NS, &out_relation) == M_RELATION_HISTORY_RESULT_PREDICTED);
|
|
CHECK(out_relation.pose.position.x > 2.f);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("u_template_historybuf")
|
|
{
|
|
HistoryBuffer<int, 4> buffer;
|
|
SECTION("behavior when empty")
|
|
{
|
|
CHECK(buffer.empty());
|
|
CHECK(0 == buffer.size()); // NOLINT
|
|
CHECK_FALSE(buffer.begin().valid());
|
|
CHECK_FALSE(buffer.end().valid());
|
|
CHECK(buffer.begin() == buffer.end());
|
|
{
|
|
INFO("Check after pop_back");
|
|
REQUIRE_FALSE(buffer.pop_back());
|
|
CHECK(buffer.empty());
|
|
}
|
|
}
|
|
SECTION("behavior with one")
|
|
{
|
|
buffer.push_back(0);
|
|
CHECK_FALSE(buffer.empty());
|
|
CHECK(buffer.size() == 1);
|
|
|
|
// check iterators
|
|
CHECK(buffer.begin().valid());
|
|
CHECK_FALSE(buffer.end().valid());
|
|
CHECK_FALSE(buffer.begin() == buffer.end());
|
|
{
|
|
auto it = buffer.end();
|
|
// should be permanently cleared
|
|
++it;
|
|
CHECK_FALSE(it.valid());
|
|
--it;
|
|
CHECK_FALSE(it.valid());
|
|
}
|
|
CHECK(buffer.begin() == buffer.cbegin());
|
|
CHECK(buffer.end() == buffer.cend());
|
|
|
|
// can we decrement our past-the-end iterator to get the begin iterator?
|
|
CHECK(buffer.begin() == --(buffer.end()));
|
|
|
|
// make sure post-decrement works right
|
|
CHECK_FALSE(buffer.begin() == (buffer.end())--);
|
|
|
|
// make sure post-increment works right
|
|
CHECK(buffer.begin() == buffer.begin()++);
|
|
|
|
// make sure pre-increment works right
|
|
CHECK_FALSE(buffer.begin() == ++(buffer.begin()));
|
|
|
|
|
|
// check contents
|
|
CHECK_NOTHROW(buffer.get_at_index(0));
|
|
CHECK_FALSE(buffer.get_at_index(0) == nullptr);
|
|
CHECK(*buffer.get_at_index(0) == 0);
|
|
|
|
CHECK_FALSE(buffer.get_at_age(0) == nullptr);
|
|
CHECK(*buffer.get_at_age(0) == 0);
|
|
CHECK_FALSE(buffer.get_at_clamped_age(0) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(0) == 0);
|
|
|
|
CHECK(buffer.get_at_age(1) == nullptr);
|
|
|
|
CHECK_FALSE(buffer.get_at_clamped_age(1) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(1) == 0);
|
|
|
|
CHECK_FALSE(buffer.get_at_clamped_age(2) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(2) == 0);
|
|
|
|
CHECK_NOTHROW(buffer.front());
|
|
CHECK(buffer.front() == 0);
|
|
|
|
CHECK_NOTHROW(buffer.back());
|
|
CHECK(buffer.back() == 0);
|
|
|
|
CHECK(*buffer.begin() == buffer.front());
|
|
|
|
{
|
|
INFO("Check after pop_back");
|
|
REQUIRE(buffer.pop_back());
|
|
CHECK(buffer.size() == 0);
|
|
|
|
REQUIRE_FALSE(buffer.pop_back());
|
|
}
|
|
}
|
|
|
|
SECTION("behavior with two")
|
|
{
|
|
buffer.push_back(0);
|
|
buffer.push_back(1);
|
|
CHECK_FALSE(buffer.empty());
|
|
CHECK(buffer.size() == 2);
|
|
SECTION("check iterators")
|
|
{
|
|
// check iterators
|
|
CHECK(buffer.begin().valid());
|
|
CHECK_FALSE(buffer.end().valid());
|
|
CHECK_FALSE(buffer.begin() == buffer.end());
|
|
{
|
|
auto it = buffer.end();
|
|
// should be permanently cleared
|
|
++it;
|
|
CHECK_FALSE(it.valid());
|
|
--it;
|
|
CHECK_FALSE(it.valid());
|
|
}
|
|
CHECK(buffer.begin() == buffer.cbegin());
|
|
CHECK(buffer.end() == buffer.cend());
|
|
|
|
// can we decrement our past-the-end iterator to get the begin iterator?
|
|
CHECK(buffer.begin() == --(--(buffer.end())));
|
|
|
|
// make sure post-decrement works right
|
|
CHECK_FALSE(buffer.begin() == (buffer.end())--);
|
|
|
|
// make sure post-increment works right
|
|
CHECK(buffer.begin() == buffer.begin()++);
|
|
|
|
// make sure pre-increment works right
|
|
CHECK_FALSE(buffer.begin() == ++(buffer.begin()));
|
|
}
|
|
SECTION("check contents")
|
|
{
|
|
// check contents
|
|
CHECK_NOTHROW(buffer.get_at_index(0));
|
|
CHECK_FALSE(buffer.get_at_index(0) == nullptr);
|
|
CHECK(*buffer.get_at_index(0) == 0);
|
|
CHECK_FALSE(buffer.get_at_index(1) == nullptr);
|
|
CHECK(*buffer.get_at_index(1) == 1);
|
|
CHECK(buffer.get_at_index(2) == nullptr);
|
|
|
|
CHECK_NOTHROW(buffer.get_at_age(0));
|
|
CHECK_FALSE(buffer.get_at_age(0) == nullptr);
|
|
CHECK(*buffer.get_at_age(0) == 1);
|
|
CHECK_FALSE(buffer.get_at_clamped_age(0) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(0) == 1);
|
|
|
|
CHECK_FALSE(buffer.get_at_age(1) == nullptr);
|
|
CHECK(*buffer.get_at_age(1) == 0);
|
|
CHECK_FALSE(buffer.get_at_clamped_age(1) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(1) == 0);
|
|
|
|
CHECK(buffer.get_at_age(2) == nullptr);
|
|
|
|
CHECK_FALSE(buffer.get_at_clamped_age(2) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(2) == 0);
|
|
|
|
CHECK_FALSE(buffer.get_at_clamped_age(3) == nullptr);
|
|
CHECK(*buffer.get_at_clamped_age(3) == 0);
|
|
|
|
CHECK_NOTHROW(buffer.front());
|
|
CHECK(buffer.front() == 0);
|
|
|
|
CHECK_NOTHROW(buffer.back());
|
|
CHECK(buffer.back() == 1);
|
|
|
|
CHECK(*buffer.begin() == buffer.front());
|
|
CHECK(buffer.back() == *(--buffer.end()));
|
|
}
|
|
SECTION("Check after pop_back")
|
|
{
|
|
REQUIRE(buffer.pop_back());
|
|
CHECK(buffer.size() == 1);
|
|
CHECK(buffer.front() == 0);
|
|
|
|
REQUIRE(buffer.pop_back());
|
|
CHECK(buffer.size() == 0);
|
|
}
|
|
}
|
|
|
|
SECTION("algorithm behavior with 3")
|
|
{
|
|
buffer.push_back(0);
|
|
buffer.push_back(2);
|
|
buffer.push_back(4);
|
|
CHECK_FALSE(buffer.empty());
|
|
CHECK(buffer.size() == 3);
|
|
CHECK(buffer.begin() == std::find(buffer.begin(), buffer.end(), 0));
|
|
CHECK(++(buffer.begin()) == std::find(buffer.begin(), buffer.end(), 2));
|
|
CHECK(buffer.end() == std::find(buffer.begin(), buffer.end(), 5));
|
|
|
|
CHECK(++(buffer.begin()) == std::lower_bound(buffer.begin(), buffer.end(), 1));
|
|
}
|
|
}
|
|
|
|
TEST_CASE("IteratorBase")
|
|
{
|
|
|
|
HistoryBuffer<int, 4> buffer;
|
|
buffer.push_back(0);
|
|
buffer.push_back(2);
|
|
buffer.push_back(4);
|
|
using namespace xrt::auxiliary::util;
|
|
using const_iterator = typename HistoryBuffer<int, 4>::const_iterator;
|
|
const_iterator default_constructed{};
|
|
const_iterator begin_constructed = buffer.begin();
|
|
const_iterator end_constructed = buffer.end();
|
|
|
|
SECTION("Check default constructed")
|
|
{
|
|
CHECK_FALSE(default_constructed.valid());
|
|
CHECK(default_constructed.is_cleared());
|
|
}
|
|
SECTION("Check begin constructed")
|
|
{
|
|
CHECK(begin_constructed.valid());
|
|
CHECK_FALSE(begin_constructed.is_cleared());
|
|
CHECK((--begin_constructed).is_cleared());
|
|
}
|
|
SECTION("Check end constructed")
|
|
{
|
|
CHECK_FALSE(end_constructed.valid());
|
|
CHECK_FALSE(end_constructed.is_cleared());
|
|
// if we go past the end, we can go backwards into validity.
|
|
CHECK_FALSE((++end_constructed).is_cleared());
|
|
}
|
|
}
|