Added missing rbdl test files

3rdparty/rbdl/tests/rbdl_tests.h

@@ -0,0 +1,114 @@
+#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