From 7a1c798e40a89a828d21c972c8a41ea39e1c2fba Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 15 Apr 2015 00:57:17 -0400
Subject: [PATCH] Fix inconsistent normal directions - The convention of FCL is
object1 --> object2 - mesh-mesh: object1 --> object2 (correct) -
shape-shape: object2 --> object1 (fixed) - shape-mesh: object2 --> object1
(fixed)
---
include/fcl/narrowphase/narrowphase.h | 2 +-
src/narrowphase/gjk_libccd.cpp | 4 ++--
src/narrowphase/narrowphase.cpp | 11 +++++++----
3 files changed, 10 insertions(+), 7 deletions(-)
diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h
index 5abf8acf..f956f97e 100644
--- a/include/fcl/narrowphase/narrowphase.h
+++ b/include/fcl/narrowphase/narrowphase.h
@@ -416,7 +416,7 @@ struct GJKSolver_indep
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
}
if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
+ if(normal) *normal = epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index 145e3a5b..1664d28d 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -775,13 +775,13 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
}
- /// libccd returns dir and pos in world space and dir is pointing from object 2 to object 1
+ /// libccd returns dir and pos in world space and dir is pointing from object 1 to object 2
res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
if(res == 0)
{
contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
*penetration_depth = depth;
- normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
+ normal->setValue(ccdVec3X(&dir), ccdVec3Y(&dir), ccdVec3Z(&dir));
return true;
}
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index b701a7ea..e419013c 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -269,13 +269,16 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
- Vec3f diff = tf1.transform(Vec3f()) - tf2.transform(Vec3f());
+ Vec3f diff = tf2.transform(Vec3f()) - tf1.transform(Vec3f());
FCL_REAL len = diff.length();
if(len > s1.radius + s2.radius)
return false;
if(penetration_depth)
*penetration_depth = s1.radius + s2.radius - len;
+
+ // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
+ // Otherwise, normal is pointing from center of object 1 to center of object 2
if(normal)
{
if(len > 0)
@@ -285,7 +288,7 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
}
if(contact_points)
- *contact_points = tf1.transform(Vec3f()) - diff * s1.radius / (s1.radius + s2.radius);
+ *contact_points = tf1.transform(Vec3f()) + diff * s1.radius / (s1.radius + s2.radius);
return true;
}
@@ -423,7 +426,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
if(has_contact)
{
- Vec3f contact_to_center = center - contact_point;
+ Vec3f contact_to_center = contact_point - center;
FCL_REAL distance_sqr = contact_to_center.sqrLength();
if(distance_sqr < radius_with_threshold * radius_with_threshold)
@@ -437,7 +440,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
}
else
{
- if(normal_) *normal_ = normal;
+ if(normal_) *normal_ = -normal;
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -radius;
}
--
GitLab