From fc638a2882fa1f586698f298960353beec9d1a41 Mon Sep 17 00:00:00 2001
From: panjia1983 <panjia1983@gmail.com>
Date: Sat, 9 Aug 2014 23:57:39 -0700
Subject: [PATCH] fix the bug in sphere-sphere distance
---
src/narrowphase/narrowphase.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 1333585f..b701a7ea 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -703,7 +703,7 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
result = Project::projectTriangle(P1, P2, P3, o);
if(result.sqr_distance > sp.radius * sp.radius)
{
- if(dist) *dist = std::sqrt(result.sqr_distance);
+ if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
Vec3f dir = o - project_p;
dir.normalize();
@@ -725,7 +725,7 @@ bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
{
- bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P2), dist, p1, p2);
+ bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
if(p2) *p2 = inverse(tf2).transform(*p2);
return res;
--
GitLab