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:
public:
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
inline void addContact(const Contact& c)
......
......@@ -304,7 +304,8 @@ private:
Transform3f 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 aabb1, aabb2;
......@@ -328,9 +329,10 @@ private:
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);
FCL_REAL distance;
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)
cresult->addContact(Contact(tree1, &s, static_cast<int>(root1 - tree1->getRoot()), Contact::NONE));
......@@ -339,13 +341,12 @@ private:
else
{
Vec3f contact;
FCL_REAL depth;
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)
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:
constructBox(bv2, tf2, box2, box2_tf);
Vec3f contact;
FCL_REAL depth;
FCL_REAL distance;
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)
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:
void leafCollides(int, int, FCL_REAL&) const
{
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;
FCL_REAL penetration_depth;
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point,
&penetration_depth, &normal))
if(nsolver->shapeIntersect(*model1, tf1, *model2, tf2, distance, true,
&contact_point, &normal))
{
is_collision = true;
if(request.num_max_contacts > result->numContacts())
result->addContact(Contact(model1, model2, Contact::NONE,
Contact::NONE, contact_point,
normal, penetration_depth));
result->addContact(Contact(model1, model2, Contact::NONE,
Contact::NONE, contact_point,
normal, distance));
}
}
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;
if(request.num_max_contacts > result->numContacts())
......
......@@ -56,7 +56,9 @@ namespace fcl
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
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);
if(enable_cached_guess) guess = cached_guess;
......@@ -71,29 +73,36 @@ namespace fcl
Vec3f w0, w1;
switch(gjk_status) {
case details::GJK::Inside:
if (!enable_penetration && contact_points == NULL && normal == NULL)
return true;
if (gjk.hasPenetrationInformation(shape)) {
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(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)
if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
|| epa_status == details::EPA::OutOfVertices // Warnings
)
{
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;
distance_lower_bound = -epa.depth;
if(normal) *normal = tf1.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
distance_lower_bound = -std::numeric_limits<FCL_REAL>::max();
// EPA failed but we know there is a collision so we should
return true;
}
break;
case details::GJK::Valid:
distance_lower_bound = gjk.distance;
break;
default:
;
}
......@@ -139,13 +148,21 @@ namespace fcl
} 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::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);
if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
|| epa_status == details::EPA::OutOfVertices // Warnings
)
{
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);
} else {
distance = -std::numeric_limits<FCL_REAL>::max();
gjk.getClosestPoints (shape, w0, w1);
p1 = p2 = tf1.transform (w0);
}
}
break;
case details::GJK::Valid:
......@@ -240,7 +257,11 @@ namespace fcl
// normal = tf1.getRotation() * epa.normal;
normal = tf2.getRotation() * epa.normal;
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;
}
......@@ -305,7 +326,7 @@ namespace fcl
/// \{
// 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) \
/** @brief Fast implementation for Shape1-Shape2 collision. */ \
doc \
......@@ -313,7 +334,8 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \
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) \
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
......@@ -325,7 +347,17 @@ namespace fcl
HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Sphere, Halfspace,);
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, Plane,);
......@@ -352,6 +384,8 @@ namespace fcl
/// \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) \
/** @brief Fast implementation for Shape-Triangle interaction. */ \
doc \
......@@ -372,7 +406,7 @@ namespace fcl
/// \{
// 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) \
/** @brief Fast implementation for Shape1-Shape2 distance. */ \
doc \
......
......@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
solver.cached_guess = request.cached_gjk_guess;
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
result.distance_lower_bound = -1;
std::size_t res;
if(request.num_max_contacts == 0)
{
......
......@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
}
return 1;
}
result.distance_lower_bound = distance;
result.updateDistanceLowerBound (distance);
return 0;
}
......
......@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
else
collisionNonRecurse(node, front_list, sqrDistLowerBound);
result.distance_lower_bound = sqrt (sqrDistLowerBound);
result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
}
}
......
......@@ -132,7 +132,7 @@ namespace fcl {
result.addContact (contact);
return 1;
}
result.distance_lower_bound = -penetrationDepth;
result.updateDistanceLowerBound (-penetrationDepth);
return 0;
}
} // namespace fcl
......
......@@ -71,7 +71,7 @@ namespace fcl {
inline bool sphereCapsuleIntersect
(const Sphere& s1, const Transform3f& tf1,
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 pos2 (tf2.transform (Vec3f (0., 0., -s2.halfLength)));
......@@ -83,16 +83,13 @@ namespace fcl {
Vec3f diff = s_c - segment_point;
FCL_REAL diffN = diff.norm();
FCL_REAL distance = diffN - s1.radius - s2.radius;
distance = diffN - s1.radius - s2.radius;
if (distance > 0)
return false;
// Vec3f normal (-diff.normalized());
if (distance < 0 && penetration_depth)
*penetration_depth = -distance;
if (normal_)
*normal_ = -diff / diffN;
......@@ -229,16 +226,14 @@ namespace fcl {
inline bool sphereSphereIntersect
(const Sphere& s1, const Transform3f& tf1,
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();
FCL_REAL len = diff.norm();
if(len > s1.radius + s2.radius)
distance = len - s1.radius - s2.radius;
if(distance > 0)
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).
// Otherwise, normal is pointing from center of object 1 to center of object 2
if(normal)
......@@ -1843,29 +1838,30 @@ namespace fcl {
ret = 0;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
Vec3f dir = (new_s1.n).cross(new_s2.n);
FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
{
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;
pl = new_s1;
return true;
}
else
return false;
}
else
{
if(new_s1.d + new_s2.d > 0)
penetration_depth = -(new_s1.d + new_s2.d);
if(penetration_depth < 0)
return false;
else
{
penetration_depth = -(new_s1.d + new_s2.d);
ret = 2;
pl = new_s1;
return true;
......@@ -1880,7 +1876,6 @@ namespace fcl {
p = origin;
d = dir;
ret = 3;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
return true;
}
......@@ -1904,6 +1899,7 @@ namespace fcl {
ret = 0;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
Vec3f dir = (new_s1.n).cross(new_s2.n);
FCL_REAL dir_norm = dir.squaredNorm();
if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
......@@ -1913,25 +1909,23 @@ namespace fcl {
if(new_s1.d < new_s2.d) // s1 is inside s2
{
ret = 1;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
s = new_s1;
}
else // s2 is inside s1
{
ret = 2;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
s = new_s2;
}
return true;
}
else
{
if(new_s1.d + new_s2.d > 0) // not collision
penetration_depth = -(new_s1.d + new_s2.d);
if(penetration_depth < 0) // not collision
return false;
else // in each other
{
ret = 3;
penetration_depth = -(new_s1.d + new_s2.d);
return true;
}
}
......@@ -1944,7 +1938,6 @@ namespace fcl {
p = origin;
d = dir;
ret = 4;
penetration_depth = std::numeric_limits<FCL_REAL>::max();
return true;
}
......
......@@ -120,7 +120,8 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
inline void getShapeSupport(const Box* box, const Vec3f& dir, Vec3f& support)
{
support.noalias() = (dir.array() > 0).select(box->halfSide, -box->halfSide);
const FCL_REAL inflate = (dir.array() == 0).any() ? 1.00000001 : 1.;
support.noalias() = (dir.array() > 0).select(inflate * box->halfSide, -inflate * box->halfSide);
}
inline void getShapeSupport(const Sphere*, const Vec3f& /*dir*/, Vec3f& support)
......
This diff is collapsed.
......@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse
}
}
}
result.distance_lower_bound = sqrt (sqrDistLowerBound);
result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
}
......
......@@ -217,3 +217,42 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
BOOST_CHECK_CLOSE (p2 [2], p2Moved [2], 1e-4);
}
BOOST_AUTO_TEST_CASE(distance_box_box_4)
{
hpp::fcl::Box s1 (1, 1, 1);
hpp::fcl::Box s2 (1, 1, 1);
// Enable computation of nearest points
DistanceRequest distanceRequest (true, 0, 0);
DistanceResult distanceResult;
double distance;
Transform3f tf1 (Vec3f (2, 0, 0));
Transform3f tf2;
hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = 1.;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 1e-4);
tf1.setTranslation(Vec3f (1.01, 0, 0));
distanceResult.clear();
hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = 0.01;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
tf1.setTranslation(Vec3f (0.99, 0, 0));
distanceResult.clear();
hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = -0.01;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
tf1.setTranslation(Vec3f (0, 0, 0));
distanceResult.clear();
hpp::fcl::distance (&s1, tf1, &s2, tf2, distanceRequest, distanceResult);
distance = -1;
BOOST_CHECK_CLOSE(distanceResult.min_distance, distance, 2e-3);
}
......@@ -104,8 +104,9 @@ BOOST_AUTO_TEST_CASE(OBB_Box_test)
GJKSolver solver;
FCL_REAL distance;
bool overlap_obb = obb1.overlap(obb2);
bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL);
bool overlap_box = solver.shapeIntersect(box1, box1_tf, box2, box2_tf, distance, false, NULL, NULL);
BOOST_CHECK(overlap_obb == overlap_box);
}
......@@ -138,13 +139,14 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
FCL_REAL len = (aabb1.max_[0] - aabb1.min_[0]) * 0.5;
OBB obb2;
GJKSolver solver;
FCL_REAL distance;
{
Sphere sphere(len);
computeBV(sphere, transforms[i], obb2);
bool overlap_obb = obb1.overlap(obb2);
bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], NULL, NULL, NULL);
bool overlap_sphere = solver.shapeIntersect(box1, box1_tf, sphere, transforms[i], distance, false, NULL, NULL);
BOOST_CHECK(overlap_obb >= overlap_sphere);
}
......@@ -153,7 +155,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
computeBV(capsule, transforms[i], obb2);
bool overlap_obb = obb1.overlap(obb2);
bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], NULL, NULL, NULL);
bool overlap_capsule = solver.shapeIntersect(box1, box1_tf, capsule, transforms[i], distance, false, NULL, NULL);
BOOST_CHECK(overlap_obb >= overlap_capsule);
}
......@@ -162,7 +164,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
computeBV(cone, transforms[i], obb2);
bool overlap_obb = obb1.overlap(obb2);
bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], NULL, NULL, NULL);
bool overlap_cone = solver.shapeIntersect(box1, box1_tf, cone, transforms[i], distance, false, NULL, NULL);
BOOST_CHECK(overlap_obb >= overlap_cone);
}
......@@ -171,7 +173,7 @@ BOOST_AUTO_TEST_CASE(OBB_shape_test)
computeBV(cylinder, transforms[i], obb2);
bool overlap_obb = obb1.overlap(obb2);
bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], NULL, NULL, NULL);
bool overlap_cylinder = solver.shapeIntersect(box1, box1_tf, cylinder, transforms[i], distance, false, NULL, NULL);
BOOST_CHECK(overlap_obb >= overlap_cylinder);
}
}
......
......@@ -217,7 +217,7 @@ template <typename Sa, typename Sb> void compareShapeDistance (
BOOST_AUTO_TEST_CASE(compare_convex_box)
{
FCL_REAL extents [6] = {0, 0, 0, 10, 10, 10};
FCL_REAL l = 1, w = 1, d = 1;
FCL_REAL l = 1, w = 1, d = 1, eps = 1e-4;
Box box(l*2, w*2, d*2);
Convex<Quadrilateral> convex_box (buildBox (l, w, d));
......@@ -225,16 +225,16 @@ BOOST_AUTO_TEST_CASE(compare_convex_box)
Transform3f tf2;
tf2.setTranslation (Vec3f (3, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
compareShapeIntersection(box, convex_box, tf1, tf2, eps);
compareShapeDistance (box, convex_box, tf1, tf2, eps);
tf2.setTranslation (Vec3f (0, 0, 0));
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
compareShapeIntersection(box, convex_box, tf1, tf2, eps);
compareShapeDistance (box, convex_box, tf1, tf2, eps);
for (int i = 0; i < 1000; ++i) {
generateRandomTransform(extents, tf2);
compareShapeIntersection(box, convex_box, tf1, tf2);
compareShapeDistance (box, convex_box, tf1, tf2);
compareShapeIntersection(box, convex_box, tf1, tf2, eps);
compareShapeDistance (box, convex_box, tf1, tf2, eps);
}
}
This diff is collapsed.
svm_type c_svc
kernel_type rbf
gamma 0.166667
nr_class 1
total_sv 0