diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp index aa870d7ca640166229e54770aa06d6751d8d1d80..2822bf67c3ed8ed0e1b0bea6ea027aa9935ec35b 100644 --- a/src/distance_capsule_capsule.cpp +++ b/src/distance_capsule_capsule.cpp @@ -178,12 +178,11 @@ namespace fcl { } else { - Contact contact (o1, o2, -1, -1); const Vec3f& p1 = distanceResult.nearest_points [0]; const Vec3f& p2 = distanceResult.nearest_points [1]; - contact.pos = 0.5 * (p1+p2); - contact.normal = distanceResult.normal; - result.addContact(contact); + Contact contact (o1, o2, -1, -1, (p1+p2)/2, distanceResult.normal, + -distanceResult.min_distance); + result.addContact (contact); return 1; } }