diff --git a/CHANGELOG.md b/CHANGELOG.md index cfef96414..fb7aa0006 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,8 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- [API change] Renamed default convergence criterion from `VDB` to `Default` ([#556](https://github.com/humanoid-path-planner/hpp-fcl/pull/556)) +- Fixed EPA returning nans on cases where it could return an estimate of the normal and penetration depth. ([#556](https://github.com/humanoid-path-planner/hpp-fcl/pull/556)) - Fixed too low tolerance in GJK/EPA asserts ([#554](https://github.com/humanoid-path-planner/hpp-fcl/pull/554)) - Fixed `normal_and_nearest_points` test (no need to have Eigen 3.4) ([#553](https://github.com/humanoid-path-planner/hpp-fcl/pull/553)) - [#549](https://github.com/humanoid-path-planner/hpp-fcl/pull/549) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index b8be19068..aaf5a9bce 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -201,7 +201,8 @@ struct HPP_FCL_DLLAPI QueryRequest { /// @brief max number of iterations for EPA size_t epa_max_iterations; - /// @brief tolerance for EPA + /// @brief tolerance for EPA. + /// Note: setting EPA's tolerance to less than GJK's is not recommended. FCL_REAL epa_tolerance; /// @brief enable timings when performing collision/distance request @@ -221,7 +222,7 @@ struct HPP_FCL_DLLAPI QueryRequest { gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), gjk_tolerance(GJK_DEFAULT_TOLERANCE), gjk_variant(GJKVariant::DefaultGJK), - gjk_convergence_criterion(GJKConvergenceCriterion::VDB), + gjk_convergence_criterion(GJKConvergenceCriterion::Default), gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), epa_tolerance(EPA_DEFAULT_TOLERANCE), diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index a787a1439..cb8a0a128 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -89,7 +89,7 @@ enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; /// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and /// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe /// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. -enum GJKConvergenceCriterion { VDB, DualityGap, Hybrid }; +enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; /// @brief Wether the convergence criterion is scaled on the norm of the /// solution or not diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index 2bb040eae..63040d2ab 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -71,15 +71,17 @@ } #ifdef HPP_FCL_TURN_ASSERT_INTO_EXCEPTION -#define HPP_FCL_ASSERT(check, message, exception) \ - { \ - if (check == false) HPP_FCL_THROW_PRETTY(message, exception); \ - } +#define HPP_FCL_ASSERT(check, message, exception) \ + do { \ + if (!(check)) { \ + HPP_FCL_THROW_PRETTY(message, exception); \ + } \ + } while (0) #else #define HPP_FCL_ASSERT(check, message, exception) \ { \ HPP_FCL_UNUSED_VARIABLE(exception(message)); \ - assert(check&& message); \ + assert((check) && message); \ } #endif diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 317d053cb..e687a92cd 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -164,12 +164,22 @@ struct HPP_FCL_DLLAPI GJK { }; /// @brief Status of the GJK algorithm: - /// Valid: GJK converged and the shapes are not in collision. - /// Inside: GJK converged and the shapes are in collision. + /// DidNotRun: GJK has not been run. + /// Failed: GJK did not converge (it exceeded the maximum number of + /// iterations). + /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited + /// before converting. The shapes are not in collision. + /// NoCollision: GJK converged and the shapes are not in collision. + /// Collision: GJK converged and the shapes are in collision. /// Failed: GJK did not converge. - /// EarlyStopped: GJK found a separating hyperplane and exited before - /// converting. The shapes are not in collision. - enum Status { DidNotRun, Failed, Valid, Inside, EarlyStopped }; + enum Status { + DidNotRun, + Failed, + NoCollisionEarlyStopped, + NoCollision, + CollisionWithPenetrationInformation, + Collision + }; public: FCL_REAL distance_upper_bound; @@ -250,19 +260,11 @@ struct HPP_FCL_DLLAPI GJK { inline Simplex* getSimplex() const { return simplex; } /// Tells whether the closest points are available. - bool hasClosestPoints() { return distance < distance_upper_bound; } - - /// Tells whether the penetration information. - /// - /// In such case, most indepth points and penetration depth can be retrieved - /// from GJK. Calling EPA has an undefined behaviour. - bool hasPenetrationInformation(const MinkowskiDiff& shape) { - return distance > -shape.inflation.sum(); - } + bool hasClosestPoints() const { return distance < distance_upper_bound; } /// Get the closest points on each object. /// @return true on success - bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); + bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const; /// @brief get the guess from current simplex Vec3f getGuessFromSimplex() const; @@ -278,7 +280,7 @@ struct HPP_FCL_DLLAPI GJK { /// @brief Convergence check used to stop GJK when shapes are not in /// collision. bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega); + const FCL_REAL& omega) const; /// @brief Get the max number of iterations of GJK. size_t getNumMaxIterations() const { return max_iterations; } @@ -503,12 +505,6 @@ struct HPP_FCL_DLLAPI EPA { bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, const SimplexVertex& b, FCL_REAL& dist); - /// @brief Add a new face to the polytope; used at the beginning of EPA. - /// Note: sometimes the origin can be located outside EPA's starting polytope. - /// This is fine, we simply make sure to compute quantities in the right - /// normal direction to set the `ignore` flag correctly. - SimplexFace* createInitialPolytopeFace(size_t id_a, size_t id_b, size_t id_c); - /// @brief Add a new face to the polytope. /// This function sets the `ignore` flag to `true` if the origin does not /// project inside the face. diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 7cdb874b4..13f34bbf4 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -82,7 +82,8 @@ struct HPP_FCL_DLLAPI GJKSolver { if (normal != NULL) *normal = n; if (contact_points != NULL) *contact_points = 0.5 * (p1 + p2); } - return (gjk.status == details::GJK::Inside); + return (gjk.status == details::GJK::Collision || + gjk.status == details::GJK::CollisionWithPenetrationInformation); } //// @brief intersection checking between one shape and a triangle with @@ -106,7 +107,8 @@ struct HPP_FCL_DLLAPI GJKSolver { runGJKAndEPA(s, tf1, tri, tf_1M2, distance, compute_penetration, p1, p2, normal, relative_transformation_already_computed); HPP_FCL_UNUSED_VARIABLE(gjk_and_epa_ran_successfully); - return (gjk.status == details::GJK::Inside); + return (gjk.status == details::GJK::Collision || + gjk.status == details::GJK::CollisionWithPenetrationInformation); } /// @brief distance computation between two shapes. @@ -219,7 +221,7 @@ struct HPP_FCL_DLLAPI GJKSolver { normal); gjk_and_epa_ran_successfully = false; break; - case details::GJK::EarlyStopped: + case details::GJK::NoCollisionEarlyStopped: // // Case where GJK early stopped because the distance was found to be // above the `distance_upper_bound`. @@ -231,7 +233,7 @@ struct HPP_FCL_DLLAPI GJKSolver { "`distance_upper_bound`.", std::logic_error); break; - case details::GJK::Valid: + case details::GJK::NoCollision: // // Case where GJK converged and proved that the shapes are not in // collision, i.e their distance is above GJK's tolerance (default @@ -244,136 +246,115 @@ struct HPP_FCL_DLLAPI GJKSolver { "distance between the closest points.", std::logic_error); break; - case details::GJK::Inside: - // - // Case where GJK found the shapes to be in collision, i.e. their - // distance is below GJK's tolerance (default 1e-6). - if (gjk.hasPenetrationInformation(minkowski_difference)) { - // - // Case where the shapes are inflated (sphere or capsule). - // When the shapes are inflated, the GJK algorithm can provide the - // witness points and the normal. - GJKCollisionWithInflationExtractWitnessPointsAndNormal( - tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(distance <= gjk.getTolerance() + dummy_precision, - "The distance found by GJK should be negative (or at )" - "least below GJK's tolerance.", - std::logic_error); - // + because the distance is negative. - HPP_FCL_ASSERT(std::abs((p1 - p2).norm() + distance) <= - gjk.getTolerance() + dummy_precision, - "The distance found by GJK should coincide with the " - "distance between the closest points.", - std::logic_error); + // + // Next are the cases where GJK found the shapes to be in collision, i.e. + // their distance is below GJK's tolerance (default 1e-6). + case details::GJK::CollisionWithPenetrationInformation: + GJKCollisionWithPenetrationInfoExtractWitnessPointsAndNormal( + tf1, distance, p1, p2, normal); + HPP_FCL_ASSERT(distance <= gjk.getTolerance() + dummy_precision, + "The distance found by GJK should be negative (or at )" + "least below GJK's tolerance.", + std::logic_error); + break; + case details::GJK::Collision: + if (!compute_penetration) { + // Skip EPA and set the witness points and the normal to nans. + GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); } else { - if (!compute_penetration) { - GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - } else { + // + // Case where the shapes are not inflated (box, cylinder, cone, + // convex meshes etc.). We need to run the EPA algorithm to find the + // witness points, penetration depth and the normal. + + // Reset EPA algorithm. Potentially allocate memory if + // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's + // current storage. + epa.reset(epa_max_iterations, epa_tolerance); + + // TODO: understand why EPA's performance is so bad on cylinders and + // cones. + epa.evaluate(gjk, -guess); + + switch (epa.status) { // - // Case where the shapes are not inflated (box, cylinder, cone, - // convex meshes etc.). We need to run the EPA algorithm to find the - // witness points, penetration depth and the normal. - - // Reset EPA algorithm. Potentially allocate memory if - // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's - // current storage. - epa.reset(epa_max_iterations, epa_tolerance); - - // TODO: understand why EPA's performance is so bad on cylinders and - // cones. - epa.evaluate(gjk, -guess); - - switch (epa.status) { - // - // In the following switch cases, until the "Valid" case, - // EPA either ran out of iterations, of faces or of vertices. - // The depth, witness points and the normal are still valid, - // simply not at the precision of EPA's tolerance. - // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these - // cases. - // - // TODO: Remove OutOfFaces and OutOfVertices statuses and simply - // compute the upper bound on max faces and max vertices as a - // function of the number of iterations. - case details::EPA::OutOfFaces: - HPP_FCL_LOG_WARNING("EPA ran out of faces."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::OutOfVertices: - HPP_FCL_LOG_WARNING("EPA ran out of vertices."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::Failed: - HPP_FCL_LOG_WARNING("EPA ran out of iterations."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::Valid: - case details::EPA::AccuracyReached: - HPP_FCL_ASSERT( - -epa.depth <= epa.getTolerance() + dummy_precision, - "EPA's penetration distance should be negative (or " - "at least below EPA's tolerance).", - std::logic_error); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - break; - // - // In the following cases, EPA failed to run, in a bad way. - // The produced witness points, penetration depth and normal - // may make no sense. - case details::EPA::DidNotRun: - HPP_FCL_ASSERT(false, "EPA did not run. It should have!", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::Degenerated: - HPP_FCL_ASSERT( - false, "EPA created a polytope with a degenerated face.", - std::logic_error); - HPP_FCL_LOG_ERROR( - "EPA error: created a polytope with a degenerated face."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::NonConvex: - HPP_FCL_ASSERT(false, "EPA got called onto non-convex shapes.", - std::logic_error); - HPP_FCL_LOG_ERROR( - "EPA error: EPA got called onto non-convex shapes."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::InvalidHull: - HPP_FCL_ASSERT(false, "EPA created an invalid polytope.", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: created an invalid polytope."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::FallBack: - HPP_FCL_ASSERT( - false, - "EPA went into fallback mode. It should never do that.", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: FallBack."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - } + // In the following switch cases, until the "Valid" case, + // EPA either ran out of iterations, of faces or of vertices. + // The depth, witness points and the normal are still valid, + // simply not at the precision of EPA's tolerance. + // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these + // cases. + // + // TODO: Remove OutOfFaces and OutOfVertices statuses and simply + // compute the upper bound on max faces and max vertices as a + // function of the number of iterations. + case details::EPA::OutOfFaces: + HPP_FCL_LOG_WARNING("EPA ran out of faces."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::OutOfVertices: + HPP_FCL_LOG_WARNING("EPA ran out of vertices."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::Failed: + HPP_FCL_LOG_WARNING("EPA ran out of iterations."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::Valid: + case details::EPA::AccuracyReached: + HPP_FCL_ASSERT( + -epa.depth <= epa.getTolerance() + dummy_precision, + "EPA's penetration distance should be negative (or " + "at least below EPA's tolerance).", + std::logic_error); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + break; + case details::EPA::Degenerated: + HPP_FCL_LOG_WARNING( + "EPA warning: created a polytope with a degenerated face."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::NonConvex: + HPP_FCL_LOG_WARNING( + "EPA warning: EPA got called onto non-convex shapes."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::InvalidHull: + HPP_FCL_LOG_WARNING("EPA warning: created an invalid polytope."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::DidNotRun: + HPP_FCL_ASSERT(false, "EPA did not run. It should have!", + std::logic_error); + HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::FallBack: + HPP_FCL_ASSERT( + false, + "EPA went into fallback mode. It should never do that.", + std::logic_error); + HPP_FCL_LOG_ERROR("EPA error: FallBack."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; } } break; // End of case details::GJK::Inside @@ -426,7 +407,7 @@ struct HPP_FCL_DLLAPI GJKSolver { p2 = tf1.transform(p2); } - void GJKCollisionWithInflationExtractWitnessPointsAndNormal( + void GJKCollisionWithPenetrationInfoExtractWitnessPointsAndNormal( const Transform3f& tf1, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { distance = gjk.distance; @@ -441,6 +422,7 @@ struct HPP_FCL_DLLAPI GJKSolver { FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + HPP_FCL_UNUSED_VARIABLE(tf1); const FCL_REAL dummy_precision = 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); @@ -449,9 +431,8 @@ struct HPP_FCL_DLLAPI GJKSolver { std::logic_error); distance = gjk.distance; gjk.getClosestPoints(minkowski_difference, p1, p2); - p1 = tf1.transform(p1); - p2 = tf1.transform(p2); - normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); + p1 = p2 = normal = + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } void EPAValidExtractWitnessPointsAndNormal(const Transform3f& tf1, @@ -539,7 +520,7 @@ struct HPP_FCL_DLLAPI GJKSolver { // Default settings for GJK algorithm gjk.gjk_variant = GJKVariant::DefaultGJK; - gjk.convergence_criterion = GJKConvergenceCriterion::VDB; + gjk.convergence_criterion = GJKConvergenceCriterion::Default; gjk.convergence_criterion_type = GJKConvergenceCriterionType::Relative; } diff --git a/python/gjk.cc b/python/gjk.cc index 91b8750e9..f1d68afb4 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -53,9 +53,13 @@ using hpp::fcl::details::MinkowskiDiff; void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKStatus") - .value("Valid", GJK::Valid) - .value("Inside", GJK::Inside) - .value("Failed", GJK::Failed) + .value("Failed", GJK::Status::Failed) + .value("DidNotRun", GJK::Status::DidNotRun) + .value("NoCollision", GJK::Status::NoCollision) + .value("NoCollisionEarlyStopped", GJK::Status::NoCollisionEarlyStopped) + .value("CollisionWithPenetrationInformation", + GJK::Status::CollisionWithPenetrationInformation) + .value("Collision", GJK::Status::Collision) .export_values(); } @@ -103,7 +107,7 @@ void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type< GJKConvergenceCriterion>()) { enum_("GJKConvergenceCriterion") - .value("VDB", GJKConvergenceCriterion::VDB) + .value("Default", GJKConvergenceCriterion::Default) .value("DualityGap", GJKConvergenceCriterion::DualityGap) .value("Hybrid", GJKConvergenceCriterion::Hybrid) .export_values(); @@ -133,7 +137,6 @@ void exposeGJK() { .DEF_CLASS_FUNC(GJK, getNumIterations) .DEF_CLASS_FUNC(GJK, getNumIterationsMomentumStopped) .DEF_CLASS_FUNC(GJK, hasClosestPoints) - .DEF_CLASS_FUNC(GJK, hasPenetrationInformation) .DEF_CLASS_FUNC(GJK, getClosestPoints) .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) .DEF_CLASS_FUNC(GJK, getGuessFromSimplex); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 6f880b670..93c35ef07 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -548,7 +548,7 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { void GJK::initialize() { distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; - convergence_criterion = GJKConvergenceCriterion::VDB; + convergence_criterion = GJKConvergenceCriterion::Default; convergence_criterion_type = GJKConvergenceCriterionType::Relative; reset(max_iterations, tolerance); } @@ -653,7 +653,8 @@ void inflate(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { } // namespace details -bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { +bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, + Vec3f& w1) const { bool res = details::getClosestPoints(*simplex, w0, w1); if (!res) return false; details::inflate(shape, w0, w1); @@ -673,7 +674,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, free_v[3] = &store_v[3]; nfree = 4; - status = Valid; + status = NoCollision; shape = &shape_; distance = 0.0; current = 0; @@ -700,15 +701,16 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, Simplex& next_simplex = simplices[next]; // check A: when origin is near the existing simplex, stop - // TODO this is an early stop which may cause the following issue. - // - EPA will not run correctly because it starts with a tetrahedron which - // does not include the origin. Note that, at this stage, we do not know - // whether a tetrahedron including the origin exists. if (rl < tolerance) // mean origin is near the face of original simplex, // return touch { + // At this point, GJK has converged but we don't know if GJK is enough to + // recover penetration information. + // EPA needs to be run. + // Unless the Minkowski difference is degenerated, EPA will run fine even + // if the final simplex of GJK is not a tetrahedron. assert(rl > 0); - status = Inside; + status = Collision; distance = -inflation; // should we take rl into account ? break; } @@ -759,7 +761,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, FCL_REAL omega = dir.dot(w) / dir.norm(); if (omega > upper_bound) { distance = omega - inflation; - status = EarlyStopped; + status = NoCollisionEarlyStopped; break; } @@ -777,9 +779,6 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // check C: when the new support point is close to the sub-simplex where the // ray point lies, stop (as the new simplex again is degenerated) bool cv_check_passed = checkConvergence(w, rl, alpha, omega); - // TODO here, we can stop at iteration 0 if this condition is met. - // We stopping at iteration 0, the closest point will not be valid. - // if(diff - tolerance * rl <= 0) if (iterations > 0 && cv_check_passed) { if (iterations > 0) removeVertex(simplices[current]); if (current_gjk_variant != DefaultGJK) { @@ -787,11 +786,16 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, iterations_momentum_stop = iterations; continue; } - distance = rl - inflation; // TODO When inflation is strictly positive, the distance may be exactly // zero (so the ray is not zero) and we are not in the case rl < // tolerance. - if (distance < tolerance) status = Inside; + + // At this point, GJK has converged and penetration information can always + // be recovered without running EPA. + distance = rl - inflation; + if (distance < tolerance) { + status = CollisionWithPenetrationInformation; + } break; } @@ -822,14 +826,14 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, current = next; if (!inside) rl = ray.norm(); if (inside || rl == 0) { - status = Inside; + status = Collision; distance = -inflation; break; } status = ((++iterations) < max_iterations) ? status : Failed; - } while (status == Valid); + } while (status == NoCollision); simplex = &simplices[current]; assert(simplex->rank > 0 && simplex->rank < 5); @@ -837,72 +841,58 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, } bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) { - FCL_REAL diff; - bool check_passed; + const FCL_REAL& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). // Each criterion provides a different guarantee on the distance to the // optimal solution. switch (convergence_criterion) { - case VDB: + case Default: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^*|| - ||x^k|| <= diff - diff = rl - alpha; - switch (convergence_criterion_type) { - case Absolute: - HPP_FCL_THROW_PRETTY("VDB convergence criterion is relative.", - std::logic_error); - break; - case Relative: - check_passed = (diff - (tolerance + tolerance * rl)) <= 0; - break; - default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); - } - break; + const FCL_REAL diff = rl - alpha; + return ((diff - (tolerance + tolerance * rl)) <= 0); + } break; - case DualityGap: + case DualityGap: { // ||x^* - x^k||^2 <= diff - diff = 2 * ray.dot(ray - w); + const FCL_REAL diff = 2 * ray.dot(ray - w); switch (convergence_criterion_type) { case Absolute: - check_passed = (diff - tolerance) <= 0; + return ((diff - tolerance) <= 0); break; case Relative: - check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0; + return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } - break; + } break; - case Hybrid: + case Hybrid: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^* - x^k||^2 <= diff - diff = rl * rl - alpha * alpha; + const FCL_REAL diff = rl * rl - alpha * alpha; switch (convergence_criterion_type) { case Absolute: - check_passed = (diff - tolerance) <= 0; + return ((diff - tolerance) <= 0); break; case Relative: - check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0; + return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } - break; + } break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); } - return check_passed; } inline void GJK::removeVertex(Simplex& simplex) { @@ -1133,6 +1123,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { next.rank = 4; \ return true; + // clang-format off if (ba_aa <= 0) { // if AB.AO >= 0 / a10 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 if (ba * da_ba + bd * ba_aa - bb * da_aa <= @@ -1483,6 +1474,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { } // end of AD.AO >= 0 } // end of AC.AO >= 0 } // end of AB.AO >= 0 + // clang-format on #undef REGION_INSIDE return false; @@ -1543,60 +1535,6 @@ bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, return false; } -EPA::SimplexFace* EPA::createInitialPolytopeFace(size_t id_a, size_t id_b, - size_t id_c) { - if (stock.root != nullptr) { - SimplexFace* face = stock.root; - stock.remove(face); - hull.append(face); - face->pass = 0; - face->vertex_id[0] = id_a; - face->vertex_id[1] = id_b; - face->vertex_id[2] = id_c; - const SimplexVertex& a = sv_store[id_a]; - const SimplexVertex& b = sv_store[id_b]; - const SimplexVertex& c = sv_store[id_c]; - face->n = (b.w - a.w).cross(c.w - a.w); - - if (face->n.norm() > Eigen::NumTraits::epsilon()) { - face->n.normalize(); - // When creating the original polytope out of GJK's simplex, - // it's possible the origin lies outside the polytope. - // This is fine, but to check if the origin lies inside/outside the face, - // we simply need to do it in the right direction. - Vec3f n = face->n; - if (face->n.dot(a.w) < -tolerance || // - face->n.dot(b.w) < -tolerance || // - face->n.dot(c.w) < -tolerance) { - n = -face->n; - } - - FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(n)); - FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(n)); - FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(n)); - if (!(a_dot_nab < -tolerance || // - b_dot_nbc < -tolerance || // - c_dot_nca < -tolerance)) { - face->d = a.w.dot(face->n); - face->ignore = false; - } else { - face->d = std::numeric_limits::max(); - face->ignore = true; - } - return face; - } else - status = Degenerated; - - hull.remove(face); - stock.append(face); - return nullptr; - } - - assert(hull.count >= fc_store.size() && "EPA: should not be out of faces."); - status = OutOfFaces; - return nullptr; -} - EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) { if (stock.root != nullptr) { SimplexFace* face = stock.root; @@ -1620,9 +1558,9 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) { FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); - if (!(a_dot_nab < -tolerance || // - b_dot_nbc < -tolerance || // - c_dot_nca < -tolerance)) { + if (a_dot_nab >= -tolerance && // + b_dot_nbc >= -tolerance && // + c_dot_nca >= -tolerance) { face->d = a.w.dot(face->n); face->ignore = false; } else { @@ -1700,10 +1638,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { sv_store[num_vertices++] = *simplex.vertex[i]; } - SimplexFace* tetrahedron[] = {createInitialPolytopeFace(0, 1, 2), // - createInitialPolytopeFace(1, 0, 3), // - createInitialPolytopeFace(2, 1, 3), // - createInitialPolytopeFace(0, 2, 3)}; + SimplexFace* tetrahedron[] = {newFace(0, 1, 2), // + newFace(1, 0, 3), // + newFace(2, 1, 3), // + newFace(0, 2, 3)}; if (hull.count == 4) { // set the face connectivity @@ -1882,9 +1820,7 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, // Uncomment the following line and the associated EPA method // to debug the infinite loop if needed. // EPAPrintExpandLooping(this, f); - if (f == closest_face) { - assert(false && "EPA is looping indefinitely."); - } + assert(f != closest_face && "EPA is looping indefinitely."); status = InvalidHull; return false; } diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 3a8b338bc..80ddd5916 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -489,7 +489,7 @@ bool GJKSolver::shapeDistance( gjk.getClosestPoints(minkowski_difference, p1, p2); - if ((gjk_status == details::GJK::Valid) || + if ((gjk_status == details::GJK::NoCollision) || (gjk_status == details::GJK::Failed)) { // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray @@ -497,7 +497,7 @@ bool GJKSolver::shapeDistance( dist = gjk.distance; return true; - } else if (gjk_status == details::GJK::Inside) { + } else if (gjk_status == details::GJK::Collision) { if (enable_penetration) { FCL_REAL penetrationDepth = details::computePenetration( t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); diff --git a/test/gjk.cpp b/test/gjk.cpp index 82deaaf8d..11b86bf26 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -339,10 +339,12 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); - if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Inside); - else - BOOST_CHECK_EQUAL(status, details::GJK::Valid); + if (expect_collision) { + BOOST_CHECK((status == details::GJK::Collision) || + (status == details::GJK::CollisionWithPenetrationInformation)); + } else { + BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); + } Vec3f w0, w1; gjk.getClosestPoints(shape, w0, w1); @@ -395,20 +397,22 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); - if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Inside); - else { - BOOST_CHECK_EQUAL(status, details::GJK::Valid); + if (expect_collision) { + BOOST_CHECK((status == details::GJK::Collision) || + (status == details::GJK::CollisionWithPenetrationInformation)); + } else { + BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected Vec3f guess = gjk.getGuessFromSimplex(); details::GJK gjk2(3, 1e-6); details::GJK::Status status2 = gjk2.evaluate(shape, guess); - BOOST_CHECK_EQUAL(status2, details::GJK::Valid); + BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision); } Vec3f w0, w1; - if (status == details::GJK::Valid || gjk.hasPenetrationInformation(shape)) { + if (status == details::GJK::NoCollision || + status == details::GJK::CollisionWithPenetrationInformation) { gjk.getClosestPoints(shape, w0, w1); } else { details::EPA epa(64, 1e-6); diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 4fc5db68e..ae315540a 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -64,11 +64,12 @@ BOOST_AUTO_TEST_CASE(set_cv_criterion) { GJK gjk(128, 1e-6); // Checking defaults - BOOST_CHECK(solver.gjk.convergence_criterion == GJKConvergenceCriterion::VDB); + BOOST_CHECK(solver.gjk.convergence_criterion == + GJKConvergenceCriterion::Default); BOOST_CHECK(solver.gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative); - BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::VDB); + BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::Default); BOOST_CHECK(gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative);