Unverified Commit 0443daf1 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #136 from jmirabel/devel

[GJK/EPA] Fix bugs + Treat sphere as point and capsule as line segment.
parents b9c68d9d cc0d5371
......@@ -71,6 +71,10 @@ struct MinkowskiDiff
/// such that @f$ p_in_0 = oR1 * p_in_1 + ot1 @f$.
Vec3f ot1;
/// @brief The radius of the sphere swepted volume.
/// The 2 values correspond to the inflation of shape 0 and shape 1.
Eigen::Array<FCL_REAL, 1, 2> inflation;
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1);
GetSupportFunction getSupportFunc;
......@@ -134,6 +138,19 @@ struct GJK
MinkowskiDiff const* shape;
Vec3f ray;
/// The distance computed by GJK. The possible values are
/// - \f$ d = - R - 1 \f$ when a collision is detected and GJK
/// cannot compute penetration informations.
/// - \f$ - R \le d \le 0 \f$ when a collision is detected and GJK can
/// compute penetration informations.
/// - \f$ 0 < d \le d_{ub} \f$ when there is no collision and GJK can compute
/// the closest points.
/// - \f$ d_{ub} < d \f$ when there is no collision and GJK cannot compute the
/// closest points.
///
/// where \f$ d \f$ is the GJK::distance, \f$ R \f$ is the sum of the \c shape
/// MinkowskiDiff::inflation and \f$ d_{ub} \f$ is the
/// GJK::distance_upper_bound.
FCL_REAL distance;
Simplex simplices[2];
......@@ -165,9 +182,24 @@ struct GJK
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();
}
/// Get the closest points on each object.
/// @return true on success
static bool getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1);
bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
......@@ -277,7 +309,17 @@ private:
public:
enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
enum Status {
Failed = 0,
Valid = 1,
AccuracyReached = 1 << 1 | Valid ,
Degenerated = 1 << 1 | Failed,
NonConvex = 2 << 1 | Failed,
InvalidHull = 3 << 1 | Failed,
OutOfFaces = 4 << 1 | Failed,
OutOfVertices = 5 << 1 | Failed,
FallBack = 6 << 1 | Failed
};
Status status;
GJK::Simplex result;
......@@ -304,8 +346,15 @@ public:
void initialize();
/// \return a Status which can be demangled using (status & Valid) or
/// (status & Failed). The other values provide a more detailled
/// status
Status evaluate(GJK& gjk, const Vec3f& guess);
/// Get the closest points on each object.
/// @return true on success
bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1);
private:
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist);
......
......@@ -68,27 +68,35 @@ namespace fcl
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
switch(gjk_status)
{
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
{
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = gjk.distance;
if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
return true;
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
else return false;
if(epa_status & details::EPA::Failed)
{
epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
// TODO The normal computed by GJK in the s1 frame so this should be
// if(normal) *normal = tf1.getRotation() * epa.normal;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
// EPA failed but we know there is a collision so we should
return true;
}
break;
default:
;
}
}
return false;
}
......@@ -119,37 +127,40 @@ namespace fcl
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
switch(gjk_status)
{
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
{
col = true;
col = true;
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
distance = gjk.distance;
normal = tf1.getRotation() * (w1 - w0).normalized();
p1 = p2 = tf1.transform((w0 + w1) / 2);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
assert (epa_status != details::EPA::Failed); (void) epa_status;
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
assert (epa_status & details::EPA::Valid); (void) epa_status;
epa.getClosestPoints (shape, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
assert (distance <= 1e-6);
break;
}
break;
case details::GJK::Valid:
case details::GJK::Failed:
{
col = false;
col = false;
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
distance = gjk.distance;
gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
distance = gjk.distance;
p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
assert (distance > 0);
}
p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
assert (distance > 0);
break;
default:
assert (false && "should not reach type part.");
......@@ -168,7 +179,6 @@ namespace fcl
#ifndef NDEBUG
FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
#endif
bool compute_normal (true);
Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess;
......@@ -184,7 +194,7 @@ namespace fcl
// TODO: understand why GJK fails between cylinder and box
assert (distance * distance < sqrt (eps));
Vec3f w0, w1;
details::GJK::getClosestPoints (*gjk.getSimplex(), w0, w1);
gjk.getClosestPoints (shape, w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
normal = Vec3f (0,0,0);
......@@ -192,7 +202,7 @@ namespace fcl
}
else if(gjk_status == details::GJK::Valid)
{
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
gjk.getClosestPoints (shape, p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
......@@ -205,29 +215,33 @@ namespace fcl
else
{
assert (gjk_status == details::GJK::Inside);
if (compute_normal)
{
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
}
}
else
if (gjk.hasPenetrationInformation (shape)) {
gjk.getClosestPoints (shape, p1, p2);
distance = gjk.distance;
// Return contact points in case of collision
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal = (tf1.getRotation() * (p2 - p1)).normalized();
p1 = p2 = tf1.transform(p1);
} else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
|| epa_status == details::EPA::OutOfVertices // Warnings
)
{
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
distance = 0;
p1 = tf1.transform (p1);
p2 = tf1.transform (p2);
Vec3f w0, w1;
epa.getClosestPoints (shape, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
// TODO should be
// normal = tf1.getRotation() * epa.normal;
normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
}
}
return false;
}
}
......
......@@ -147,7 +147,10 @@ public:
}
};
/// @brief Center at zero point capsule
/// @brief Capsule
/// It is \f$ { x \in \mathcal{R}^3, d(x, AB) < radius } \f$
/// where \f$ d(x, AB) \f$ is the distance between the point x and the capsule
/// segment AB, with \f$ A = (0,0,-halfLength), B = (0,0,halfLength) \f$.
class Capsule : public ShapeBase
{
public:
......@@ -190,7 +193,9 @@ public:
};
/// @brief Center at zero cone
/// @brief Cone
/// The base of the cone is at \f$ z = - halfLength \f$ and the top is at
/// \f$ z = halfLength \f$.
class Cone : public ShapeBase
{
public:
......
......@@ -98,6 +98,7 @@ SET(${LIBRARY_NAME}_SOURCES
collision.cc
distance.cc
fcl.cc
gjk.cc
)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS})
......
......@@ -91,4 +91,5 @@ BOOST_PYTHON_MODULE(hppfcl)
exposeMeshLoader();
exposeCollisionAPI();
exposeDistanceAPI();
exposeGJK();
}
......@@ -9,3 +9,5 @@ void exposeMeshLoader();
void exposeCollisionAPI();
void exposeDistanceAPI();
void exposeGJK();
//
// Software License Agreement (BSD License)
//
// Copyright (c) 2020 CNRS-LAAS
// Author: Joseph Mirabel
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of CNRS-LAAS. 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.
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/registration.hpp>
#include "fcl.hh"
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/narrowphase/gjk.h>
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
#include "doxygen_autodoc/functions.h"
#include "doxygen_autodoc/hpp/fcl/narrowphase/gjk.h"
#endif
#include "../doc/python/doxygen.hh"
#include "../doc/python/doxygen-boost.hh"
#define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB) \
def_readwrite (#ATTRIB, &CLASS::ATTRIB, \
doxygen::class_attrib_doc<CLASS>(#ATTRIB))
#define DEF_CLASS_FUNC(CLASS, ATTRIB) \
def (#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB))
using namespace boost::python;
using namespace hpp::fcl;
using hpp::fcl::details::MinkowskiDiff;
using hpp::fcl::details::GJK;
using hpp::fcl::details::EPA;
void exposeGJK()
{
if(!eigenpy::register_symbolic_link_to_registered_type<GJK::Status>())
{
enum_ <GJK::Status> ("GJKStatus")
.value ("Valid", GJK::Valid)
.value ("Inside", GJK::Inside)
.value ("Failed", GJK::Failed)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type<MinkowskiDiff>())
{
class_ <MinkowskiDiff> ("MinkowskiDiff",
doxygen::class_doc<MinkowskiDiff>(), init<>(
doxygen::constructor_doc<MinkowskiDiff>()))
.def ("set", static_cast < void (MinkowskiDiff::*)(
const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set),
doxygen::member_func_doc(
static_cast < void (MinkowskiDiff::*)(
const ShapeBase*, const ShapeBase*)> (&MinkowskiDiff::set)))
.def ("set", static_cast < void (MinkowskiDiff::*)(
const ShapeBase*, const ShapeBase*,
const Transform3f& tf0, const Transform3f& tf1)> (&MinkowskiDiff::set),
doxygen::member_func_doc(
static_cast < void (MinkowskiDiff::*)(
const ShapeBase*, const ShapeBase*,
const Transform3f& tf0, const Transform3f& tf1)> (&MinkowskiDiff::set)))
.DEF_CLASS_FUNC(MinkowskiDiff, support0)
.DEF_CLASS_FUNC(MinkowskiDiff, support1)
.DEF_CLASS_FUNC(MinkowskiDiff, support)
.DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type<GJK>())
{
class_ <GJK> ("GJK",
doxygen::class_doc<GJK>(), init<unsigned int, FCL_REAL>(
doxygen::constructor_doc<GJK, unsigned int, FCL_REAL>()))
.DEF_RW_CLASS_ATTRIB (GJK, distance)
.DEF_RW_CLASS_ATTRIB (GJK, ray )
.DEF_CLASS_FUNC(GJK, evaluate)
.DEF_CLASS_FUNC(GJK, hasClosestPoints)
.DEF_CLASS_FUNC(GJK, hasPenetrationInformation)
.DEF_CLASS_FUNC(GJK, getClosestPoints)
.DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak)
;
}
}
......@@ -67,6 +67,18 @@ template <> struct shape_traits<Box> : shape_traits_base
};
};
template <> struct shape_traits<Sphere> : shape_traits_base
{
enum { NeedNormalizedDir = false
};
};
template <> struct shape_traits<Capsule> : shape_traits_base
{
enum { NeedNormalizedDir = false
};
};
template <> struct shape_traits<Cone> : shape_traits_base
{
enum { NeedNormalizedDir = false
......@@ -111,60 +123,76 @@ inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support)
support.noalias() = (dir.array() > 0).select(box->halfSide, -box->halfSide);
}
inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& support)
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support)
{
support = dir * sphere->radius;
support.setZero();
}
inline void getShapeSupport(const Capsule* capsule, const Vec3f& dir, Vec3f& support)
{
support = capsule->radius * dir;
if (dir[2] > 0) support[2] += capsule->halfLength;
else support[2] -= capsule->halfLength;
support.head<2>().setZero();
if (dir[2] > 0) support[2] = capsule->halfLength;
else support[2] = - capsule->halfLength;
}
void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
{
// TODO (Joseph Mirabel)
// this assumes that the cone radius is, for -0.5 < z < 0.5:
// (lz/2 - z) * radius / lz
//
// I did not change the code myself. However, I think it should be revised.
// 1. It can be optimized.
// 2. I am not sure this is what conePlaneIntersect and coneHalfspaceIntersect
// assumes...
// The cone radius is, for -h < z < h, (h - z) * r / (2*h)
static const FCL_REAL inflate = 1.00001;
FCL_REAL h = cone->halfLength;
FCL_REAL r = cone->radius;
if (dir.head<2>().isZero()) {
support.head<2>().setZero();
if (dir[2] > 0)
support[2] = h;
else
support[2] = - inflate * h;
return;
}
FCL_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
FCL_REAL len = zdist + dir[2] * dir[2];
zdist = std::sqrt(zdist);
len = std::sqrt(len);
FCL_REAL half_h = cone->halfLength;
FCL_REAL radius = cone->radius;
FCL_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
if (dir[2] <= 0) {
FCL_REAL rad = r / zdist;
support.head<2>() = rad * dir.head<2>();
support[2] = -h;
return;
}
len = std::sqrt(len);
FCL_REAL sin_a = r / std::sqrt(r * r + 4 * h * h);
if(dir[2] > len * sin_a)
support = Vec3f(0, 0, half_h);
else if(zdist > 0)
{
FCL_REAL rad = radius / zdist;
support = Vec3f(rad * dir[0], rad * dir[1], -half_h);
support << 0, 0, h;
else {
FCL_REAL rad = r / zdist;
support.head<2>() = rad * dir.head<2>();
support[2] = -h;
}
else
support = Vec3f(0, 0, -half_h);
}
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
{
static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
// The inflation makes the object look strictly convex to GJK and EPA. This
// helps solving particular cases (e.g. a cylinder with itself at the same
// position...)
static const FCL_REAL inflate = 1.00001;
FCL_REAL half_h = cylinder->halfLength;
if (dir [2] > eps) support[2] = half_h;
else if (dir [2] < -eps) support[2] = -half_h;
else support[2] = 0;
if (dir.head<2>().isZero())
FCL_REAL r = cylinder->radius;
if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero()) half_h *= inflate;
if (dir[2] > 0) support[2] = half_h;
else if (dir[2] < 0) support[2] = -half_h;
else { support[2] = 0; r *= inflate; }
if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero())
support.head<2>().setZero();
else
support.head<2>() = dir.head<2>().normalized() * cylinder->radius;
assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
support.head<2>() = dir.head<2>().normalized() * r;
assert (fabs (support [0] * dir [1] - support [1] * dir [0])
< sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support)
......@@ -276,8 +304,10 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
}
template <typename Shape0>
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity)
MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1, bool identity,
Eigen::Array<FCL_REAL, 1, 2>& inflation)
{
inflation[1] = 0;
switch(s1->getNodeType())
{
case GEOM_TRIANGLE:
......@@ -287,9 +317,11 @@ MinkowskiDiff::GetSupportFunction makeGetSupportFunction1 (const ShapeBase* s1,
if (identity) return getSupportFuncTpl<Shape0, Box, true >;
else return getSupportFuncTpl<Shape0, Box, false>;
case GEOM_SPHERE:
inflation[1] = static_cast<const Sphere*>(s1)->radius;
if (identity) return getSupportFuncTpl<Shape0, Sphere, true >;
else return getSupportFuncTpl<Shape0, Sphere, false>;
case GEOM_CAPSULE:
inflation[1] = static_cast<const Capsule*>(s1)->radius;
if (identity) return getSupportFuncTpl<Shape0, Capsule, true >;