Unverified Commit 5e94b16d authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #289 from lmontaut/feature/nesterov_acceleration

Feature: Nesterov acceleration for GJK
parents d2c76ba1 24f29e85
Pipeline #18739 passed with stage
in 39 minutes and 56 seconds
......@@ -120,6 +120,9 @@ struct HPP_FCL_DLLAPI QueryRequest {
/// @brief whether enable gjk initial guess
bool enable_cached_gjk_guess;
/// @brief whether to enable the Nesterov accleration of GJK
GJKVariant gjk_variant;
/// @brief the gjk initial guess set by user
Vec3f cached_gjk_guess;
......@@ -131,6 +134,7 @@ struct HPP_FCL_DLLAPI QueryRequest {
QueryRequest()
: enable_cached_gjk_guess(false),
gjk_variant(GJKVariant::DefaultGJK),
cached_gjk_guess(1, 0, 0),
cached_support_func_guess(support_func_guess_t::Zero()),
enable_timings(false) {}
......
......@@ -71,6 +71,9 @@ typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3> Matrixx3i;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
typedef Eigen::Vector2i support_func_guess_t;
/// @brief Variant to use for the GJK algorithm
enum GJKVariant { DefaultGJK, NesterovAcceleration };
/// @brief Triangle with 3 indices for points
class HPP_FCL_DLLAPI Triangle {
public:
......
......@@ -86,6 +86,11 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
/// \note It must set before the call to \ref set.
int linear_log_convex_threshold;
/// @brief Wether or not to use the normalize heuristic in the GJK Nesterov
/// acceleration. This setting is only applied if the Nesterov acceleration in
/// the GJK class is active.
bool normalize_support_direction;
typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized,
Vec3f& support0, Vec3f& support1,
......@@ -122,6 +127,15 @@ struct HPP_FCL_DLLAPI MinkowskiDiff {
getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint,
const_cast<ShapeData*>(data));
}
/// @brief Set wether or not to use the normalization heuristic when computing
/// a support point. Only effective if acceleration version of GJK is used.
/// By default, when MinkowskiDiff::set is called, the normalization heuristic
/// is deduced from the shapes. The user can override this behavior with this
/// function.
inline void setNormalizeSupportDirection(bool normalize) {
normalize_support_direction = normalize;
}
};
/// @brief class for GJK algorithm
......@@ -228,6 +242,12 @@ struct HPP_FCL_DLLAPI GJK {
distance_upper_bound = dup;
}
/// @brief Set which GJK version to use. Default is Vanilla.
inline void setGJKVariant(GJKVariant variant) { gjk_variant = variant; }
/// @brief Get GJK number of iterations.
inline size_t getIterations() { return iterations; }
private:
SimplexV store_v[4];
SimplexV* free_v[4];
......@@ -239,6 +259,8 @@ struct HPP_FCL_DLLAPI GJK {
unsigned int max_iterations;
FCL_REAL tolerance;
FCL_REAL distance_upper_bound;
GJKVariant gjk_variant;
size_t iterations;
/// @brief discard one vertex from the simplex
inline void removeVertex(Simplex& simplex);
......
......@@ -71,6 +71,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
gjk.setDistanceEarlyBreak(distance_upper_bound);
gjk.setGJKVariant(gjk_variant);
details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
if (enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
......@@ -228,6 +230,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
gjk.setDistanceEarlyBreak(distance_upper_bound);
gjk.setGJKVariant(gjk_variant);
details::GJK::Status gjk_status = gjk.evaluate(shape, guess, support_hint);
if (enable_cached_guess) {
cached_guess = gjk.getGuessFromSimplex();
......@@ -306,6 +310,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
cached_guess = Vec3f(1, 0, 0);
support_func_cached_guess = support_func_guess_t::Zero();
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
gjk_variant = DefaultGJK;
}
void enableCachedGuess(bool if_enable) const {
......@@ -316,6 +321,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
Vec3f getCachedGuess() const { return cached_guess; }
void setGJKVariant(const GJKVariant& variant) const { gjk_variant = variant; }
bool operator==(const GJKSolver& other) const {
return epa_max_face_num == other.epa_max_face_num &&
epa_max_vertex_num == other.epa_max_vertex_num &&
......@@ -325,7 +332,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
enable_cached_guess == other.enable_cached_guess &&
cached_guess == other.cached_guess &&
support_func_cached_guess == other.support_func_cached_guess &&
distance_upper_bound == other.distance_upper_bound;
distance_upper_bound == other.distance_upper_bound &&
gjk_variant == other.gjk_variant;
}
bool operator!=(const GJKSolver& other) const { return !(*this == other); }
......@@ -354,6 +362,9 @@ struct HPP_FCL_DLLAPI GJKSolver {
/// @brief smart guess
mutable Vec3f cached_guess;
/// @brief Variant to use for the GJK algorithm
mutable GJKVariant gjk_variant;
/// @brief smart guess for the support function
mutable support_func_guess_t support_func_cached_guess;
......
......@@ -80,9 +80,17 @@ void exposeGJK() {
.DEF_CLASS_FUNC(MinkowskiDiff, support0)
.DEF_CLASS_FUNC(MinkowskiDiff, support1)
.DEF_CLASS_FUNC(MinkowskiDiff, support)
.DEF_CLASS_FUNC(MinkowskiDiff, setNormalizeSupportDirection)
.DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation);
}
if (!eigenpy::register_symbolic_link_to_registered_type<GJKVariant>()) {
enum_<GJKVariant>("GJKVariant")
.value("DefaultGJK", DefaultGJK)
.value("NesterovAcceleration", NesterovAcceleration)
.export_values();
}
if (!eigenpy::register_symbolic_link_to_registered_type<GJK>()) {
class_<GJK>("GJK", doxygen::class_doc<GJK>(), no_init)
.def(doxygen::visitor::init<GJK, unsigned int, FCL_REAL>())
......@@ -94,6 +102,8 @@ void exposeGJK() {
.DEF_CLASS_FUNC(GJK, hasPenetrationInformation)
.DEF_CLASS_FUNC(GJK, getClosestPoints)
.DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak)
.DEF_CLASS_FUNC(GJK, getGuessFromSimplex);
.DEF_CLASS_FUNC(GJK, getGuessFromSimplex)
.DEF_CLASS_FUNC(GJK, setGJKVariant)
.DEF_CLASS_FUNC(GJK, getIterations);
}
}
......@@ -77,6 +77,7 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
}
GJKSolver solver;
solver.enable_cached_guess = request.enable_cached_gjk_guess;
solver.setGJKVariant(request.gjk_variant);
if (solver.enable_cached_guess) {
solver.cached_guess = request.cached_gjk_guess;
solver.support_func_cached_guess = request.cached_support_func_guess;
......
......@@ -80,6 +80,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1,
DistanceResult distanceResult;
DistanceRequest distanceRequest(request.enable_contact);
nsolver->setGJKVariant(request.gjk_variant);
FCL_REAL distance = ShapeShapeDistance<T_SH1, T_SH2>(
o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
......
......@@ -62,6 +62,7 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const DistanceRequest& request, DistanceResult& result) {
GJKSolver solver;
solver.enable_cached_guess = request.enable_cached_gjk_guess;
solver.setGJKVariant(request.gjk_variant);
if (solver.enable_cached_guess) {
solver.cached_guess = request.cached_gjk_guess;
solver.support_func_cached_guess = request.cached_support_func_guess;
......
......@@ -77,6 +77,8 @@ FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1,
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
nsolver->setGJKVariant(request.gjk_variant);
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
distance(&node);
......
......@@ -45,7 +45,7 @@ namespace fcl {
namespace details {
struct HPP_FCL_LOCAL shape_traits_base {
enum { NeedNormalizedDir = true };
enum { NeedNormalizedDir = true, NeedNesterovNormalizeHeuristic = false };
};
template <typename Shape>
......@@ -53,37 +53,42 @@ struct HPP_FCL_LOCAL shape_traits : shape_traits_base {};
template <>
struct HPP_FCL_LOCAL shape_traits<TriangleP> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Box> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Sphere> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Ellipsoid> : shape_traits_base {
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Capsule> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Cone> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<Cylinder> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false };
};
template <>
struct HPP_FCL_LOCAL shape_traits<ConvexBase> : shape_traits_base {
enum { NeedNormalizedDir = false };
enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = true };
};
void getShapeSupport(const TriangleP* triangle, const Vec3f& dir,
......@@ -488,10 +493,50 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction0(
}
}
bool getNormalizeSupportDirection(const ShapeBase* shape) {
switch (shape->getNodeType()) {
case GEOM_TRIANGLE:
return (bool)shape_traits<TriangleP>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_BOX:
return (bool)shape_traits<Box>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_SPHERE:
return (bool)shape_traits<Sphere>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_ELLIPSOID:
return (bool)shape_traits<Ellipsoid>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_CAPSULE:
return (bool)shape_traits<Capsule>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_CONE:
return (bool)shape_traits<Cone>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_CYLINDER:
return (bool)shape_traits<Cylinder>::NeedNesterovNormalizeHeuristic;
break;
case GEOM_CONVEX:
return (bool)shape_traits<ConvexBase>::NeedNesterovNormalizeHeuristic;
break;
default:
throw std::logic_error("Unsupported geometric shape");
}
}
void getNormalizeSupportDirectionFromShapes(const ShapeBase* shape0,
const ShapeBase* shape1,
bool& normalize_support_direction) {
normalize_support_direction = getNormalizeSupportDirection(shape0) &&
getNormalizeSupportDirection(shape1);
}
void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
const Transform3f& tf0, const Transform3f& tf1) {
shapes[0] = shape0;
shapes[1] = shape1;
getNormalizeSupportDirectionFromShapes(shape0, shape1,
normalize_support_direction);
oR1 = tf0.getRotation().transpose() * tf1.getRotation();
ot1 = tf0.getRotation().transpose() *
......@@ -506,6 +551,8 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1,
void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) {
shapes[0] = shape0;
shapes[1] = shape1;
getNormalizeSupportDirectionFromShapes(shape0, shape1,
normalize_support_direction);
oR1.setIdentity();
ot1.setZero();
......@@ -519,6 +566,7 @@ void GJK::initialize() {
status = Failed;
distance_upper_bound = (std::numeric_limits<FCL_REAL>::max)();
simplex = NULL;
gjk_variant = DefaultGJK;
}
Vec3f GJK::getGuessFromSimplex() const { return ray; }
......@@ -619,8 +667,8 @@ bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) {
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
const support_func_guess_t& supportHint) {
size_t iterations = 0;
FCL_REAL alpha = 0;
iterations = 0;
const FCL_REAL inflation = shape_.inflation.sum();
const FCL_REAL upper_bound = distance_upper_bound + inflation;
......@@ -644,6 +692,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
} else
ray = guess;
// Momentum
GJKVariant current_gjk_variant = gjk_variant;
Vec3f w = ray;
Vec3f dir = ray;
Vec3f y;
FCL_REAL momentum;
bool normalize_support_direction = shape->normalize_support_direction;
do {
vertex_id_t next = (vertex_id_t)(1 - current);
Simplex& curr_simplex = simplices[current];
......@@ -663,20 +718,60 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
break;
}
appendVertex(curr_simplex, -ray, false,
// Compute direction for support call
switch (current_gjk_variant) {
case DefaultGJK:
dir = ray;
break;
case NesterovAcceleration:
// Normalize heuristic for collision pairs involving convex but not
// strictly-convex shapes This corresponds to most use cases.
if (normalize_support_direction) {
momentum = (FCL_REAL(iterations) + 2) / (FCL_REAL(iterations) + 3);
y = momentum * ray + (1 - momentum) * w;
FCL_REAL y_norm = y.norm();
// ray is the point of the Minkowski difference which currently the
// closest to the origin. Therefore, y.norm() > ray.norm() Hence, if
// check A above has not stopped the algorithm, we necessarily have
// y.norm() > tolerance. The following assert is just a safety check.
assert(y_norm > tolerance);
dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm;
} else {
momentum = (FCL_REAL(iterations) + 1) / (FCL_REAL(iterations) + 3);
y = momentum * ray + (1 - momentum) * w;
dir = momentum * dir + (1 - momentum) * y;
}
break;
default:
throw std::logic_error("Invalid momentum variant.");
}
appendVertex(curr_simplex, -dir, false,
support_hint); // see below, ray points away from origin
// check removed (by ?): when the new support point is close to previous
// support points, stop (as the new simplex is degenerated)
const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
// check B: no collision if omega > 0
FCL_REAL omega = ray.dot(w) / rl;
FCL_REAL omega = dir.dot(w) / dir.norm();
if (omega > upper_bound) {
distance = omega - inflation;
break;
}
// Check to remove acceleration
if (current_gjk_variant != DefaultGJK) {
FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w);
if (frank_wolfe_duality_gap - tolerance <= 0) {
removeVertex(simplices[current]);
current_gjk_variant = DefaultGJK;
continue; // continue to next iteration
}
}
// 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)
alpha = std::max(alpha, omega);
......@@ -687,6 +782,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
// if(diff - tolerance * rl <= 0)
if (iterations > 0 && diff - tolerance * rl <= 0) {
if (iterations > 0) removeVertex(simplices[current]);
if (current_gjk_variant != DefaultGJK) {
current_gjk_variant = DefaultGJK;
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 <
......
......@@ -57,6 +57,7 @@ add_fcl_test(hfields hfields.cpp)
add_fcl_test(profiling profiling.cpp)
add_fcl_test(gjk gjk.cpp)
add_fcl_test(nesterov_gjk nesterov_gjk.cpp)
if(HPP_FCL_HAS_OCTOMAP)
add_fcl_test(octree octree.cpp)
endif(HPP_FCL_HAS_OCTOMAP)
......
......@@ -47,6 +47,7 @@
using hpp::fcl::FCL_REAL;
using hpp::fcl::GJKSolver;
using hpp::fcl::GJKVariant;
using hpp::fcl::Matrix3f;
using hpp::fcl::Quaternion3f;
using hpp::fcl::Transform3f;
......@@ -66,13 +67,16 @@ struct Result {
typedef std::vector<Result> Results_t;
BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) {
void test_gjk_distance_triangle_triangle(
bool enable_gjk_nesterov_acceleration) {
Eigen::IOFormat numpy(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", ", ",
"np.array ((", "))", "", "");
Eigen::IOFormat tuple(Eigen::FullPrecision, Eigen::DontAlignCols, "", ", ",
"", "", "(", ")");
std::size_t N = 10000;
GJKSolver solver;
if (enable_gjk_nesterov_acceleration)
solver.setGJKVariant(GJKVariant::NesterovAcceleration);
Transform3f tf1, tf2;
Vec3f p1, p2, a1, a2;
Matrix3f M;
......@@ -307,8 +311,14 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) {
<< "s" << std::endl;
}
BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1) {
test_gjk_distance_triangle_triangle(false);
test_gjk_distance_triangle_triangle(true);
}
void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
bool expect_collision) {
bool expect_collision,
bool use_gjk_nesterov_acceleration) {
using namespace hpp::fcl;
Sphere sphere(1.);
......@@ -323,6 +333,8 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
BOOST_CHECK_EQUAL(shape.inflation[1], sphere.radius);
details::GJK gjk(2, 1e-6);
if (use_gjk_nesterov_acceleration)
gjk.setGJKVariant(GJKVariant::NesterovAcceleration);
details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
if (expect_collision)
......@@ -341,18 +353,27 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray,
}
BOOST_AUTO_TEST_CASE(sphere_sphere) {
test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false);
test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false);
test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true);
test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true);
test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false);
test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false);
test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true);
test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true);
test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false, false);
test_gjk_unit_sphere(3., Vec3f(1, 0, 0), false, true);
test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false, false);
test_gjk_unit_sphere(2.01, Vec3f(1, 0, 0), false, true);
test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true, false);
test_gjk_unit_sphere(2., Vec3f(1, 0, 0), true, true);
test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true, false);
test_gjk_unit_sphere(1., Vec3f(1, 0, 0), true, true);
test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false, false);
test_gjk_unit_sphere(3., Vec3f::Random().normalized(), false, true);
test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false, false);
test_gjk_unit_sphere(2.01, Vec3f::Random().normalized(), false, true);
test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true, false);
test_gjk_unit_sphere(2., Vec3f::Random().normalized(), true, true);
test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true, false);
test_gjk_unit_sphere(1., Vec3f::Random().normalized(), true, true);
}
void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
bool use_gjk_nesterov_acceleration,
Vec3f w0_expected, Vec3f w1_expected) {
using namespace hpp::fcl;
Capsule capsule(1., 2.); // Radius 1 and length 2
......@@ -368,6 +389,8 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
BOOST_CHECK_EQUAL(shape.inflation[1], 0.);
details::GJK gjk(10, 1e-6);
if (use_gjk_nesterov_acceleration)
gjk.setGJKVariant(GJKVariant::NesterovAcceleration);
details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0));
if (expect_collision)
......@@ -398,14 +421,23 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision,
BOOST_AUTO_TEST_CASE(triangle_capsule) {
// GJK -> no collision
test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, Vec3f(1., 0, 0),
test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, false, Vec3f(1., 0, 0),
Vec3f(0., 0, 0));
// GJK + Nesterov acceleration -> no collision
test_gjk_triangle_capsule(Vec3f(1.01, 0, 0), false, true, Vec3f(1., 0, 0),
Vec3f(0., 0, 0));
// GJK -> collision
test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, Vec3f(1., 0, 0),
test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, false, Vec3f(1., 0, 0),
Vec3f(0., 0, 0));
// GJK + Nesterov acceleration -> collision
test_gjk_triangle_capsule(Vec3f(0.5, 0, 0), true, true, Vec3f(1., 0, 0),
Vec3f(0., 0, 0));
// GJK + EPA -> collision
test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, Vec3f(0, 1, 0),
test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, false, Vec3f(0, 1, 0),
Vec3f(0.5, 0, 0));
// GJK + Nesterov accleration + EPA -> collision
test_gjk_triangle_capsule(Vec3f(-0.5, -0.01, 0), true, true, Vec3f(0, 1, 0),
Vec3f(0.5, 0, 0));
}
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, CNRS-LAAS 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 Willow Garage, Inc. 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 Louis Montaut */
#define BOOST_TEST_MODULE FCL_NESTEROV_GJK
#include <boost/test/included/unit_test.hpp>
#include <Eigen/Geometry>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/internal/tools.h>
#include "utility.h"
using hpp::fcl::Box;
using hpp::fcl::Capsule;
using hpp::fcl::constructPolytopeFromEllipsoid;
using hpp::fcl::Convex;
using hpp::fcl::Ellipsoid;
using hpp::fcl::FCL_REAL;
using hpp::fcl::GJKVariant;
using hpp::fcl::ShapeBase;
using hpp::fcl::support_func_guess_t;
using hpp::fcl::Transform3f;
using hpp::fcl::Triangle;