rbdlsim/3rdparty/rbdl/tests/rbdl_tests.h

115 lines
2.9 KiB
C++

#ifndef RBDL_RBDL_TESTS_H
#define RBDL_RBDL_TESTS_H
#include <cmath>
#include "catch.hpp"
#include "rbdl/rbdl_math.h"
template <typename T>
struct IsCloseMatcher : Catch::MatcherBase<T> {
IsCloseMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
using namespace Catch::Matchers::Floating;
if (std::abs(v - m_comparator)
> (m_atol + m_rtol * std::abs(m_comparator))) {
return false;
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
IsCloseMatcher<T>
IsClose(T const& comparator, double atol = 1.0e-8, double rtol = 1.0e-5) {
return IsCloseMatcher<T>(comparator, atol, rtol);
}
template <typename T>
struct AllCloseVectorMatcher : Catch::MatcherBase<T> {
AllCloseVectorMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
using namespace Catch::Matchers::Floating;
if (m_comparator.size() != v.size()) {
return false;
}
for (std::size_t i = 0; i < v.size(); ++i) {
if (!IsClose(m_comparator[i], m_atol, m_rtol).match(v[i])) {
return false;
}
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T const& m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
AllCloseVectorMatcher<T> AllCloseVector(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5) {
return AllCloseVectorMatcher<T>(comparator, atol, rtol);
}
template <typename T>
struct AllCloseMatrixMatcher : Catch::MatcherBase<T> {
AllCloseMatrixMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
if ((m_comparator.rows() != v.rows()) && (m_comparator.cols() != v.cols()))
return false;
for (int i = 0; i < v.rows(); ++i) {
for (int j = 0; j < v.cols(); ++j) {
if (!IsClose(m_comparator(i, j), m_atol, m_rtol).match(v(i, j))) {
return false;
}
}
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T const& m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
AllCloseMatrixMatcher<T> AllCloseMatrix(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5) {
return AllCloseMatrixMatcher<T>(comparator, atol, rtol);
}
#endif // RBDL_RBDL_TESTS_H