diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp
index d4b08ab8f72eb433437ffbfb59402023e61692b2..dab1eb9c46e5fbc704a41e5cd0fc62d085a66436 100644
--- a/src/collision_func_matrix.cpp
+++ b/src/collision_func_matrix.cpp
@@ -84,24 +84,27 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
 
   if (distance <= 0) {
     if (result.numContacts () < request.num_max_contacts) {
-      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
       const Vec3f& p1 = distanceResult.nearest_points [0];
-      assert (p1 == distanceResult.nearest_points [1]);
-      contact.pos = p1;
-      contact.normal = distanceResult.normal;
-      contact.penetration_depth = -distance;
+      assert (!request.enable_contact || p1 == distanceResult.nearest_points [1]);
+
+      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
+          p1,
+          distanceResult.normal,
+          -distance+request.security_margin);
+
       result.addContact (contact);
     }
     return 1;
   }
   if (distance <= request.security_margin) {
     if (result.numContacts () < request.num_max_contacts) {
-      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
       const Vec3f& p1 = distanceResult.nearest_points [0];
       const Vec3f& p2 = distanceResult.nearest_points [1];
-      contact.pos = .5 * (p1 + p2);
-      contact.normal = (p2-p1).normalized ();
-      contact.penetration_depth = -distance;
+
+      Contact contact (o1, o2, distanceResult.b1, distanceResult.b2,
+          .5 * (p1 + p2),
+          (p2-p1).normalized (),
+          -distance+request.security_margin);
       result.addContact (contact);
     }
     return 1;
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 2822bf67c3ed8ed0e1b0bea6ea027aa9935ec35b..22a6b1f17a067101d20487c8909b1884afe3ac14 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -158,35 +158,6 @@ namespace fcl {
     return distance;
   }
 
-  template <>
-  std::size_t ShapeShapeCollide <Capsule, Capsule>
-  (const CollisionGeometry* o1, const Transform3f& tf1,
-   const CollisionGeometry* o2, const Transform3f& tf2,
-   const GJKSolver*, const CollisionRequest& request,
-   CollisionResult& result)
-  {
-    GJKSolver* unused = 0x0;
-    DistanceResult distanceResult;
-    DistanceRequest distanceRequest (request.enable_contact);
-
-    FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
-      (o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
-
-    if (distance > 0)
-    {
-      return 0;
-    }
-    else
-    {
-      const Vec3f& p1 = distanceResult.nearest_points [0];
-      const Vec3f& p2 = distanceResult.nearest_points [1];
-      Contact contact (o1, o2, -1, -1, (p1+p2)/2, distanceResult.normal,
-          -distanceResult.min_distance);
-      result.addContact (contact);
-      return 1;
-    }
-  }
-
 } // namespace fcl
 
 } // namespace hpp
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index ef1effd3fedf30037d3caeb284956193b888db53..ce492d2a154c82d175dca37204e5d6e919278be2 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -99,7 +99,11 @@ BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial)
     size_t sphere_num_collisions = collide(&sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult);
     size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
     
-    BOOST_CHECK(sphere_num_collisions == capsule_num_collisions);
+    BOOST_CHECK_EQUAL(sphere_num_collisions, capsule_num_collisions);
+    if (sphere_num_collisions == 0 && capsule_num_collisions == 0)
+      BOOST_CHECK_CLOSE(sphere_collisionResult.distance_lower_bound,
+                        capsule_collisionResult.distance_lower_bound,
+                        1e-6);
   }
 }