From de6a4365e5dbd6879c2e69e78693c310347e5b10 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 28 Jan 2020 13:29:13 +0100
Subject: [PATCH] Fix ShapeShapeCollide.
---
src/collision_func_matrix.cpp | 21 ++++++++++++---------
src/distance_capsule_capsule.cpp | 29 -----------------------------
test/capsule_capsule.cpp | 6 +++++-
3 files changed, 17 insertions(+), 39 deletions(-)
diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index d4b08ab8..dab1eb9c 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -84,24 +84,27 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
if (distance <= 0) {
if (result.numContacts () < request.num_max_contacts) {
- Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
- assert (p1 == distanceResult.nearest_points [1]);
- contact.pos = p1;
- contact.normal = distanceResult.normal;
- contact.penetration_depth = -distance;
+ assert (!request.enable_contact || p1 == distanceResult.nearest_points [1]);
+
+ Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
+ p1,
+ distanceResult.normal,
+ -distance+request.security_margin);
+
result.addContact (contact);
}
return 1;
}
if (distance <= request.security_margin) {
if (result.numContacts () < request.num_max_contacts) {
- Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
- contact.pos = .5 * (p1 + p2);
- contact.normal = (p2-p1).normalized ();
- contact.penetration_depth = -distance;
+
+ Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
+ .5 * (p1 + p2),
+ (p2-p1).normalized (),
+ -distance+request.security_margin);
result.addContact (contact);
}
return 1;
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 2822bf67..22a6b1f1 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -158,35 +158,6 @@ namespace fcl {
return distance;
}
- template <>
- std::size_t ShapeShapeCollide <Capsule, Capsule>
- (const CollisionGeometry* o1, const Transform3f& tf1,
- const CollisionGeometry* o2, const Transform3f& tf2,
- const GJKSolver*, const CollisionRequest& request,
- CollisionResult& result)
- {
- GJKSolver* unused = 0x0;
- DistanceResult distanceResult;
- DistanceRequest distanceRequest (request.enable_contact);
-
- FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
- (o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
-
- if (distance > 0)
- {
- return 0;
- }
- else
- {
- const Vec3f& p1 = distanceResult.nearest_points [0];
- const Vec3f& p2 = distanceResult.nearest_points [1];
- Contact contact (o1, o2, -1, -1, (p1+p2)/2, distanceResult.normal,
- -distanceResult.min_distance);
- result.addContact (contact);
- return 1;
- }
- }
-
} // namespace fcl
} // namespace hpp
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index ef1effd3..ce492d2a 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -99,7 +99,11 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial)
size_t sphere_num_collisions = collide(&sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult);
size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
- BOOST_CHECK(sphere_num_collisions == capsule_num_collisions);
+ BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions);
+ if (sphere_num_collisions == 0 && capsule_num_collisions == 0)
+ BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound,
+ capsule_collisionResult.distance_lower_bound,
+ 1e-6);
}
}
--
GitLab