Fix bug limiting collision between half space and ellipse

CHANGELOG.md

@@ -22,6 +22,10 @@
     [#467](https://github.com/flexible-collision-library/fcl/pull/467)
   * Convex gets *some* validation and improved support for the GJK `supportVertex()` API:
     [#488](https://github.com/flexible-collision-library/fcl/pull/488)
+  * Fixed bug in collision function matrix that only allowed calculation of
+    collision between ellipsoid and half space *with that ordering*. Now also
+    supports half space and ellipsoid.
+    [#520](https://github.com/flexible-collision-library/fcl/pull/520)
 
 * Broadphase
 

include/fcl/narrowphase/detail/collision_func_matrix-inl.h

@@ -753,6 +753,7 @@ CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
 
   collision_matrix[GEOM_HALFSPACE][GEOM_BOX] = &ShapeShapeCollide<Halfspace<S>, Box<S>, NarrowPhaseSolver>;
   collision_matrix[GEOM_HALFSPACE][GEOM_SPHERE] = &ShapeShapeCollide<Halfspace<S>, Sphere<S>, NarrowPhaseSolver>;
+  collision_matrix[GEOM_HALFSPACE][GEOM_ELLIPSOID] = &ShapeShapeCollide<Halfspace<S>, Ellipsoid<S>, NarrowPhaseSolver>;
   collision_matrix[GEOM_HALFSPACE][GEOM_CAPSULE] = &ShapeShapeCollide<Halfspace<S>, Capsule<S>, NarrowPhaseSolver>;
   collision_matrix[GEOM_HALFSPACE][GEOM_CONE] = &ShapeShapeCollide<Halfspace<S>, Cone<S>, NarrowPhaseSolver>;
   collision_matrix[GEOM_HALFSPACE][GEOM_CYLINDER] = &ShapeShapeCollide<Halfspace<S>, Cylinder<S>, NarrowPhaseSolver>;

test/narrowphase/detail/CMakeLists.txt

@@ -1,2 +1,11 @@
+set(tests
+    test_collision_func_matrix.cpp
+    )
+
+# Build all the tests
+foreach(test ${tests})
+    add_fcl_test(${test})
+endforeach(test)
+
 add_subdirectory(convexity_based_algorithm)
 add_subdirectory(primitive_shape_algorithm)

test/narrowphase/detail/test_collision_func_matrix.cpp

@@ -0,0 +1,169 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2021. Toyota Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of CNRS-LAAS and AIST nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Sean Curtis (sean@tri.global) (2021) */
+
+// Tests the custom sphere-box tests: distance and collision.
+
+#include "fcl/narrowphase/detail/collision_func_matrix.h"
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+#include "fcl/geometry/collision_geometry.h"
+#include "fcl/narrowphase/detail/gjk_solver_libccd.h"
+#include "fcl/narrowphase/detail/gjk_solver_indep.h"
+
+namespace fcl {
+
+#define NODE_CASE(node) case node: \
+  out << #node; \
+  break;
+
+std::ostream& operator<<(std::ostream& out, const NODE_TYPE& node) {
+  switch (node) {
+    NODE_CASE(BV_UNKNOWN)
+    NODE_CASE(BV_AABB)
+    NODE_CASE(BV_OBB)
+    NODE_CASE(BV_RSS)
+    NODE_CASE(BV_kIOS)
+    NODE_CASE(BV_OBBRSS)
+    NODE_CASE(BV_KDOP16)
+    NODE_CASE(BV_KDOP18)
+    NODE_CASE(BV_KDOP24)
+    NODE_CASE(GEOM_BOX)
+    NODE_CASE(GEOM_SPHERE)
+    NODE_CASE(GEOM_ELLIPSOID)
+    NODE_CASE(GEOM_CAPSULE)
+    NODE_CASE(GEOM_CONE)
+    NODE_CASE(GEOM_CYLINDER)
+    NODE_CASE(GEOM_CONVEX)
+    NODE_CASE(GEOM_PLANE)
+    NODE_CASE(GEOM_HALFSPACE)
+    NODE_CASE(GEOM_TRIANGLE)
+    NODE_CASE(GEOM_OCTREE)
+    NODE_CASE(NODE_COUNT)
+  }
+  return out;
+}
+
+#undef NODE_CASE
+
+namespace detail {
+namespace {
+
+using std::array;
+
+/* The collision function matrix defines the function that can be used to
+ evaluate collision between two CollisionGeometry instances based on their
+ node type. In NODE_TYPE, there are nine primitive types, eight BV types, and
+ three special values (see below). We want to confirm the table satisfies the
+ following invariants:
+
+  1 There's a function for all primitive pairs, in both orderings.
+  2 There's a function for each (BV, geometry) combination (but not
+    (geometry, BV)).
+  3 There's a function for each BV type with its *own* type.
+  4 If octomap is available, there should be functions between GEOM_OCTREE
+    and every geometry type (in both orderings), with itself, and with each
+    BV type (in both orderings).
+
+ Primitive geometries: GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE,
+                       GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE,
+                       GEOM_HALFSPACE
+ BV types: BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18,
+           BV_KDOP24
+ Special: GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT, BV_UNKNOWN
+
+ NODE_COUNT is merely a convenient symbol for reporting the number of enumerated
+ values. BV_UNKNOWN and GEOM_TRIANGLE are not part of the function matrix. */
+template <typename Solver>
+void ConfirmSupportedGeometryForSolver() {
+  const array<NODE_TYPE, 9> geoms{GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID,
+                                  GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER,
+                                  GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE};
+  const array<NODE_TYPE, 8> bvs{BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS,
+                                BV_KDOP16, BV_KDOP18, BV_KDOP24};
+
+  const CollisionFunctionMatrix<Solver> matrix_struct;
+  const auto& matrix = matrix_struct.collision_matrix;
+
+  // 1) Confirm a function between geom_A -> geom_B for all geometries.
+  for (const NODE_TYPE& g1 : geoms) {
+    for (const NODE_TYPE& g2 : geoms) {
+      EXPECT_NE(matrix[g1][g2], nullptr) << "(" << g1 << ", " << g2 << ")";
+    }
+  }
+
+  // 2) and 3) There's a function for BV -> geometry and BV -> self.
+  for (const NODE_TYPE& bv : bvs) {
+    EXPECT_NE(matrix[bv][bv], nullptr) << "(" << bv << ", " << bv << ")";
+    for (const NODE_TYPE& g : geoms) {
+      EXPECT_NE(matrix[bv][g], nullptr) << "(" << bv << ", " << g << ")";
+    }
+  }
+
+#if FCL_HAVE_OCTOMAP
+  // 4) Octomap has functions.
+  const NODE_TYPE oct = GEOM_OCTREE;
+  EXPECT_NE(matrix[oct][oct], nullptr) << "(" << oct << ", " << oct << ")";
+  for (const NODE_TYPE& g: geoms) {
+    EXPECT_NE(matrix[oct][g], nullptr) << "(" << oct << ", " << g << ")";
+    EXPECT_NE(matrix[g][oct], nullptr) << "(" << g << ", " << oct << ")";
+  }
+  for (const NODE_TYPE& bv: bvs) {
+    EXPECT_NE(matrix[oct][bv], nullptr) << "(" << oct << ", " << bv << ")";
+    EXPECT_NE(matrix[bv][oct], nullptr) << "(" << bv << ", " << oct << ")";
+  }
+#endif
+}
+
+GTEST_TEST(CollisionFuncMatrix, LibCccdSolverSupport) {
+  ConfirmSupportedGeometryForSolver<GJKSolver_libccd<double>>();
+}
+
+GTEST_TEST(CollisionFuncMatrix, IndepSolverSupport) {
+  ConfirmSupportedGeometryForSolver<GJKSolver_indep<double>>();
+}
+
+} // namespace
+} // namespace detail
+} // namespace fcl
+
+//==============================================================================
+int main(int argc, char *argv[]) {
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}