From 5e9376248a602e9e8d0a643bb7f6be09af1c4143 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Wed, 20 Mar 2019 13:19:33 -0700 Subject: [PATCH] Report configuration state for narrowphase errors (#381) This allows low-level, narrowphase algorithms to indicate to higher-level callers that something went so wrong that it would be helpful to capture the configuration that led to the error. Add exception 1. Add exception type. 2. Instrument some know problem call sites with the new exception. 3. Expand unit tests to cover the exceptions. i. Add FCL_EXPECT_THROWS_MESSAGE* functionality for tests. Add exception catching 1. Add exception processing at an appropriate call site. 1. Requires printing out shapes and solver data; so streaming operator support has been added to all corresponding parties. 2. Added *only* to signed distance functions of both solvers. Note that `CollisionGeometry` can span mulitple types and this doesn't necessarily support all types. However, unit tests pass and this can be extended to encompass more geometry types as necessary. --- doc/Doxyfile.in | 2 +- include/fcl/geometry/shape/box.h | 8 + include/fcl/geometry/shape/capsule.h | 8 + include/fcl/geometry/shape/cone.h | 8 + include/fcl/geometry/shape/convex.h | 9 + include/fcl/geometry/shape/cylinder.h | 8 + include/fcl/geometry/shape/ellipsoid.h | 8 + include/fcl/geometry/shape/halfspace.h | 9 + include/fcl/geometry/shape/plane.h | 8 + include/fcl/geometry/shape/sphere.h | 8 + include/fcl/geometry/shape/triangle_p.h | 9 + .../gjk_libccd-inl.h | 60 ++-- .../detail/failed_at_this_configuration.h | 125 ++++++++ .../narrowphase/detail/gjk_solver_indep-inl.h | 9 +- .../fcl/narrowphase/detail/gjk_solver_indep.h | 16 + .../detail/gjk_solver_libccd-inl.h | 11 +- .../narrowphase/detail/gjk_solver_libccd.h | 13 + .../detail/failed_at_this_configuration.cpp | 17 + test/expect_throws_message.h | 143 +++++++++ .../test_gjk_libccd-inl_epa.cpp | 302 ++++++++++-------- 20 files changed, 628 insertions(+), 153 deletions(-) create mode 100644 include/fcl/narrowphase/detail/failed_at_this_configuration.h create mode 100644 src/narrowphase/detail/failed_at_this_configuration.cpp create mode 100644 test/expect_throws_message.h diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index a8090d6f7..230daa70e 100644 --- a/doc/Doxyfile.in +++ b/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 diff --git a/include/fcl/geometry/shape/box.h b/include/fcl/geometry/shape/box.h index 1ec03d90f..d15e9f269 100644 --- a/include/fcl/geometry/shape/box.h +++ b/include/fcl/geometry/shape/box.h @@ -40,6 +40,8 @@ #include "fcl/geometry/shape/shape_base.h" +#include + namespace fcl { @@ -78,6 +80,12 @@ class FCL_EXPORT Box : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Box& box) { + out << "Box" << box.side.transpose(); + return out; + } }; using Boxf = Box; diff --git a/include/fcl/geometry/shape/capsule.h b/include/fcl/geometry/shape/capsule.h index 8bb3ffbea..5add95e27 100644 --- a/include/fcl/geometry/shape/capsule.h +++ b/include/fcl/geometry/shape/capsule.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CAPSULE_H #define FCL_SHAPE_CAPSULE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Capsule : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& 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; diff --git a/include/fcl/geometry/shape/cone.h b/include/fcl/geometry/shape/cone.h index f4cc617fe..ef9180c2a 100644 --- a/include/fcl/geometry/shape/cone.h +++ b/include/fcl/geometry/shape/cone.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CONE_H #define FCL_SHAPE_CONE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -77,6 +79,12 @@ class FCL_EXPORT Cone : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& 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; diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index 229d6ec9f..03c0489a5 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -39,6 +39,8 @@ #ifndef FCL_SHAPE_CONVEX_H #define FCL_SHAPE_CONVEX_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -158,6 +160,13 @@ class FCL_EXPORT Convex : public ShapeBase /// a specific configuration std::vector> getBoundVertices(const Transform3& 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>> vertices_; const int num_faces_; diff --git a/include/fcl/geometry/shape/cylinder.h b/include/fcl/geometry/shape/cylinder.h index 43be1def3..bd67d3132 100644 --- a/include/fcl/geometry/shape/cylinder.h +++ b/include/fcl/geometry/shape/cylinder.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_CYLINDER_H #define FCL_SHAPE_CYLINDER_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Cylinder : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& 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; diff --git a/include/fcl/geometry/shape/ellipsoid.h b/include/fcl/geometry/shape/ellipsoid.h index c876be685..85632a900 100644 --- a/include/fcl/geometry/shape/ellipsoid.h +++ b/include/fcl/geometry/shape/ellipsoid.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_ELLIPSOID_H #define FCL_SHAPE_ELLIPSOID_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Ellipsoid : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Ellipsoid& ellipsoid) { + out << "Ellipsoid(radii: " << ellipsoid.radii.transpose() << ")"; + return out; + } }; using Ellipsoidf = Ellipsoid; diff --git a/include/fcl/geometry/shape/halfspace.h b/include/fcl/geometry/shape/halfspace.h index 05f9d0881..374b9e264 100644 --- a/include/fcl/geometry/shape/halfspace.h +++ b/include/fcl/geometry/shape/halfspace.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_HALFSPACE_H #define FCL_SHAPE_HALFSPACE_H +#include + #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 /// @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 diff --git a/include/fcl/geometry/shape/plane.h b/include/fcl/geometry/shape/plane.h index f3518b1ce..bcf34212e 100644 --- a/include/fcl/geometry/shape/plane.h +++ b/include/fcl/geometry/shape/plane.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_PLANE_H #define FCL_SHAPE_PLANE_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -75,6 +77,12 @@ class FCL_EXPORT Plane : public ShapeBase /// @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 diff --git a/include/fcl/geometry/shape/sphere.h b/include/fcl/geometry/shape/sphere.h index 0c9082573..48b67927f 100644 --- a/include/fcl/geometry/shape/sphere.h +++ b/include/fcl/geometry/shape/sphere.h @@ -40,6 +40,8 @@ #include "fcl/geometry/shape/shape_base.h" +#include + namespace fcl { @@ -69,6 +71,12 @@ class FCL_EXPORT Sphere : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& tf) const; + + friend + std::ostream& operator<<(std::ostream& out, const Sphere& sphere) { + out << "Sphere(" << sphere.radius << ")"; + return out; + } }; using Spheref = Sphere; diff --git a/include/fcl/geometry/shape/triangle_p.h b/include/fcl/geometry/shape/triangle_p.h index 7c26fa67a..7cc18caa0 100644 --- a/include/fcl/geometry/shape/triangle_p.h +++ b/include/fcl/geometry/shape/triangle_p.h @@ -38,6 +38,8 @@ #ifndef FCL_SHAPE_TRIANGLE_P_H #define FCL_SHAPE_TRIANGLE_P_H +#include + #include "fcl/geometry/shape/shape_base.h" namespace fcl @@ -68,6 +70,13 @@ class FCL_EXPORT TriangleP : public ShapeBase /// @brief get the vertices of some convex shape which can bound this shape in /// a specific configuration std::vector> getBoundVertices(const Transform3& 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; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 2cd84eb72..90450f5d3 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/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 #include @@ -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,7 +1272,7 @@ 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. @@ -1263,10 +1280,13 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el, // 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(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 diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h new file mode 100644 index 000000000..b36bb5952 --- /dev/null +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -0,0 +1,125 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019. 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) */ + +#ifndef FCL_FAILED_AT_THIS_CONFIGURATION_H +#define FCL_FAILED_AT_THIS_CONFIGURATION_H + +#include +#include +#include +#include +#include +#include + +#include "fcl/export.h" + +namespace fcl { +namespace detail { + +/** A custom exception type that can be thrown in low-level code indicating + that the program has reached an unexpected configuration and that the caller + that ultimately has access to the collision objects and their transforms + should report the configuration that led to the exception. + + This is strictly an _internal_ exception; it should *always* be caught and + transformed into an exception that propagates to the operating system. + + Recommended usage is to throw by invoking the macro + FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION defined below. Code that exercises + functionality that throws this type of exception should catch it and transform + it to a `std::logic_error` by invoking ThrowDetailedConfiguration(). */ +class FCL_EXPORT FailedAtThisConfiguration final + : public std::exception { + public: + FailedAtThisConfiguration(const std::string& message) + : std::exception(), message_(message) {} + + const char* what() const noexcept final { return message_.c_str(); } + + private: + std::string message_; +}; + +/** Helper function for dispatching an `FailedAtThisConfiguration`. + Because this exception is designed to be caught and repackaged, we lose the + automatic association with file and line number. This wraps them into the + message of the exception so it can be preserved in the re-wrapping. + + @param message The error message itself. + @param func The name of the invoking function. + @param file The name of the file associated with the exception. + @param line The line number where the exception is thrown. */ +FCL_EXPORT void ThrowFailedAtThisConfiguration( + const std::string& message, const char* func, const char* file, int line); + +/** Helper class for propagating a low-level exception upwards but with + configuration-specific details appended. The parameters + + @param s1 The first shape in the query. + @param X_FS1 The pose of the first shape in frame F. + @param s2 The second shape in the query. + @param X_FS2 The pose of the second shape in frame F. + @param solver The solver. + @param e The original exception. + @tparam Shape1 The type of shape for shape 1. + @tparam Shape2 The type of shape for shape 2. + @tparam Solver The solver type (with scalar type erase). + @tparam Pose The pose type (a Transform with scalar type erased). + */ +template +void ThrowDetailedConfiguration(const Shape1& s1, const Pose& X_FS1, + const Shape2& s2, const Pose& X_FS2, + const Solver& solver, const std::exception& e) { + std::stringstream ss; + ss << std::setprecision(20); + ss << "Error with configuration" + << "\n Original error message: " << e.what() + << "\n Shape 1: " << s1 + << "\n X_FS1\n" << X_FS1.matrix() + << "\n Shape 2: " << s2 + << "\n X_FS2\n" << X_FS2.matrix() + << "\n Solver: " << solver; + throw std::logic_error(ss.str()); +} + +} // namespace detail +} // namespace fcl + +#define FCL_THROW_FAILED_AT_THIS_CONFIGURATION(message) \ + ::fcl::detail::ThrowFailedAtThisConfiguration(message, __func__, __FILE__, \ + __LINE__) + +#endif // FCL_FAILED_AT_THIS_CONFIGURATION_H diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index cf93d4558..ef9f8b158 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -57,6 +57,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h" +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" namespace fcl { @@ -669,8 +670,14 @@ bool GJKSolver_indep::shapeSignedDistance( Vector3* p2) const { // TODO: should implement the signed distance version - return ShapeDistanceIndepImpl::run( + bool result = false; + try { + result = ShapeDistanceIndepImpl::run( *this, s1, tf1, s2, tf2, dist, p1, p2); + } catch (const FailedAtThisConfiguration& e) { + ThrowDetailedConfiguration(s1, tf1, s2, tf2, *this, e); + } + return result; } // Shape distance algorithms not using built-in GJK algorithm diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep.h b/include/fcl/narrowphase/detail/gjk_solver_indep.h index 1d08e3fa7..14eb10ada 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep.h @@ -38,6 +38,8 @@ #ifndef FCL_NARROWPHASE_GJKSOLVERINDEP_H #define FCL_NARROWPHASE_GJKSOLVERINDEP_H +#include + #include "fcl/common/types.h" #include "fcl/narrowphase/contact_point.h" @@ -179,6 +181,20 @@ struct FCL_EXPORT GJKSolver_indep /// @brief smart guess mutable Vector3 cached_guess; + + friend + std::ostream& operator<<(std::ostream& out, const GJKSolver_indep& solver) { + out << "GjkSolver_indep" + << "\n gjk tolerance: " << solver.gjk_tolerance + << "\n gjk max iterations: " << solver.gjk_max_iterations + << "\n epa tolerance: " << solver.epa_tolerance + << "\n epa max face num: " << solver.epa_max_face_num + << "\n epa max vertex num: " << solver.epa_max_vertex_num + << "\n epa max iterations: " << solver.epa_max_iterations + << "\n enable cahced guess: " << solver.enable_cached_guess; + if (solver.enable_cached_guess) out << solver.cached_guess.transpose(); + return out; + } }; using GJKSolver_indepf = GJKSolver_indep; diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index d9ffffe80..3cef4d657 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -54,6 +54,7 @@ #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h" #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane.h" +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" namespace fcl { @@ -556,7 +557,7 @@ struct ShapeSignedDistanceLibccdImpl void* o1 = detail::GJKInitializer::createGJKObject(s1, tf1); void* o2 = detail::GJKInitializer::createGJKObject(s2, tf2); - bool res = detail::GJKSignedDistance( + bool res = detail::GJKSignedDistance( o1, detail::GJKInitializer::getSupportFunction(), o2, @@ -585,8 +586,14 @@ bool GJKSolver_libccd::shapeSignedDistance( Vector3* p1, Vector3* p2) const { - return ShapeSignedDistanceLibccdImpl::run( + bool result = false; + try { + result = ShapeSignedDistanceLibccdImpl::run( *this, s1, tf1, s2, tf2, dist, p1, p2); + } catch (const FailedAtThisConfiguration& e) { + ThrowDetailedConfiguration(s1, tf1, s2, tf2, *this, e); + } + return result; } diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd.h b/include/fcl/narrowphase/detail/gjk_solver_libccd.h index 2676427da..218a89481 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd.h @@ -38,6 +38,8 @@ #ifndef FCL_NARROWPHASE_GJKSOLVERLIBCCD_H #define FCL_NARROWPHASE_GJKSOLVERLIBCCD_H +#include + #include "fcl/common/types.h" #include "fcl/narrowphase/contact_point.h" @@ -168,6 +170,17 @@ struct FCL_EXPORT GJKSolver_libccd /// @brief the threshold used in GJK algorithm to stop distance iteration S distance_tolerance; + friend + std::ostream& operator<<(std::ostream& out, const GJKSolver_libccd& solver) { + out << "GjkSolver_libccd" + << "\n collision_tolerance: " << solver.collision_tolerance + << "\n max collision iterations: " << solver.max_collision_iterations + << "\n distance tolerance: " << solver.distance_tolerance + << "\n max distance iterations: " << solver.max_distance_iterations; + // NOTE: Cached guesses are not supported. + return out; + } + }; using GJKSolver_libccdf = GJKSolver_libccd; diff --git a/src/narrowphase/detail/failed_at_this_configuration.cpp b/src/narrowphase/detail/failed_at_this_configuration.cpp new file mode 100644 index 000000000..ba7875d3e --- /dev/null +++ b/src/narrowphase/detail/failed_at_this_configuration.cpp @@ -0,0 +1,17 @@ +#include "fcl/narrowphase/detail/failed_at_this_configuration.h" + +#include + +namespace fcl { +namespace detail { + +void ThrowFailedAtThisConfiguration(const std::string& message, + const char* func, + const char* file, int line) { + std::stringstream ss; + ss << file << ":(" << line << "): " << func << "(): " << message; + throw FailedAtThisConfiguration(ss.str()); +} + +} // namespace detail +} // namespace fcl diff --git a/test/expect_throws_message.h b/test/expect_throws_message.h new file mode 100644 index 000000000..3932492f2 --- /dev/null +++ b/test/expect_throws_message.h @@ -0,0 +1,143 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018. 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 Open Source Robotics Foundation 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. + */ + +// This code was taken from Drake. +// https://github.com/RobotLocomotion/drake/blob/master/common/test_utilities/expect_throws_message.h + +#ifndef FCL_EXPECT_THROWS_MESSAGE_H +#define FCL_EXPECT_THROWS_MESSAGE_H + +#include +#include + +#ifdef FCL_DOXYGEN_CXX + +/** Unit test helper macro for "expecting" an exception to be thrown but also +testing the error message against a provided regular expression. This is +like GTest's `EXPECT_THROW` but is fussier about the particular error message. +Usage example: @code + FCL_EXPECT_THROWS_MESSAGE( + StatementUnderTest(), // You expect this statement to throw ... + std::logic_error, // ... this exception with ... + ".*some important.*phrases.*that must appear.*"); // ... this message. +@endcode +The regular expression must match the entire error message. If there is +boilerplate you don't care to match at the beginning and end, surround with +`.*` to ignore. + +Following GTest's conventions, failure to perform as expected here is a +non-fatal test error. An `ASSERT` variant is provided to make it fatal. There +are also `*_IF_ARMED` variants. These require an exception in Debug builds. In +Release builds, the expression will pass if it _doesn't_ throw or if it throws +an exception that would pass the same test as in Debug builds. There is no +mechanism for testing _exclusive_ throwing behavior (i.e., only throws in +Debug). +@see FCL_ASSERT_THROWS_MESSAGE +@see FCL_EXPECT_THROWS_MESSAGE_IF_ARMED, FCL_ASSERT_THROWS_MESSAGE_IF_ARMED */ +#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) + +/** Fatal error version of `FCL_EXPECT_THROWS_MESSAGE`. +@see FCL_EXPECT_THROWS_MESSAGE */ +#define FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) + +/** Same as `FCL_EXPECT_THROWS_MESSAGE` in Debug builds, but doesn't require +a throw in Release builds. However, if the Release build does throw it must +throw the right message. More precisely, the thrown message is required +whenever `FCL_ENABLE_ASSERTS` is defined, which Debug builds do by default. +@see FCL_EXPECT_THROWS_MESSAGE */ +#define FCL_EXPECT_THROWS_MESSAGE_IF_ARMED(expression, exception, regexp) + +/** Same as `FCL_ASSERT_THROWS_MESSAGE` in Debug builds, but doesn't require +a throw in Release builds. However, if the Release build does throw it must +throw the right message. More precisely, the thrown message is required +whenever `FCL_ENABLE_ASSERTS` is defined, which Debug builds do by default. +@see FCL_ASSERT_THROWS_MESSAGE */ +#define FCL_ASSERT_THROWS_MESSAGE_IF_ARMED(expression, exception, regexp) + +#else // FCL_DOXYGEN_CXX + +#define FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + must_throw, fatal_failure) \ +try { \ + expression; \ + if (must_throw) { \ + if (fatal_failure) { \ + GTEST_FATAL_FAILURE_("\t" #expression " failed to throw " #exception); \ + } else { \ + GTEST_NONFATAL_FAILURE_("\t" #expression " failed to throw " #exception);\ + } \ + } \ +} catch (const exception& err) { \ + auto matcher = [](const char* s, const std::string& re) { \ + return std::regex_match(s, std::regex(re)); }; \ + if (fatal_failure) { \ + ASSERT_PRED2(matcher, err.what(), regexp); \ + } else { \ + EXPECT_PRED2(matcher, err.what(), regexp); \ + } \ +} + +#define FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + true /*must_throw*/, false /*non-fatal*/) + +#define FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + true /*must_throw*/, true /*fatal*/) + +#ifdef NDEBUG +// Throwing the expected message is optional in this case. + +#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + false /*optional*/, false /*non-fatal*/) + +#define FCL_ASSERT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE_HELPER(expression, exception, regexp, \ + false /*optional*/, true /*fatal*/) + +#else // NDEBUG +// Throwing the expected message is required in this case. + +#define FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_EXPECT_THROWS_MESSAGE(expression, exception, regexp) + +#define FCL_ASSERT_THROWS_MESSAGE_IF_DEBUG(expression, exception, regexp) \ + FCL_ASSERT_THROWS_MESSAGE(expression, exception, regexp) + +#endif // NDEBUG + +#endif // FCL_DOXYGEN_CXX + +#endif // FCL_EXPECT_THROWS_MESSAGE_H diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp index 248bfb7fb..b7c2a2822 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp +++ b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp @@ -47,6 +47,7 @@ #include #include "fcl/narrowphase/detail/convexity_based_algorithm/polytope.h" +#include "expect_throws_message.h" namespace fcl { namespace detail { @@ -140,6 +141,25 @@ std::array, 4> EquilateralTetrahedronVertices( return vertices; } +// Produces a corrupted equilateral tetrahedron, but moves the top vertex to be +// on one of the bottom face's edges. +std::array, 4> TetrahedronColinearVertices() { + std::array, 4> vertices = EquilateralTetrahedronVertices( + 0, 0, 0, 1); + vertices[3] = (vertices[0] + vertices[1]) / 2; + return vertices; +} + +// Produces a corrupted equilateral tetrahedron, but the bottom face is shrunk +// down too small to be trusted. +std::array, 4> TetrahedronSmallFaceVertices() { + std::array, 4> vertices = EquilateralTetrahedronVertices( + 0, 0, 0, 1); + const ccd_real_t delta = constants::eps() / 4; + for (int i = 0; i < 3; ++i) vertices[i] *= delta; + return vertices; +} + /** Simple equilateral tetrahedron. @@ -275,6 +295,34 @@ GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardOriginNearFace2) { CheckTetrahedronFaceNormal(p); } +// Tests the error condition for this operation -- i.e., a degenerate triangle +// in a polytope. +GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutwardError) { + { + Tetrahedron bad_tet(TetrahedronColinearVertices()); + + // Degenerate triangle (in this case, co-linear vertices) in polytope. + // By construction, face 1 is the triangle that has been made degenerate. + FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG( + libccd_extension::faceNormalPointingOutward(&bad_tet.polytope(), + &bad_tet.f(1)), + FailedAtThisConfiguration, + ".*faceNormalPointingOutward.*zero-area.*"); + } + + { + Tetrahedron bad_tet(TetrahedronSmallFaceVertices()); + + // Degenerate triangle (in this case, a face is too small) in polytope. + // By construction, face 1 is the triangle that has been made degenerate. + FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG( + libccd_extension::faceNormalPointingOutward(&bad_tet.polytope(), + &bad_tet.f(1)), + FailedAtThisConfiguration, + ".*faceNormalPointingOutward.*zero-area.*"); + } +} + GTEST_TEST(FCL_GJK_EPA, supportEPADirection) { auto CheckSupportEPADirection = []( const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt, @@ -317,7 +365,7 @@ GTEST_TEST(FCL_GJK_EPA, supportEPADirection) { EXPECT_THROW( libccd_extension::supportEPADirection( &p3.polytope(), reinterpret_cast(&p3.v(0))), - std::logic_error); + FailedAtThisConfiguration); // Origin is an internal point of the bottom triangle EquilateralTetrahedron p4(0, 0, 0); @@ -333,6 +381,18 @@ GTEST_TEST(FCL_GJK_EPA, supportEPADirection) { Vector3(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol); } +GTEST_TEST(FCL_GJK_EPA, supportEPADirectionError) { + EquilateralTetrahedron tet; + // Note: the exception is only thrown if the nearest feature's disatance is + // zero. + tet.v(1).dist = 0; + FCL_EXPECT_THROWS_MESSAGE( + libccd_extension::supportEPADirection( + &tet.polytope(), reinterpret_cast(&tet.v(1))), + FailedAtThisConfiguration, + ".+supportEPADirection.+nearest point to the origin is a vertex.+"); +} + GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { EquilateralTetrahedron p; @@ -370,142 +430,37 @@ GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) { CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside); } -#ifndef NDEBUG - -/** A degenerate tetrahedron due to vertices considered to be coincident. - It is, strictly speaking a valid tetrahedron, but the points are so close that - the calculations on edge lengths cannot be trusted. - - More particularly, one face is *very* small but the other three faces are quite - long with horrible aspect ratio. - - Vertices v0, v1, and v2 are all close to each other, v3 is distant. - Edges e0, e1, and e2 connect vertices (v0, v1, and v2) and, as such, have very - short length. Edges e3, e4, and e5 connect to the distance vertex and have - long length. - - Face 0 is the small face. Faces 1-3 all include one edge of the small face. - - All faces should be considered degenerate due to coincident points. */ -class CoincidentTetrahedron : public Polytope { - public: - CoincidentTetrahedron() : Polytope() { - const ccd_real_t delta = constants::eps() / 4; - v().resize(4); - e().resize(6); - f().resize(4); - auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y, - ccd_real_t z) { - return ccdPtAddVertexCoords(&this->polytope(), x, y, z); - }; - v()[0] = AddTetrahedronVertex(0.5, delta, delta); - v()[1] = AddTetrahedronVertex(0.5, -delta, delta); - v()[2] = AddTetrahedronVertex(0.5, -delta, -delta); - v()[3] = AddTetrahedronVertex(-0.5, 0, 0); - e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); - e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); - e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); - e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); - e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); - e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); - f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); - f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); - f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); - f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); - } -}; - -// Tests against a polytope with a face where all the points are too close to -// distinguish. -GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident0) { - ::testing::FLAGS_gtest_death_test_style = "threadsafe"; - CoincidentTetrahedron p; - +// Tests against a degenerate polytope. +GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFaceError) { // The test point doesn't matter; it'll never get that far. // NOTE: For platform compatibility, the assertion message is pared down to // the simplest component: the actual function call in the assertion. ccd_vec3_t pt{{10, 10, 10}}; - ASSERT_DEATH( - libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), &pt), - ".*!triangle_area_is_zero.*"); -} - -// Tests against a polytope with a face where *two* points are too close to -// distinguish. -GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident1) { - ::testing::FLAGS_gtest_death_test_style = "threadsafe"; - CoincidentTetrahedron p; - // The test point doesn't matter; it'll never get that far. - // NOTE: For platform compatibility, the assertion message is pared down to - // the simplest component: the actual function call in the assertion. - ccd_vec3_t pt{{10, 10, 10}}; - ASSERT_DEATH( - libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt), - ".*!triangle_area_is_zero.*"); -} - -/** A degenerate tetrahedron due to vertices considered to be colinear. - It is, strictly speaking a valid tetrahedron, but the vertices are so close to - being colinear, that the area can't meaningfully be computed. - - More particularly, one face is *very* large but the fourth vertex lies just - slightly off that plane *over* one of the edges. The face that is incident to - that edge and vertex will have colinear edges. - - Vertices v0, v1, and v2 are form the large triangle. v3 is the slightly - off-plane vertex. Edges e0, e1, and e2 connect vertices (v0, v1, and v2). v3 - projects onto edge e0. Edges e3 and e4 connect v0 and v1 to v3, respectively. - Edges e3 and e4 are colinear. Edge e5 is the remaining, uninteresting edge. - Face 0 is the large triangle. - Face 1 is the bad face. Faces 2 and 3 are irrelevant. */ -class ColinearTetrahedron : public Polytope { - public: - ColinearTetrahedron() : Polytope() { - const ccd_real_t delta = constants::eps() / 100; - v().resize(4); - e().resize(6); - f().resize(4); - auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y, - ccd_real_t z) { - return ccdPtAddVertexCoords(&this->polytope(), x, y, z); - }; - v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), -1); - v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), -1); - v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), -1); - // This point should lie *slightly* above the edge connecting v0 and v1. - v()[3] = AddTetrahedronVertex(0, -0.5 / std::sqrt(3), -1 + delta); - - e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1)); - e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2)); - e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0)); - e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3)); - e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3)); - e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3)); - f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2)); - f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4)); - f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5)); - f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2)); + { + Tetrahedron bad_tet(TetrahedronColinearVertices()); + + // Degenerate triangle (in this case, co-linear vertices) in polytope. + // By construction, face 1 is the triangle that has been made degenerate. + FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG( + libccd_extension::isOutsidePolytopeFace(&bad_tet.polytope(), + &bad_tet.f(1), &pt), + FailedAtThisConfiguration, + ".*faceNormalPointingOutward.*zero-area.*"); } -}; -// Tests against a polytope with a face where two edges are colinear. -GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Colinear) { - ::testing::FLAGS_gtest_death_test_style = "threadsafe"; - ColinearTetrahedron p; - - // This test point should pass w.r.t. the big face. - ccd_vec3_t pt{{0, 0, -10}}; - EXPECT_TRUE( - libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), &pt)); - // Face 1, however, is definitely colinear. - // NOTE: For platform compatibility, the assertion message is pared down to - // the simplest component: the actual function call in the assertion. - ASSERT_DEATH( - libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt), - ".*!triangle_area_is_zero.*"); + { + Tetrahedron bad_tet(TetrahedronSmallFaceVertices()); + + // Degenerate triangle (in this case, a face is too small) in polytope. + // By construction, face 1 is the triangle that has been made degenerate. + FCL_EXPECT_THROWS_MESSAGE_IF_DEBUG( + libccd_extension::isOutsidePolytopeFace(&bad_tet.polytope(), + &bad_tet.f(0), &pt), + FailedAtThisConfiguration, + ".*faceNormalPointingOutward.*zero-area.*"); + } } -#endif // Construct a polytope with the following shape, namely an equilateral triangle // on the top, and an equilateral triangle of the same size, but rotate by 60 @@ -735,6 +690,71 @@ GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) { {0, 1}, {0}); } +// Tests that the sanity check causes `ComputeVisiblePatch()` to throw in +// debug builds. +GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchSanityCheck) { +#ifndef NDEBUG + // NOTE: The sanity check function only gets compiled in debug mode. + EquilateralTetrahedron tet; + std::unordered_set border_edges; + std::unordered_set visible_faces; + std::unordered_set internal_edges; + + // Top view labels of vertices, edges and faces. + // . + // v2 . + // /\ /\ . + // / |\ / |\ . + // /e5| \ / | \ . + // e2 / | \ / |f2\ . + // / ╱╲v3 \e1 /f3 ╱╲ \ // f0 is the bottom face. . + // / ╱ ╲ \ / ╱ ╲ \ . + // / ╱e3 e4╲ \ / ╱ f1 ╲ \ . + // /╱____________╲\ /╱____________╲\ . + // v0 e0 v1 . + // + // The tet is centered on the origin, pointing upwards (seen from above in + // the diagram above. We define a point above this to define the visible patch + // The *correct* patch should have the following: + // visible faces: f1, f2, f3 + // internal edges: e3, e4, e5 + // border edges: e0, e1, e2 + auto set_ideal = [&border_edges, &internal_edges, &visible_faces, &tet]() { + border_edges = + std::unordered_set{&tet.e(0), &tet.e(1), &tet.e(2)}; + internal_edges = + std::unordered_set{&tet.e(3), &tet.e(4), &tet.e(5)}; + visible_faces = + std::unordered_set{&tet.f(1), &tet.f(2), &tet.f(3)}; + }; + + set_ideal(); + EXPECT_TRUE(libccd_extension::ComputeVisiblePatchRecursiveSanityCheck( + tet.polytope(), border_edges, visible_faces, internal_edges)); + + // Failure conditions: + // Two adjacent faces have edge e --> e is internal edge. + set_ideal(); + internal_edges.erase(&tet.e(5)); + EXPECT_FALSE(libccd_extension::ComputeVisiblePatchRecursiveSanityCheck( + tet.polytope(), border_edges, visible_faces, internal_edges)); + + // Edge in internal edge --> two adjacent faces visible. + set_ideal(); + visible_faces.erase(&tet.f(3)); + EXPECT_FALSE(libccd_extension::ComputeVisiblePatchRecursiveSanityCheck( + tet.polytope(), border_edges, visible_faces, internal_edges)); + + // Edge in border_edges --> one (and only one) visible face. + set_ideal(); + internal_edges.erase(&tet.e(5)); + border_edges.insert(&tet.e(5)); + EXPECT_FALSE(libccd_extension::ComputeVisiblePatchRecursiveSanityCheck( + tet.polytope(), border_edges, visible_faces, internal_edges)); + +#endif // NDEBUG +} + // Returns true if the the distance difference between the two vertices are // below tol. bool VertexPositionCoincide(const ccd_pt_vertex_t* v1, @@ -1102,6 +1122,30 @@ GTEST_TEST(FCL_GJK_EPA, expandPolytope_hexagram_4_visible_faces) { constants::eps_34()); } +GTEST_TEST(FCL_GJK_EPA, expandPolytope_error) { + EquilateralTetrahedron tet; + ccd_support_t newv; + + // Error condition 1: expanding with the nearest feature being a vertex. + FCL_EXPECT_THROWS_MESSAGE( + libccd_extension::expandPolytope( + &tet.polytope(), reinterpret_cast(&tet.v(0)), &newv), + FailedAtThisConfiguration, + ".*expandPolytope.*The visible feature is a vertex.*"); + + // Error condition 2: feature is edge, and vertex lies on the edge. + ccd_vec3_t nearest; + ccdVec3Copy(&nearest, &tet.v(0).v.v); + ccdVec3Add(&nearest, &tet.v(1).v.v); + ccdVec3Scale(&nearest, 0.5); + newv.v = nearest; + FCL_EXPECT_THROWS_MESSAGE( + libccd_extension::expandPolytope( + &tet.polytope(), reinterpret_cast(&tet.e(0)), &newv), + FailedAtThisConfiguration, + ".*expandPolytope.* nearest point and the new vertex are on an edge.*"); +} + void CompareCcdVec3(const ccd_vec3_t& v, const ccd_vec3_t& v_expected, ccd_real_t tol) { for (int i = 0; i < 3; ++i) {