From 90b9b42643b510a2f74d09f6de510fca4eb9e207 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Fri, 12 Apr 2019 16:18:21 +0200
Subject: [PATCH] Fix uninitialized closest points in capsulePlaneIntersect.
---
src/narrowphase/details.h | 27 ++++++++++++++++++---------
1 file changed, 18 insertions(+), 9 deletions(-)
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index 61258892..bd4b9708 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -2213,14 +2213,15 @@ namespace fcl {
{
Plane new_s2 = transform(s2, tf2);
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
+ // position orientation of capsule
+ const Matrix3f& R1 = tf1.getRotation();
+ const Vec3f& T1 = tf1.getTranslation();
- Vec3f dir_z = R.col(2);
+ Vec3f dir_z = R1.col(2);
-
- Vec3f a1 = T + dir_z * (0.5 * s1.lz);
- Vec3f a2 = T - dir_z * (0.5 * s1.lz);
+ // ends of capsule inner segment
+ Vec3f a1 = T1 + dir_z * (0.5 * s1.lz);
+ Vec3f a2 = T1 - dir_z * (0.5 * s1.lz);
FCL_REAL d1 = new_s2.signedDistance(a1);
FCL_REAL d2 = new_s2.signedDistance(a2);
@@ -2248,6 +2249,7 @@ namespace fcl {
a2 * (abs_d1 / (abs_d1 + abs_d2));
if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n;
}
+ assert (!p1.hasNaN () && !p2.hasNaN ());
return true;
}
@@ -2263,16 +2265,19 @@ namespace fcl {
p1 = a2 - s1.radius * normal;
p2 = p1 - distance * normal;
}
+ assert (!p1.hasNaN () && !p2.hasNaN ());
return false;
}
else
{
+ // Both capsule ends are on the same side of the plane, but one
+ // is closer than the capsule radius, hence collision
distance = std::min(abs_d1, abs_d2) - s1.radius;
if(abs_d1 <= s1.radius && abs_d2 <= s1.radius)
{
- Vec3f c1 = a1 - new_s2.n * d2;
- Vec3f c2 = p2 - new_s2.n * d1;
+ Vec3f c1 = a1 - new_s2.n * d1;
+ Vec3f c2 = a2 - new_s2.n * d2;
p1 = p2 = (c1 + c2) * 0.5;
}
else if(abs_d1 <= s1.radius)
@@ -2282,13 +2287,17 @@ namespace fcl {
}
else if(abs_d2 <= s1.radius)
{
- Vec3f c = p2 - new_s2.n * d2;
+ Vec3f c = a2 - new_s2.n * d2;
p1 = p2 = c;
+ } else {
+ assert (false);
}
if (d1 < 0) normal = new_s2.n; else normal = -new_s2.n;
+ assert (!p1.hasNaN () && !p2.hasNaN ());
return true;
}
+ assert (false);
}
/// @brief cylinder-plane intersect
--
GitLab