Verified Commit d1c7b905 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

collision: update distance lower bound for sphere-sphere case

parent 85182356
Pipeline #14894 passed with stage
in 35 minutes and 40 seconds
......@@ -119,9 +119,9 @@ namespace fcl {
Vec3f unit (0,0,0);
if(dist > epsilon)
unit = c1c2/dist;
FCL_REAL penetrationDepth;
// Unlike in distance computation, we consider the security margin.
penetrationDepth = r1 + r2 + margin - dist;
FCL_REAL penetrationDepth = r1 + r2 + margin - dist;
result.updateDistanceLowerBound (-penetrationDepth);
bool collision = (penetrationDepth >= 0);
if (collision) {
// Take contact point at the middle of intersection between each sphere
......@@ -132,7 +132,6 @@ namespace fcl {
result.addContact (contact);
return 1;
}
result.updateDistanceLowerBound (-penetrationDepth);
return 0;
}
} // namespace fcl
......
......@@ -200,6 +200,42 @@ BOOST_AUTO_TEST_CASE(box_sphere)
}
}
BOOST_AUTO_TEST_CASE(sphere_sphere)
{
shared_ptr < hpp::fcl::Sphere > sphere1(new hpp::fcl::Sphere(0.5));
shared_ptr < hpp::fcl::Sphere > sphere2(new hpp::fcl::Sphere(1.));
Transform3f M1; M1.setIdentity();
Transform3f M2; M2.setIdentity();
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-2., -2., -2., 2., 2., 2.};
const std::size_t n = 1000;
generateRandomTransforms(extents, transforms, n);
FCL_REAL distanceLowerBound, distance;
bool col1, col2;
col1 = testDistanceLowerBound(M1,sphere1,sphere2,distanceLowerBound);
col2 = testDistance(M1,sphere1,sphere2,distance);
BOOST_CHECK(col1 == col2);
BOOST_CHECK(distanceLowerBound <= distance);
for(std::size_t i = 0; i < transforms.size(); ++i)
{
FCL_REAL distanceLowerBound, distance;
bool col1, col2;
col1 = testDistanceLowerBound (transforms[i], sphere1, sphere2, distanceLowerBound);
col2 = testDistance (transforms[i], sphere1, sphere2, distance);
BOOST_CHECK (col1 == col2);
if (!col1) {
BOOST_CHECK_MESSAGE (distanceLowerBound <= distance,
"distance = " << distance << ", lower bound = "
<< distanceLowerBound);
}
}
}
BOOST_AUTO_TEST_CASE(box_mesh)
{
std::vector<Vec3f> p1;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment