Report configuration state for narrowphase errors

doc/Doxyfile.in

@@ -2037,7 +2037,7 @@ INCLUDE_FILE_PATTERNS  =
 # recursively expanded use the := operator instead of the = operator.
 # This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
 
-PREDEFINED             =
+PREDEFINED             = FCL_DOXYGEN_CXX=1
 
 # If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
 # tag can be used to specify a list of macro names that should be expanded. The

include/fcl/geometry/shape/box.h

@@ -40,6 +40,8 @@
 
 #include "fcl/geometry/shape/shape_base.h"
 
+#include <iostream>
+
 namespace fcl
 {
 
@@ -78,6 +80,12 @@ class FCL_EXPORT Box : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Box& box) {
+    out << "Box" << box.side.transpose();
+    return out;
+  }
 };
 
 using Boxf = Box<float>;

include/fcl/geometry/shape/capsule.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_CAPSULE_H
 #define FCL_SHAPE_CAPSULE_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -75,6 +77,12 @@ class FCL_EXPORT Capsule : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Capsule& capsule) {
+    out << "Capsule(r: " << capsule.radius << ", lz: " << capsule.lz << ")";
+    return out;
+  }
 };
 
 using Capsulef = Capsule<float>;

include/fcl/geometry/shape/cone.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_CONE_H
 #define FCL_SHAPE_CONE_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -77,6 +79,12 @@ class FCL_EXPORT Cone : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Cone& cone) {
+    out << "Cone(r: " << cone.radius << ", lz: " << cone.lz << ")";
+    return out;
+  }
 };
 
 using Conef = Cone<float>;

include/fcl/geometry/shape/convex.h

@@ -39,6 +39,8 @@
 #ifndef FCL_SHAPE_CONVEX_H
 #define FCL_SHAPE_CONVEX_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -158,6 +160,13 @@ class FCL_EXPORT Convex : public ShapeBase<S_>
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
 
+  friend
+  std::ostream& operator<<(std::ostream& out, const Convex& convex) {
+    out << "Convex(v count: " << convex.vertices_->size() << ", f count: "
+        << convex.getFaceCount() << ")";
+    return out;
+  }
+
 private:
   const std::shared_ptr<const std::vector<Vector3<S>>> vertices_;
   const int num_faces_;

include/fcl/geometry/shape/cylinder.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_CYLINDER_H
 #define FCL_SHAPE_CYLINDER_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -75,6 +77,12 @@ class FCL_EXPORT Cylinder : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Cylinder& cylinder) {
+    out << "Cylinder(r: " << cylinder.radius << ", lz: " << cylinder.lz << ")";
+    return out;
+  }
 };
 
 using Cylinderf = Cylinder<float>;

include/fcl/geometry/shape/ellipsoid.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_ELLIPSOID_H
 #define FCL_SHAPE_ELLIPSOID_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -75,6 +77,12 @@ class FCL_EXPORT Ellipsoid : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Ellipsoid& ellipsoid) {
+    out << "Ellipsoid(radii: " << ellipsoid.radii.transpose() << ")";
+    return out;
+  }
 };
 
 using Ellipsoidf = Ellipsoid<float>;

include/fcl/geometry/shape/halfspace.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_HALFSPACE_H
 #define FCL_SHAPE_HALFSPACE_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 #include "fcl/math/bv/OBB.h"
 #include "fcl/math/bv/RSS.h"
@@ -82,6 +84,13 @@ class FCL_EXPORT Halfspace : public ShapeBase<S_>
   /// @brief Planed offset
   S d;
 
+  friend
+  std::ostream& operator<<(std::ostream& out, const Halfspace& halfspace) {
+    out << "Halfspace(n: " << halfspace.n.transpose() << ", d: "
+        << halfspace.d << ")";
+    return out;
+  }
+
 protected:
 
   /// @brief Turn non-unit normal into unit

include/fcl/geometry/shape/plane.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_PLANE_H
 #define FCL_SHAPE_PLANE_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -75,6 +77,12 @@ class FCL_EXPORT Plane : public ShapeBase<S_>
   /// @brief Plane offset 
   S d;
 
+  friend
+  std::ostream& operator<<(std::ostream& out, const Plane& plane) {
+    out << "Plane(n: " << plane.n.transpose() << ", d: " << plane.d << ")";
+    return out;
+  }
+
 protected:
 
   /// @brief Turn non-unit normal into unit 

include/fcl/geometry/shape/sphere.h

@@ -40,6 +40,8 @@
 
 #include "fcl/geometry/shape/shape_base.h"
 
+#include <iostream>
+
 namespace fcl
 {
 
@@ -69,6 +71,12 @@ class FCL_EXPORT Sphere : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const Sphere& sphere) {
+    out << "Sphere(" << sphere.radius << ")";
+    return out;
+  }
 };
 
 using Spheref = Sphere<float>;

include/fcl/geometry/shape/triangle_p.h

@@ -38,6 +38,8 @@
 #ifndef FCL_SHAPE_TRIANGLE_P_H
 #define FCL_SHAPE_TRIANGLE_P_H
 
+#include <iostream>
+
 #include "fcl/geometry/shape/shape_base.h"
 
 namespace fcl
@@ -68,6 +70,13 @@ class FCL_EXPORT TriangleP : public ShapeBase<S_>
   /// @brief get the vertices of some convex shape which can bound this shape in
   /// a specific configuration
   std::vector<Vector3<S>> getBoundVertices(const Transform3<S>& tf) const;
+
+  friend
+  std::ostream& operator<<(std::ostream& out, const TriangleP& tri) {
+    out << "TriangleP(a: " << tri.a.transpose()
+        << ", b: " << tri.b.transpose() << ", c: " << tri.c.transpose() << ")";
+    return out;
+  }
 };
 
 using TrianglePf = TriangleP<float>;

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

@@ -39,6 +39,7 @@
 #define FCL_NARROWPHASE_DETAIL_GJKLIBCCD_INL_H
 
 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h"
+#include "fcl/narrowphase/detail/failed_at_this_configuration.h"
 
 #include <unordered_map>
 #include <unordered_set>
@@ -949,6 +950,8 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
  * @retval dir The vector normal to the face, and points outward from the
  * polytope. `dir` is unnormalized, that it does not necessarily have a unit
  * length.
+ * @throws UnexpectedConfigurationException if built in debug mode _and_ the
+ * triangle has zero area (either by being too small, or being co-linear).
  */
 static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
                                             const ccd_pt_face_t* face) {
@@ -961,7 +964,10 @@ static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
   const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v;
   const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ?
                         face->edge[1]->vertex[1]->v.v : test_v;
-  assert(!triangle_area_is_zero(a, b, c));
+  if (triangle_area_is_zero(a, b, c)) {
+    FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+        "Cannot compute face normal for a degenerate (zero-area) triangle");
+  }
 #endif
   // We find two edges of the triangle as e1 and e2, and the normal vector
   // of the face is e1.cross(e2).
@@ -1076,7 +1082,7 @@ static bool isOutsidePolytopeFace(const ccd_pt_t* polytope,
  * The invariant for computing the visible patch is that for each edge in the
  * polytope, if both neighbouring faces are visible, then the edge is an
  * internal edge; if only one neighbouring face is visible, then the edge
- * is a border edge. 
+ * is a border edge.
  * For each face, if one of its edges is an internal edge, then the face is
  * visible.
  */
@@ -1188,13 +1194,15 @@ static void ComputeVisiblePatchRecursive(
  * @param[out] border_edges    The collection of patch border edges.
  * @param[out] visible_faces   The collection of patch faces.
  * @param[out] internal_edges  The collection of internal edges.
+ * @throws UnexpectedConfigurationException in debug builds if the resulting
+ * patch is not consistent.
  *
  * @pre The `polytope` is convex.
  * @pre The face `f` is visible from `query_point`.
  * @pre Output parameters are non-null.
  * TODO(hongkai.dai@tri.global) Replace patch computation with patch deletion
  * and return border edges as an optimization.
- * TODO(hongkai.dai@tri.global) Consider caching results of per-face visiblity
+ * TODO(hongkai.dai@tri.global) Consider caching results of per-face visibility
  * status to prevent redundant recalculation -- or by associating the face
  * normal with the face.
  */
@@ -1216,8 +1224,13 @@ static void ComputeVisiblePatch(
     ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point,
                                  border_edges, visible_faces, internal_edges);
   }
-  assert(ComputeVisiblePatchRecursiveSanityCheck(
-      polytope, *border_edges, *visible_faces, *internal_edges));
+#ifndef NDEBUG
+  if (!ComputeVisiblePatchRecursiveSanityCheck(
+          polytope, *border_edges, *visible_faces, *internal_edges)) {
+    FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+        "The visible patch failed its sanity check");
+  }
+#endif
 }
 
 /** Expands the polytope by adding a new vertex `newv` to the polytope. The
@@ -1241,6 +1254,10 @@ static void ComputeVisiblePatch(
  * expandPolytope() function.
  * @param[in] newv The new vertex add to the polytope.
  * @retval status Returns 0 on success. Returns -2 otherwise.
+ * @throws UnexpectedConfigurationException if expanding is meaningless either
+ * because 1) the nearest feature is a vertex, 2) the new vertex lies on
+ * an edge of the current polytope, or 3) the visible feature is an edge with
+ * one or more adjacent faces with no area.
  */
 static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
                           const ccd_support_t *newv)
@@ -1255,18 +1272,21 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
   // face on which the closest point lives, and then do a depth-first search on
   // its neighbouring triangles, until the triangle cannot be seen from the new
   // vertex.
-  // TODO(hongkai.dai@tri.global): it is inefficient to store visible 
+  // TODO(hongkai.dai@tri.global): it is inefficient to store visible
   // faces/edges. A better implementation should remove visible faces and
   // internal edges inside ComputeVisiblePatch() function, when traversing the
   // faces on the polytope. We focus on the correctness in the first place.
   // Later when we make sure that the whole EPA implementation is bug free, we
   // will improve the performance.
 
   ccd_pt_face_t* start_face = NULL;
-  // If the feature is a point, in the EPA algorithm, this means that the two
-  // objects are in touching contact. The EPA should terminate before calling
-  // this expandPolytope function, when it detects touching contact.
-  assert(el->type != CCD_PT_VERTEX);
+
+  if (el->type == CCD_PT_VERTEX) {
+    FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+        "The visible feature is a vertex. This should already have been "
+        "identified as a touching contact");
+  }
+
   // Start with the face on which the closest point lives
   if (el->type == CCD_PT_FACE) {
     start_face = reinterpret_cast<ccd_pt_face_t*>(el);
@@ -1279,11 +1299,10 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
     } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) {
       start_face = f[1];
     } else {
-      throw std::logic_error(
-          "FCL expandPolytope(): This should not happen. Both the nearest "
-          "point and the new vertex are on an edge, thus the nearest distance "
-          "should be 0. This is touching contact, and we should not expand the "
-          "polytope for touching contact.");
+      FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+          "Both the nearest point and the new vertex are on an edge, thus the "
+          "nearest distance should be 0. This is touching contact, and should "
+          "already have been identified");
     }
   }
 
@@ -1312,7 +1331,7 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
   // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that
   // all vertices has at least one face/edge invisible from the new vertex
   // `newv`.
-  
+
   // Now add the new vertex.
   ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(polytope, newv);
 
@@ -1351,6 +1370,7 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
  * boundary of the polytope to the origin.
  * @retval dir The support direction along which to expand the polytope. Notice
  * that dir is a normalized vector.
+ * @throws UnexpectedConfigurationException if the nearest feature is a vertex.
  */
 static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope,
                                       const ccd_pt_el_t* nearest_feature) {
@@ -1365,10 +1385,10 @@ static ccd_vec3_t supportEPADirection(const ccd_pt_t* polytope,
     // nearest point is the origin.
     switch (nearest_feature->type) {
       case CCD_PT_VERTEX: {
-        throw std::logic_error(
-            "FCL supportEPADirection(): The nearest point to the origin is a "
-            "vertex of the polytope. This should be identified as a touching "
-            "contact, before calling function supportEPADirection()");
+        FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+            "The nearest point to the origin is a vertex of the polytope. This "
+            "should be identified as a touching contact");
+        break;
       }
       case CCD_PT_EDGE: {
         // When the nearest point is on an edge, the origin must be on that