narrowphase.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2011-2014, Willow Garage, Inc.
 * Copyright (c) 2014-2015, Open Source Robotics Foundation
 * Copyright (c) 2018-2019, Centre National de la Recherche Scientifique
 * All rights reserved.
 * Copyright (c) 2021-2024, INRIA
 * 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.
 */


#ifndef COAL_NARROWPHASE_H
#define COAL_NARROWPHASE_H

#include <limits>

#include "coal/narrowphase/gjk.h"
#include "coal/collision_data.h"
#include "coal/narrowphase/narrowphase_defaults.h"
#include "coal/logging.h"

namespace coal {

struct COAL_DLLAPI GJKSolver {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 mutable details::GJK gjk;

 size_t gjk_max_iterations;

 CoalScalar gjk_tolerance;

 GJKInitialGuess gjk_initial_guess;

 COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
 bool enable_cached_guess;

 mutable Vec3s cached_guess;

 mutable support_func_guess_t support_func_cached_guess;

 CoalScalar distance_upper_bound;

 GJKVariant gjk_variant;

 GJKConvergenceCriterion gjk_convergence_criterion;

 GJKConvergenceCriterionType gjk_convergence_criterion_type;

 mutable details::EPA epa;

 size_t epa_max_iterations;

 CoalScalar epa_tolerance;

 mutable details::MinkowskiDiff minkowski_difference;

 private:
 // Used internally for assertion checks.
 static constexpr CoalScalar m_dummy_precision = 1e-6;

 public:
 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 GJKSolver()
 : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE),
 gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
 gjk_tolerance(GJK_DEFAULT_TOLERANCE),
 gjk_initial_guess(GJKInitialGuess::DefaultGuess),
 enable_cached_guess(false), // Use gjk_initial_guess instead
 cached_guess(Vec3s(1, 0, 0)),
 support_func_cached_guess(support_func_guess_t::Zero()),
 distance_upper_bound((std::numeric_limits<CoalScalar>::max)()),
 gjk_variant(GJKVariant::DefaultGJK),
 gjk_convergence_criterion(GJKConvergenceCriterion::Default),
 gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute),
 epa(0, EPA_DEFAULT_TOLERANCE),
 epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
 epa_tolerance(EPA_DEFAULT_TOLERANCE) {}

 explicit GJKSolver(const DistanceRequest& request)
 : gjk(request.gjk_max_iterations, request.gjk_tolerance),
 epa(0, request.epa_tolerance) {
 this->cached_guess = Vec3s(1, 0, 0);
 this->support_func_cached_guess = support_func_guess_t::Zero();

 set(request);
 }

 void set(const DistanceRequest& request) {
 // ---------------------
 // GJK settings
 this->gjk_initial_guess = request.gjk_initial_guess;
 this->enable_cached_guess = request.enable_cached_gjk_guess;
 if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
 this->enable_cached_guess) {
 this->cached_guess = request.cached_gjk_guess;
 this->support_func_cached_guess = request.cached_support_func_guess;
 }
 this->gjk_max_iterations = request.gjk_max_iterations;
 this->gjk_tolerance = request.gjk_tolerance;
 // For distance computation, we don't want GJK to early stop
 this->distance_upper_bound = (std::numeric_limits<CoalScalar>::max)();
 this->gjk_variant = request.gjk_variant;
 this->gjk_convergence_criterion = request.gjk_convergence_criterion;
 this->gjk_convergence_criterion_type =
 request.gjk_convergence_criterion_type;

 // ---------------------
 // EPA settings
 this->epa_max_iterations = request.epa_max_iterations;
 this->epa_tolerance = request.epa_tolerance;

 // ---------------------
 // Reset GJK and EPA status
 this->epa.status = details::EPA::Status::DidNotRun;
 this->gjk.status = details::GJK::Status::DidNotRun;
 }

 explicit GJKSolver(const CollisionRequest& request)
 : gjk(request.gjk_max_iterations, request.gjk_tolerance),
 epa(0, request.epa_tolerance) {
 this->cached_guess = Vec3s(1, 0, 0);
 this->support_func_cached_guess = support_func_guess_t::Zero();

 set(request);
 }

 void set(const CollisionRequest& request) {
 // ---------------------
 // GJK settings
 this->gjk_initial_guess = request.gjk_initial_guess;
 this->enable_cached_guess = request.enable_cached_gjk_guess;
 if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
 this->enable_cached_guess) {
 this->cached_guess = request.cached_gjk_guess;
 this->support_func_cached_guess = request.cached_support_func_guess;
 }
 this->gjk_tolerance = request.gjk_tolerance;
 this->gjk_max_iterations = request.gjk_max_iterations;
 // The distance upper bound should be at least greater to the requested
 // security margin. Otherwise, we will likely miss some collisions.
 this->distance_upper_bound = (std::max)(
 0., (std::max)(request.distance_upper_bound, request.security_margin));
 this->gjk_variant = request.gjk_variant;
 this->gjk_convergence_criterion = request.gjk_convergence_criterion;
 this->gjk_convergence_criterion_type =
 request.gjk_convergence_criterion_type;

 // ---------------------
 // EPA settings
 this->epa_max_iterations = request.epa_max_iterations;
 this->epa_tolerance = request.epa_tolerance;

 // ---------------------
 // Reset GJK and EPA status
 this->gjk.status = details::GJK::Status::DidNotRun;
 this->epa.status = details::EPA::Status::DidNotRun;
 }

 GJKSolver(const GJKSolver& other) = default;

 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 bool operator==(const GJKSolver& other) const {
 return this->enable_cached_guess ==
 other.enable_cached_guess && // use gjk_initial_guess instead
 this->cached_guess == other.cached_guess &&
 this->support_func_cached_guess == other.support_func_cached_guess &&
 this->gjk_max_iterations == other.gjk_max_iterations &&
 this->gjk_tolerance == other.gjk_tolerance &&
 this->distance_upper_bound == other.distance_upper_bound &&
 this->gjk_variant == other.gjk_variant &&
 this->gjk_convergence_criterion == other.gjk_convergence_criterion &&
 this->gjk_convergence_criterion_type ==
 other.gjk_convergence_criterion_type &&
 this->gjk_initial_guess == other.gjk_initial_guess &&
 this->epa_max_iterations == other.epa_max_iterations &&
 this->epa_tolerance == other.epa_tolerance;
 }
 COAL_COMPILER_DIAGNOSTIC_POP

 bool operator!=(const GJKSolver& other) const { return !(*this == other); }

 CoalScalar getDistancePrecision(const bool compute_penetration) const {
 return compute_penetration
 ? (std::max)(this->gjk_tolerance, this->epa_tolerance)
 : this->gjk_tolerance;
 }

 template <typename S1, typename S2>
 CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2,
 const Transform3s& tf2,
 const bool compute_penetration, Vec3s& p1, Vec3s& p2,
 Vec3s& normal) const {
 constexpr bool relative_transformation_already_computed = false;
 CoalScalar distance;
 this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
 normal, relative_transformation_already_computed);
 return distance;
 }

 template <typename S1>
 CoalScalar shapeDistance(const S1& s1, const Transform3s& tf1,
 const TriangleP& s2, const Transform3s& tf2,
 const bool compute_penetration, Vec3s& p1, Vec3s& p2,
 Vec3s& normal) const {
 const Transform3s tf_1M2(tf1.inverseTimes(tf2));
 TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
 tf_1M2.transform(s2.c));

 constexpr bool relative_transformation_already_computed = true;
 CoalScalar distance;
 this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1,
 p2, normal, relative_transformation_already_computed);
 return distance;
 }

 template <typename S2>
 CoalScalar shapeDistance(const TriangleP& s1, const Transform3s& tf1,
 const S2& s2, const Transform3s& tf2,
 const bool compute_penetration, Vec3s& p1, Vec3s& p2,
 Vec3s& normal) const {
 CoalScalar distance = this->shapeDistance<S2>(
 s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
 normal = -normal;
 return distance;
 }

 protected:
 template <typename S1, typename S2>
 void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess,
 support_func_guess_t& support_hint,
 const Vec3s& default_guess = Vec3s(1, 0, 0)) const {
 // There is no reason not to warm-start the support function, so we always
 // do it.
 support_hint = this->support_func_cached_guess;
 // The following switch takes care of the GJK warm-start.
 switch (gjk_initial_guess) {
 case GJKInitialGuess::DefaultGuess:
 guess = default_guess;
 break;
 case GJKInitialGuess::CachedGuess:
 guess = this->cached_guess;
 break;
 case GJKInitialGuess::BoundingVolumeGuess:
 if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
 COAL_THROW_PRETTY(
 "computeLocalAABB must have been called on the shapes before "
 "using "
 "GJKInitialGuess::BoundingVolumeGuess.",
 std::logic_error);
 }
 guess.noalias() =
 s1.aabb_local.center() -
 (this->minkowski_difference.oR1 * s2.aabb_local.center() +
 this->minkowski_difference.ot1);
 break;
 default:
 COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
 }
 // TODO: use gjk_initial_guess instead
 COAL_COMPILER_DIAGNOSTIC_PUSH
 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
 if (this->enable_cached_guess) {
 guess = this->cached_guess;
 }
 COAL_COMPILER_DIAGNOSTIC_POP
 }

 template <typename S1, typename S2,
 int _SupportOptions = details::SupportOptions::NoSweptSphere>
 void runGJKAndEPA(
 const S1& s1, const Transform3s& tf1, const S2& s2,
 const Transform3s& tf2, const bool compute_penetration,
 CoalScalar& distance, Vec3s& p1, Vec3s& p2, Vec3s& normal,
 const bool relative_transformation_already_computed = false) const {
 // Reset internal state of GJK algorithm
 if (relative_transformation_already_computed)
 this->minkowski_difference.set<_SupportOptions>(&s1, &s2);
 else
 this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2);
 this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance);
 this->gjk.setDistanceEarlyBreak(this->distance_upper_bound);
 this->gjk.gjk_variant = this->gjk_variant;
 this->gjk.convergence_criterion = this->gjk_convergence_criterion;
 this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type;
 this->epa.status = details::EPA::Status::DidNotRun;

 // Get initial guess for GJK: default, cached or bounding volume guess
 Vec3s guess;
 support_func_guess_t support_hint;
 getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
 *(this->minkowski_difference.shapes[1]), guess,
 support_hint);

 this->gjk.evaluate(this->minkowski_difference, guess, support_hint);

 switch (this->gjk.status) {
 case details::GJK::DidNotRun:
 COAL_ASSERT(false, "GJK did not run. It should have!",
 std::logic_error);
 this->cached_guess = Vec3s(1, 0, 0);
 this->support_func_cached_guess.setZero();
 distance = -(std::numeric_limits<CoalScalar>::max)();
 p1 = p2 = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 break;
 case details::GJK::Failed:
 //
 // GJK ran out of iterations.
 COAL_LOG_WARNING("GJK ran out of iterations.");
 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::GJK::NoCollisionEarlyStopped:
 //
 // Case where GJK early stopped because the distance was found to be
 // above the `distance_upper_bound`.
 // The two witness points have no meaning.
 GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
 normal);
 COAL_ASSERT(distance >= this->gjk.distance_upper_bound -
 this->m_dummy_precision,
 "The distance should be bigger than GJK's "
 "`distance_upper_bound`.",
 std::logic_error);
 break;
 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
 // 1e-6).
 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <=
 this->gjk.getTolerance() + this->m_dummy_precision,
 "The distance found by GJK should coincide with the "
 "distance between the closest points.",
 std::logic_error);
 break;
 //
 // 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:
 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 COAL_ASSERT(
 distance <= this->gjk.getTolerance() + this->m_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 {
 //
 // GJK was not enough to recover the penetration information.
 // 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.
 this->epa.reset(this->epa_max_iterations, this->epa_tolerance);

 // TODO: understand why EPA's performance is so bad on cylinders and
 // cones.
 this->epa.evaluate(this->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 `COAL_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:
 COAL_LOG_WARNING("EPA ran out of faces.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::OutOfVertices:
 COAL_LOG_WARNING("EPA ran out of vertices.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::Failed:
 COAL_LOG_WARNING("EPA ran out of iterations.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::Valid:
 case details::EPA::AccuracyReached:
 COAL_ASSERT(
 -epa.depth <= epa.getTolerance() + this->m_dummy_precision,
 "EPA's penetration distance should be negative (or "
 "at least below EPA's tolerance).",
 std::logic_error);
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::Degenerated:
 COAL_LOG_WARNING(
 "EPA warning: created a polytope with a degenerated face.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::NonConvex:
 COAL_LOG_WARNING(
 "EPA warning: EPA got called onto non-convex shapes.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::InvalidHull:
 COAL_LOG_WARNING("EPA warning: created an invalid polytope.");
 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
 break;
 case details::EPA::DidNotRun:
 COAL_ASSERT(false, "EPA did not run. It should have!",
 std::logic_error);
 COAL_LOG_ERROR("EPA error: did not run. It should have.");
 EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
 normal);
 break;
 case details::EPA::FallBack:
 COAL_ASSERT(
 false,
 "EPA went into fallback mode. It should never do that.",
 std::logic_error);
 COAL_LOG_ERROR("EPA error: FallBack.");
 EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
 normal);
 break;
 }
 }
 break; // End of case details::GJK::Collision
 }
 }

 void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1,
 CoalScalar& distance,
 Vec3s& p1, Vec3s& p2,
 Vec3s& normal) const {
 COAL_UNUSED_VARIABLE(tf1);
 // Cache gjk result for potential future call to this GJKSolver.
 this->cached_guess = this->gjk.ray;
 this->support_func_cached_guess = this->gjk.support_hint;

 distance = this->gjk.distance;
 p1 = p2 = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 // If we absolutely want to return some witness points, we could use
 // the following code (or simply merge the early stopped case with the
 // valid case below):
 // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal);
 // p1 = tf1.transform(p1);
 // p2 = tf1.transform(p2);
 // normal = tf1.getRotation() * normal;
 }

 void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1,
 CoalScalar& distance, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) const {
 // Apart from early stopping, there are two cases where GJK says there is no
 // collision:
 // 1. GJK proved the distance is above its tolerance (default 1e-6).
 // 2. GJK ran out of iterations.
 // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
 // it can safely be normalized.
 COAL_ASSERT(this->gjk.ray.norm() >
 this->gjk.getTolerance() - this->m_dummy_precision,
 "The norm of GJK's ray should be bigger than GJK's tolerance.",
 std::logic_error);
 // Cache gjk result for potential future call to this GJKSolver.
 this->cached_guess = this->gjk.ray;
 this->support_func_cached_guess = this->gjk.support_hint;

 distance = this->gjk.distance;
 // TODO: On degenerated case, the closest points may be non-unique.
 // (i.e. an object face normal is colinear to `gjk.ray`)
 gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2, normal);
 Vec3s p = tf1.transform(0.5 * (p1 + p2));
 normal = tf1.getRotation() * normal;
 p1.noalias() = p - 0.5 * distance * normal;
 p2.noalias() = p + 0.5 * distance * normal;
 }

 void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1,
 CoalScalar& distance,
 Vec3s& p1, Vec3s& p2,
 Vec3s& normal) const {
 COAL_UNUSED_VARIABLE(tf1);
 COAL_ASSERT(this->gjk.distance <=
 this->gjk.getTolerance() + this->m_dummy_precision,
 "The distance should be lower than GJK's tolerance.",
 std::logic_error);
 // Because GJK has returned the `Collision` status and EPA has not run,
 // we purposefully do not cache the result of GJK, because ray is zero.
 // However, we can cache the support function hint.
 // this->cached_guess = this->gjk.ray;
 this->support_func_cached_guess = this->gjk.support_hint;

 distance = this->gjk.distance;
 p1 = p2 = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }

 void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1,
 CoalScalar& distance, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) const {
 // Cache EPA result for potential future call to this GJKSolver.
 // This caching allows to warm-start the next GJK call.
 this->cached_guess = -(this->epa.depth * this->epa.normal);
 this->support_func_cached_guess = this->epa.support_hint;
 distance = (std::min)(0., -this->epa.depth);
 this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1, p2,
 normal);
 // The following is very important to understand why EPA can sometimes
 // return a normal that is not colinear to the vector $p_1 - p_2$ when
 // working with tolerances like $\epsilon = 10^{-3}$.
 // It can be resumed with a simple idea:
 // EPA is an algorithm meant to find the penetration depth and the
 // normal. It is not meant to find the closest points.
 // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$.
 //
 // More details:
 // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and
 // $p_2$ the witness points. In theory, when EPA converges to $\epsilon =
 // 0$, the normal and witness points verify the following property (P):
 // - $p_1 \in \partial \sigma_{S_1}(n)$,
 // - $p_2 \in \partial \sigma_{S_2}(-n),
 // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of
 // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set
 // of the support function in the direction $n$. (Note: I am leaving out the
 // details of frame choice for the support function, to avoid making the
 // mathematical notation too heavy.)
 // --> In practice, EPA converges to $\epsilon > 0$.
 // On polytopes and the likes, this does not change much and the property
 // given above is still valid.
 // --> However, this is very different on curved surfaces, such as
 // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging
 // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the
 // normal much, but the property (P) given above is no longer valid, which
 // means that the points $p_1$ and $p_2$ do not necessarily belong to the
 // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not
 // colinear.
 //
 // Do not panic! This is fine.
 // Although the property above is not verified, it's almost verified,
 // meaning that $p_1$ and $p_2$ belong to support sets in directions that
 // are very close to $n$.
 //
 // Solution to compute better $p_1$ and $p_2$:
 // We compute the middle points of the current $p_1$ and $p_2$ and we use
 // the normal and the distance given by EPA to compute the new $p_1$ and
 // $p_2$.
 Vec3s p = tf1.transform(0.5 * (p1 + p2));
 normal = tf1.getRotation() * normal;
 p1.noalias() = p - 0.5 * distance * normal;
 p2.noalias() = p + 0.5 * distance * normal;
 }

 void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1,
 CoalScalar& distance, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) const {
 this->cached_guess = Vec3s(1, 0, 0);
 this->support_func_cached_guess.setZero();

 COAL_UNUSED_VARIABLE(tf1);
 distance = -(std::numeric_limits<CoalScalar>::max)();
 p1 = p2 = normal =
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN());
 }
};

} // namespace coal

#endif