Unverified Commit 473fc01c authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #149 from jmirabel/devel

Bug fix + prepare optimization of collision using GJK / EPA
parents f7d19b72 829b33c6
Pipeline #8962 passed with stage
in 187 minutes and 58 seconds
...@@ -223,9 +223,16 @@ public: ...@@ -223,9 +223,16 @@ public:
public: public:
CollisionResult() CollisionResult()
: distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
{ {
} }
/// @brief Update the lower bound only if the distance in inferior.
inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
{
if (distance_lower_bound_ < distance_lower_bound)
distance_lower_bound = distance_lower_bound_;
}
/// @brief add one contact into result structure /// @brief add one contact into result structure
inline void addContact(const Contact& c) inline void addContact(const Contact& c)
......
...@@ -304,7 +304,8 @@ private: ...@@ -304,7 +304,8 @@ private:
Transform3f box_tf; Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf); constructBox(bv1, tf1, box, box_tf);
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL)) FCL_REAL distance;
if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
{ {
AABB overlap_part; AABB overlap_part;
AABB aabb1, aabb2; AABB aabb1, aabb2;
...@@ -328,9 +329,10 @@ private: ...@@ -328,9 +329,10 @@ private:
Transform3f box_tf; Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf); constructBox(bv1, tf1, box, box_tf);
FCL_REAL distance;
if(!crequest->enable_contact) if(!crequest->enable_contact)
{ {
if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL)) if(solver->shapeIntersect(box, box_tf, s, tf2, distance, false, NULL, NULL))
{ {
if(cresult->numContacts() < crequest->num_max_contacts) if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE)); cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE));
...@@ -339,13 +341,12 @@ private: ...@@ -339,13 +341,12 @@ private:
else else
{ {
Vec3f contact; Vec3f contact;
FCL_REAL depth;
Vec3f normal; Vec3f normal;
if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal)) if(solver->shapeIntersect(box, box_tf, s, tf2, distance, true, &contact, &normal))
{ {
if(cresult->numContacts() < crequest->num_max_contacts) if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE, contact, normal, depth)); cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE, contact, normal, distance));
} }
} }
...@@ -819,12 +820,12 @@ private: ...@@ -819,12 +820,12 @@ private:
constructBox(bv2, tf2, box2, box2_tf); constructBox(bv2, tf2, box2, box2_tf);
Vec3f contact; Vec3f contact;
FCL_REAL depth; FCL_REAL distance;
Vec3f normal; Vec3f normal;
if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, &contact, &depth, &normal)) if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, distance, true, &contact, &normal))
{ {
if(cresult->numContacts() < crequest->num_max_contacts) if(cresult->numContacts() < crequest->num_max_contacts)
cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot()), contact, normal, depth)); cresult->addContact(Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()), static_cast<int>(root2 - tree2->getRoot()), contact, normal, distance));
} }
} }
......
...@@ -84,23 +84,26 @@ public: ...@@ -84,23 +84,26 @@ public:
void leafCollides(int, int, FCL_REAL&) const void leafCollides(int, int, FCL_REAL&) const
{ {
bool is_collision = false; bool is_collision = false;
if(request.enable_contact) FCL_REAL distance;
if(request.enable_contact && request.num_max_contacts > result->numContacts())
{ {
Vec3f contact_point, normal; Vec3f contact_point, normal;
FCL_REAL penetration_depth; if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true,
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &contact_point, &normal))
&penetration_depth, &normal))
{ {
is_collision = true; is_collision = true;
if(request.num_max_contacts > result->numContacts()) result->addContact(Contact(model1, model2, Contact::NONE,
result->addContact(Contact(model1, model2, Contact::NONE, Contact::NONE, contact_point,
Contact::NONE, contact_point, normal, distance));
normal, penetration_depth));
} }
} }
else else
{ {
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL)) bool res = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance,
request.enable_distance_lower_bound, NULL, NULL);
if (request.enable_distance_lower_bound)
result->updateDistanceLowerBound (distance);
if(res)
{ {
is_collision = true; is_collision = true;
if(request.num_max_contacts > result->numContacts()) if(request.num_max_contacts > result->numContacts())
......
...@@ -56,7 +56,9 @@ namespace fcl ...@@ -56,7 +56,9 @@ namespace fcl
template<typename S1, typename S2> template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1, bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2, const S2& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const FCL_REAL& distance_lower_bound,
bool enable_penetration,
Vec3f* contact_points, Vec3f* normal) const
{ {
Vec3f guess(1, 0, 0); Vec3f guess(1, 0, 0);
if(enable_cached_guess) guess = cached_guess; if(enable_cached_guess) guess = cached_guess;
...@@ -71,29 +73,36 @@ namespace fcl ...@@ -71,29 +73,36 @@ namespace fcl
Vec3f w0, w1; Vec3f w0, w1;
switch(gjk_status) { switch(gjk_status) {
case details::GJK::Inside: case details::GJK::Inside:
if (!enable_penetration && contact_points == NULL && normal == NULL)
return true;
if (gjk.hasPenetrationInformation(shape)) { if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1); gjk.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = gjk.distance; distance_lower_bound = gjk.distance;
if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized(); if(normal) *normal = tf1.getRotation() * (w0 - w1).normalized();
if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2); if(contact_points) *contact_points = tf1.transform((w0 + w1) / 2);
return true; return true;
} else { } else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); 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); details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status & details::EPA::Failed) if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
|| epa_status == details::EPA::OutOfVertices // Warnings
)
{ {
epa.getClosestPoints (shape, w0, w1); epa.getClosestPoints (shape, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth; distance_lower_bound = -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 = 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)); if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true; return true;
} }
distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
// EPA failed but we know there is a collision so we should // EPA failed but we know there is a collision so we should
return true; return true;
} }
break; break;
case details::GJK::Valid:
distance_lower_bound = gjk.distance;
break;
default: default:
; ;
} }
...@@ -139,13 +148,21 @@ namespace fcl ...@@ -139,13 +148,21 @@ namespace fcl
} else { } else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); 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); details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
assert (epa_status & details::EPA::Valid); (void) epa_status; if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
epa.getClosestPoints (shape, w0, w1); || epa_status == details::EPA::OutOfVertices // Warnings
distance = -epa.depth; )
normal = -epa.normal; {
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); epa.getClosestPoints (shape, w0, w1);
assert (distance <= 1e-6); distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
assert (distance <= 1e-6);
} else {
distance = -std::numeric_limits<FCL_REAL>::max();
gjk.getClosestPoints (shape, w0, w1);
p1 = p2 = tf1.transform (w0);
}
} }
break; break;
case details::GJK::Valid: case details::GJK::Valid:
...@@ -240,7 +257,11 @@ namespace fcl ...@@ -240,7 +257,11 @@ namespace fcl
// normal = tf1.getRotation() * epa.normal; // normal = tf1.getRotation() * epa.normal;
normal = tf2.getRotation() * epa.normal; normal = tf2.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return false;
} }
distance = -std::numeric_limits<FCL_REAL>::max();
gjk.getClosestPoints (shape, p1, p2);
p1 = p2 = tf1.transform (p1);
} }
return false; return false;
} }
...@@ -305,7 +326,7 @@ namespace fcl ...@@ -305,7 +326,7 @@ namespace fcl
/// \{ /// \{
// param doc is the doxygen detailled description (should be enclosed in /** */ // param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no space. // and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \ #define HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape1,Shape2,doc) \
/** @brief Fast implementation for Shape1-Shape2 collision. */ \ /** @brief Fast implementation for Shape1-Shape2 collision. */ \
doc \ doc \
...@@ -313,7 +334,8 @@ namespace fcl ...@@ -313,7 +334,8 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \ bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \ (const Shape1& s1, const Transform3f& tf1, \
const Shape2& s2, const Transform3f& tf2, \ const Shape2& s2, const Transform3f& tf2, \
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const FCL_REAL& distance_lower_bound, bool enable_penetration, \
Vec3f* contact_points, Vec3f* normal) const
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \ #define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc) HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \ #define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
...@@ -325,7 +347,17 @@ namespace fcl ...@@ -325,7 +347,17 @@ namespace fcl
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Plane,);
HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,); #ifdef IS_DOXYGEN // for doxygen only
/** \todo currently disabled and to re-enable it, API of function
* \ref obbDisjointAndLowerBoundDistance should be modified.
* */
template<> bool GJKSolver::shapeIntersect<Box, Box>
(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
FCL_REAL& distance_lower_bound, bool enable_penetration,
Vec3f* contact_points, Vec3f* normal) const;
#endif
//HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Box,);
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Halfspace,);
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,); HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Box, Plane,);
...@@ -352,6 +384,8 @@ namespace fcl ...@@ -352,6 +384,8 @@ namespace fcl
/// \name Shape triangle interaction specializations /// \name Shape triangle interaction specializations
/// \{ /// \{
// param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \ #define HPP_FCL_DECLARE_SHAPE_TRIANGLE(Shape,doc) \
/** @brief Fast implementation for Shape-Triangle interaction. */ \ /** @brief Fast implementation for Shape-Triangle interaction. */ \
doc \ doc \
...@@ -372,7 +406,7 @@ namespace fcl ...@@ -372,7 +406,7 @@ namespace fcl
/// \{ /// \{
// param doc is the doxygen detailled description (should be enclosed in /** */ // param doc is the doxygen detailled description (should be enclosed in /** */
// and contain no space. // and contain no dot for some obscure reasons).
#define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \ #define HPP_FCL_DECLARE_SHAPE_DISTANCE(Shape1,Shape2,doc) \
/** @brief Fast implementation for Shape1-Shape2 distance. */ \ /** @brief Fast implementation for Shape1-Shape2 distance. */ \
doc \ doc \
......
...@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
solver.cached_guess = request.cached_gjk_guess; solver.cached_guess = request.cached_gjk_guess;
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
result.distance_lower_bound = -1;
std::size_t res; std::size_t res;
if(request.num_max_contacts == 0) if(request.num_max_contacts == 0)
{ {
......
...@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf ...@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
} }
return 1; return 1;
} }
result.distance_lower_bound = distance; result.updateDistanceLowerBound (distance);
return 0; return 0;
} }
......
...@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node, ...@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
else else
collisionNonRecurse(node, front_list, sqrDistLowerBound); collisionNonRecurse(node, front_list, sqrDistLowerBound);
result.distance_lower_bound = sqrt (sqrDistLowerBound); result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
} }
} }
......
...@@ -132,7 +132,7 @@ namespace fcl { ...@@ -132,7 +132,7 @@ namespace fcl {
result.addContact (contact); result.addContact (contact);
return 1; return 1;
} }
result.distance_lower_bound = -penetrationDepth; result.updateDistanceLowerBound (-penetrationDepth);
return 0; return 0;
} }
} // namespace fcl } // namespace fcl
......
...@@ -71,7 +71,7 @@ namespace fcl { ...@@ -71,7 +71,7 @@ namespace fcl {
inline bool sphereCapsuleIntersect inline bool sphereCapsuleIntersect
(const Sphere& s1, const Transform3f& tf1, (const Sphere& s1, const Transform3f& tf1,
const Capsule& s2, const Transform3f& tf2, const Capsule& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_) FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal_)
{ {
Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function Vec3f pos1 (tf2.transform (Vec3f (0., 0., s2.halfLength))); // from distance function
Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength))); Vec3f pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
...@@ -83,16 +83,13 @@ namespace fcl { ...@@ -83,16 +83,13 @@ namespace fcl {
Vec3f diff = s_c - segment_point; Vec3f diff = s_c - segment_point;
FCL_REAL diffN = diff.norm(); FCL_REAL diffN = diff.norm();
FCL_REAL distance = diffN - s1.radius - s2.radius; distance = diffN - s1.radius - s2.radius;
if (distance > 0) if (distance > 0)
return false; return false;
// Vec3f normal (-diff.normalized()); // Vec3f normal (-diff.normalized());
if (distance < 0 && penetration_depth)
*penetration_depth = -distance;
if (normal_) if (normal_)
*normal_ = -diff / diffN; *normal_ = -diff / diffN;
...@@ -229,16 +226,14 @@ namespace fcl { ...@@ -229,16 +226,14 @@ namespace fcl {
inline bool sphereSphereIntersect inline bool sphereSphereIntersect
(const Sphere& s1, const Transform3f& tf1, (const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2, const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) FCL_REAL& distance, Vec3f* contact_points, Vec3f* normal)
{ {
const Vec3f diff = tf2.getTranslation() - tf1.getTranslation(); const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
FCL_REAL len = diff.norm(); FCL_REAL len = diff.norm();
if(len > s1.radius + s2.radius) distance = len - s1.radius - s2.radius;
if(distance > 0)
return false; return false;
if(penetration_depth)
*penetration_depth = s1.radius + s2.radius - len;
// If the centers of two sphere are at the same position, the normal is (0, 0, 0). // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
// Otherwise, normal is pointing from center of object 1 to center of object 2 // Otherwise, normal is pointing from center of object 1 to center of object 2
if(normal) if(normal)
...@@ -1843,29 +1838,30 @@ namespace fcl { ...@@ -1843,29 +1838,30 @@ namespace fcl {
ret = 0; ret = 0;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
Vec3f dir = (new_s1.n).cross(new_s2.n); Vec3f dir = (new_s1.n).cross(new_s2.n);
FCL_REAL dir_norm = dir.squaredNorm(); FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
{ {
if((new_s1.n).dot(new_s2.n) > 0) if((new_s1.n).dot(new_s2.n) > 0)
{ {
if(new_s1.d < new_s2.d) penetration_depth = new_s2.d - new_s1.d;
if(penetration_depth < 0)
return false;
else
{ {
penetration_depth = new_s2.d - new_s1.d;
ret = 1; ret = 1;
pl = new_s1; pl = new_s1;
return true; return true;
} }
else
return false;
} }
else else
{ {
if(new_s1.d + new_s2.d > 0) penetration_depth = -(new_s1.d + new_s2.d);
if(penetration_depth < 0)
return false; return false;
else else
{ {
penetration_depth = -(new_s1.d + new_s2.d);
ret = 2; ret = 2;
pl = new_s1; pl = new_s1;
return true; return true;
...@@ -1880,7 +1876,6 @@ namespace fcl { ...@@ -1880,7 +1876,6 @@ namespace fcl {
p = origin; p = origin;
d = dir; d = dir;
ret = 3; ret = 3;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
return true; return true;
} }
...@@ -1904,6 +1899,7 @@ namespace fcl { ...@@ -1904,6 +1899,7 @@ namespace fcl {
ret = 0; ret = 0;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
Vec3f dir = (new_s1.n).cross(new_s2.n); Vec3f dir = (new_s1.n).cross(new_s2.n);
FCL_REAL dir_norm = dir.squaredNorm(); FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
...@@ -1913,25 +1909,23 @@ namespace fcl { ...@@ -1913,25 +1909,23 @@ namespace fcl {
if(new_s1.d < new_s2.d) // s1 is inside s2 if(new_s1.d < new_s2.d) // s1 is inside s2
{ {
ret = 1; ret = 1;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
s = new_s1; s = new_s1;
} }
else // s2 is inside s1 else // s2 is inside s1
{ {
ret = 2; ret = 2;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
s = new_s2; s = new_s2;
} }
return true; return true;
} }
else else
{ {
if(new_s1.d + new_s2.d > 0) // not collision penetration_depth = -(new_s1.d + new_s2.d);