Added another sconv scene

3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h

@@ -819,17 +819,19 @@ struct Storage {
 
   inline size_t cols() const { return NumCols; }
 
-  void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
+#ifdef NDEBUG
+  void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {}
+#else
+  void resize(int num_rows, int num_cols) {
   // Resizing of fixed size matrices not allowed
-  #ifndef NDEBUG
       if (num_rows != NumRows || num_cols != NumCols) {
         std::cout << "Error: trying to resize fixed matrix from "
           << NumRows << ", " << NumCols << " to "
           << num_rows << ", " << num_cols << "." << std::endl;
       }
-  #endif
       assert (num_rows == NumRows && num_cols == NumCols);
   }
+#endif
 
   inline ScalarType& coeff(int row_index, int col_index) {
 //    assert (row_index >= 0 && row_index <= NumRows);

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

src/sconvcol.h

@@ -168,7 +168,7 @@ void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_p
 
 sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
 
-void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
+void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert);
 
 float sch_query_face_directions(
     const sch_hull* hull_A,
@@ -489,14 +489,17 @@ sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4
   return NULL;
 }
 
-void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert) {
+void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert) {
   sch_edge* edge = hull->faces[0].edge;
   sch_edge* last_edge = edge;
   float normal_dot_edge = -1.;
 
+  simd4f n_local;
+  simd4x4f_inv_ortho_matrix_vector3_mul(trans, &n, &n_local);
+
   do {
     simd4f dir = simd4f_normalize3(simd4f_sub (edge->next->vert->p, edge->vert->p));
-    normal_dot_edge = simd4f_dot3_scalar(dir, normal);
+    normal_dot_edge = simd4f_dot3_scalar(dir, n_local);
 
     if (normal_dot_edge <= 0.) {
       edge = edge->twin->next;
@@ -506,7 +509,7 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
     }
   } while (last_edge != edge || normal_dot_edge > 0);
 
-  *out_vert = edge->vert->p;
+	simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
 }
 
 float sch_query_face_directions(
@@ -519,18 +522,14 @@ float sch_query_face_directions(
     sch_plane plane;
     sch_hull_calc_plane(hull_A, fi, &plane);
     simd4f vert_B;
-    simd4f plane_n_B;
+    sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B);
-    simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &plane.n, &plane_n_B);
-    sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
 
-    simd4f vert_A;
+    float distance = sch_plane_distance(&plane, &vert_B);
-    simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
-    float distance = sch_plane_distance(&plane, &vert_A);
     if (distance > result->dist) {
       result->dist = distance;
       result->face_idx = fi;
       result->plane = plane;
-      result->vert = vert_A;
+      result->vert = vert_B;
     }
   }
 
@@ -572,10 +571,12 @@ float sch_query_edge_directions(
       // Note: in Gregorius talk he uses the origin of edge_A. This is wrong as
       // there are likely points in hull_A that are further out along the plane
       // normal. We therefore use the support point of hull_A along the axis.
-      sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
+      simd4x4f identity;
+      simd4x4f_identity(&identity);
+      sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p);
 
       simd4f vert_B_B;
-      sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
+      sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
       simd4f vert_B;
       simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);
 

src/vissim.cc

@@ -624,6 +624,22 @@ struct SconvHullScene {
     simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
   }
 
+	void setupRotatedBoxes () {
+		simd4x4f trans_A;
+		simd4x4f trans_B;
+		simd4x4f_axis_rotation (&trans_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
+
+		simd4x4f rot_B;
+		simd4x4f_axis_rotation (&rot_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
+		simd4x4f translation_B;
+		simd4x4f_translation (&translation_B, sqrt(2.f) + 0.001, 0.f, 0.f);
+
+		simd4x4f_matrix_mul (&translation_B, &rot_B, &trans_B);
+
+		mHullA.transform = trans_A;
+		mHullB.transform = trans_B;
+	}
+
   void init() {
     sch_create_unitbox(&mHullA.hull);
     mHullA.createMesh();
@@ -631,7 +647,8 @@ struct SconvHullScene {
     sch_create_unitbox(&mHullB.hull);
     mHullB.createMesh();
 
-    setupSceneEdgeCollision();
+    // setupSceneEdgeCollision();
+		setupRotatedBoxes();
 
     mCurrentHull = &mHullA;
   };

tests/sconvcolTests.cc

@@ -261,10 +261,13 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
     sch_hull hull;
     sch_create_unitbox(&hull);
 
+		simd4x4f identity;
+		simd4x4f_identity(&identity);
+
     WHEN("Querying support for 2, 2, 2") {
       simd4f support;
       simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
-      sch_hull_get_support(&hull, normal, &support);
+      sch_hull_get_support(&hull, &identity, normal, &support);
       THEN ("support is 0.5, 0.5, 0.5") {
         simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
         REQUIRE (sch_simd4f_equal(reference, support));
@@ -274,7 +277,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
     WHEN("Querying support for -2, -2, -2") {
       simd4f support;
       simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
-      sch_hull_get_support(&hull, normal, &support);
+      sch_hull_get_support(&hull, &identity, normal, &support);
       THEN ("support is -0.5, -0.5, -0.5") {
         simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
         REQUIRE (sch_simd4f_equal(reference, support));
@@ -284,7 +287,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
     WHEN("Querying support for 0, 1, 0") {
       simd4f support;
       simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
-      sch_hull_get_support(&hull, normal, &support);
+      sch_hull_get_support(&hull, &identity, normal, &support);
       THEN ("y component of support is 0.5") {
         REQUIRE (simd4f_get_y(support) == 0.5);
       }