From af62f24f74107ec8ca121afe78945f279fca81ea Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 10 May 2017 18:45:28 +0200
Subject: [PATCH] Express normal from sphere-capsule intersection in world
coordinate.
---
src/narrowphase/narrowphase.cpp | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index a2ed4d32..7636d0b4 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -84,18 +84,19 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
Vec3f diff = s_c - segment_point;
- FCL_REAL distance = diff.norm() - s1.radius - s2.radius;
+ FCL_REAL diffN = diff.norm();
+ FCL_REAL distance = diffN - s1.radius - s2.radius;
if (distance > 0)
return false;
- Vec3f normal (-diff.normalized());
+ // Vec3f normal (-diff.normalized());
if (distance < 0 && penetration_depth)
*penetration_depth = -distance;
if (normal_)
- *normal_ = tf2.getQuatRotation() * normal;
+ *normal_ = -diff / diffN;
if (contact_points) {
*contact_points = segment_point + diff * s2.radius;
--
GitLab