Commit 31d382bd authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Update shape-shape collision regarding security margin.

parent 076a11ed
Pipeline #13675 passed with stage
in 49 minutes and 55 seconds
......@@ -82,34 +82,20 @@ 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);
if (distance <= 0) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
(p1+p2)/2,
distanceResult.normal,
-distance+request.security_margin);
result.addContact (contact);
}
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
FCL_REAL distToCollision = distance - request.security_margin;
internal::updateDistanceLowerBoundFromLeaf (request, result, distToCollision, p1, p2);
if (distToCollision <= 0 && result.numContacts () < request.num_max_contacts) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
(p1+p2)/2,
(distance <= 0 ? distanceResult.normal : (p2-p1).normalized ()),
-distToCollision);
result.addContact (contact);
return 1;
}
if (distance <= request.security_margin) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
.5 * (p1 + p2),
(p2-p1).normalized (),
-distance+request.security_margin);
result.addContact (contact);
}
return 1;
}
result.updateDistanceLowerBound (distance);
return 0;
}
......
......@@ -38,6 +38,7 @@
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/internal/shape_shape_func.h>
#include <hpp/fcl/internal/traversal_node_base.h>
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
......@@ -122,8 +123,10 @@ namespace fcl {
FCL_REAL penetrationDepth;
// Unlike in distance computation, we consider the security margin.
penetrationDepth = r1 + r2 + margin - dist;
bool collision = (penetrationDepth >= 0);
if (collision) {
internal::updateDistanceLowerBoundFromLeaf (request, result, -penetrationDepth,
center1 + unit * r1, center2 - unit * r2);
if (penetrationDepth >= 0) {
// Take contact point at the middle of intersection between each sphere
// and segment [c1 c2].
FCL_REAL abscissa = .5 * r1 + .5 * (dist - r2);
......@@ -132,7 +135,6 @@ namespace fcl {
result.addContact (contact);
return 1;
}
result.updateDistanceLowerBound (-penetrationDepth);
return 0;
}
} // namespace fcl
......
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