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

Merge pull request #228 from jcarpent/devel

Fix update of distanceLowerBound
parents 19d44ba2 19918b9e
Pipeline #14888 passed with stage
in 90 minutes and 33 seconds
Subproject commit 3a52692a40839b10f38352c1b06ccfebc0b53f36
Subproject commit 094834088ad9de32f1abdc2d56d29bca2190772c
......@@ -285,7 +285,7 @@ public:
{
}
/// @brief Update the lower bound only if the distance in inferior.
/// @brief Update the lower bound only if the distance is inferior.
inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
{
if (distance_lower_bound_ < distance_lower_bound)
......
......@@ -82,6 +82,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
size_t num_contacts = 0;
if (distance <= 0) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
......@@ -94,7 +95,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
result.addContact (contact);
}
return 1;
num_contacts = result.numContacts();
}
if (distance <= request.security_margin) {
if (result.numContacts () < request.num_max_contacts) {
......@@ -107,10 +108,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
-distance+request.security_margin);
result.addContact (contact);
}
return 1;
num_contacts = result.numContacts();
}
result.updateDistanceLowerBound (distance);
return 0;
return num_contacts;
}
namespace details
......
......@@ -151,7 +151,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound);
col2 = testDistance (transforms[i], m1, m2, distance);
col3 = testCollide (transforms[i], m1, m2);
std::cout << "col1 = " << col1 << ", col2 = " << col2
std::cout << "col1 = " << col1 << ", col2 = " << col2
<< ", col3 = " << col3 << std::endl;
std::cout << "distance lower bound = " << distanceLowerBound << std::endl;
std::cout << "distance = " << distance << std::endl;
......@@ -164,6 +164,42 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
}
}
BOOST_AUTO_TEST_CASE(box_sphere)
{
shared_ptr < hpp::fcl::Sphere > sphere(new hpp::fcl::Sphere(0.5));
shared_ptr < hpp::fcl::Box > box(new hpp::fcl::Box(1.,1.,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,sphere,box,distanceLowerBound);
col2 = testDistance(M1,sphere,box,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], sphere, box, distanceLowerBound);
col2 = testDistance (transforms[i], sphere, box, 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;
......
Markdown is supported
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