Program Listing for File narrowphase.h
↰ Return to documentation for file (include/coal/narrowphase/narrowphase.h)
/*
* 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