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) {