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

@@ -819,19 +819,17 @@ struct Storage {
 
   inline size_t cols() const { return NumCols; }
 
-#ifdef NDEBUG
+  void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
-  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

@@ -1,114 +0,0 @@
-#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, simd4x4f* trans, simd4f n, simd4f* out_vert);
+void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
 
 float sch_query_face_directions(
     const sch_hull* hull_A,
@@ -489,17 +489,14 @@ 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, simd4x4f* trans, simd4f n, simd4f* out_vert) {
+void sch_hull_get_support(const sch_hull* hull, simd4f normal, 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, n_local);
+    normal_dot_edge = simd4f_dot3_scalar(dir, normal);
 
     if (normal_dot_edge <= 0.) {
       edge = edge->twin->next;
@@ -509,7 +506,7 @@ void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4
     }
   } while (last_edge != edge || normal_dot_edge > 0);
 
-	simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
+  *out_vert = edge->vert->p;
 }
 
 float sch_query_face_directions(
@@ -522,14 +519,18 @@ float sch_query_face_directions(
     sch_plane plane;
     sch_hull_calc_plane(hull_A, fi, &plane);
     simd4f vert_B;
-    sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B);
+    simd4f plane_n_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);
 
-    float distance = sch_plane_distance(&plane, &vert_B);
+    simd4f vert_A;
+    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_B;
+      result->vert = vert_A;
     }
   }
 
@@ -571,12 +572,10 @@ 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.
-      simd4x4f identity;
+      sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
-      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, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
+      sch_hull_get_support(hull_B, 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,22 +624,6 @@ 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();
@@ -647,8 +631,7 @@ struct SconvHullScene {
     sch_create_unitbox(&mHullB.hull);
     mHullB.createMesh();
 
-    // setupSceneEdgeCollision();
+    setupSceneEdgeCollision();
-		setupRotatedBoxes();
 
     mCurrentHull = &mHullA;
   };

tests/sconvcolTests.cc

@@ -261,13 +261,10 @@ 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, &identity, normal, &support);
+      sch_hull_get_support(&hull, 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));
@@ -277,7 +274,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, &identity, normal, &support);
+      sch_hull_get_support(&hull, 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));
@@ -287,7 +284,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, &identity, normal, &support);
+      sch_hull_get_support(&hull, normal, &support);
       THEN ("y component of support is 0.5") {
         REQUIRE (simd4f_get_y(support) == 0.5);
       }
@@ -642,4 +639,4 @@ TEST_CASE ("ClipFaces", "[sconvcol]") {
       sch_manifold_free_memory(&manifold);
     }
   }
-}
+}