Commit d166d216 authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Merge pull request #61 from dartsim/fix_inconsistent_normal

Fix inconsistent normal directions
parents 5dbc1d3c a743c704
......@@ -477,7 +477,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;
}
......
......@@ -776,13 +776,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;
}
......
......@@ -270,13 +270,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)
......@@ -286,7 +289,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;
}
......@@ -424,7 +427,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)
......@@ -438,7 +441,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;
}
......
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment