diff --git a/include/fcl/narrowphase/narrowphase.h b/include/fcl/narrowphase/narrowphase.h index 5abf8acf6f3b8d8420f05eec8bf0f8c8ab3bef8c..f956f97e24ed61abb2a526fa275ff0a435ebc71b 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 145e3a5b168d61e074957b4a32431ea45a1c754a..1664d28d15ec481f0e3ec776127c8fe83386d9eb 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 b701a7ea997e4be63ef0d9448cafe6ab46781340..e419013c294ebeea1dfd11e6e1265c99f377d276 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; }