diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
index 5e4b0d3dc..f430d2edd 100644
--- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h
@@ -215,17 +215,15 @@ class MeshShapeCollisionTraversalNode
     }
 
     FCL_REAL distToCollision = distance - this->request.security_margin;
-    //    if (collision) {
-    //      sqrDistLowerBound = 0;
-    //      if (this->request.num_max_contacts > this->result->numContacts()) {
-    //        this->result->addContact(Contact(this->model1, this->model2,
-    //                                         primitive_id, Contact::NONE, c1,
-    //                                         -normal, -distance));
-    //        assert(this->result->isCollision());
-    //      }
-    //    }
-    //    else
-    if (distToCollision <= this->request.collision_distance_threshold) {
+    if (collision) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         primitive_id, Contact::NONE, c1,
+                                         -normal, -distance));
+        assert(this->result->isCollision());
+      }
+    } else if (distToCollision <= this->request.collision_distance_threshold) {
       sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(
@@ -331,17 +329,15 @@ class ShapeMeshCollisionTraversalNode
     }
 
     FCL_REAL distToCollision = distance - this->request.security_margin;
-    //    if (collision) {
-    //      sqrDistLowerBound = 0;
-    //      if (this->request.num_max_contacts > this->result->numContacts()) {
-    //        this->result->addContact(Contact(this->model1, this->model2,
-    //                                         Contact::NONE, primitive_id, c1,
-    //                                         normal, -distance));
-    //        assert(this->result->isCollision());
-    //      }
-    //    }
-    //    else
-    if (distToCollision <= this->request.collision_distance_threshold) {
+    if (collision) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         Contact::NONE, primitive_id, c1,
+                                         normal, -distance));
+        assert(this->result->isCollision());
+      }
+    } else if (distToCollision <= this->request.collision_distance_threshold) {
       sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(
diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
index 7adfef7a1..19f3a928e 100644
--- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h
+++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h
@@ -325,23 +325,20 @@ class HeightFieldShapeCollisionTraversalNode
     FCL_REAL distance;
     Vec3f c1, c2, normal;
 
-    //    bool collision =
-    this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), this->tf2,
-                        distance, c1, c2, normal);
+    bool collision =
+        this->shapeDistance(convex1, convex2, this->tf1, *(this->model2),
+                            this->tf2, distance, c1, c2, normal);
 
     FCL_REAL distToCollision = distance - this->request.security_margin;
-    //    if (collision) {
-    //      sqrDistLowerBound = 0;
-    //      if (this->request.num_max_contacts > this->result->numContacts()) {
-    //        this->result->addContact(Contact(this->model1, this->model2,
-    //        (int)b1,
-    //                                         (int)Contact::NONE, c1, normal,
-    //                                         -distance));
-    //        assert(this->result->isCollision());
-    //      }
-    //    }
-    //    else
-    if (distToCollision <= this->request.collision_distance_threshold) {
+    if (collision) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(Contact(this->model1, this->model2, (int)b1,
+                                         (int)Contact::NONE, c1, normal,
+                                         -distance));
+        assert(this->result->isCollision());
+      }
+    } else if (distToCollision <= this->request.collision_distance_threshold) {
       sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(Contact(this->model1, this->model2, (int)b1,
@@ -509,22 +506,21 @@ class ShapeHeightFieldCollisionTraversalNode
     Vec3f normal;
     Vec3f c1, c2;  // closest points
 
-    //    bool collision =
-    this->shapeDistance(*(this->model1), this->tf1, convex1, convex2, this->tf2,
-                        distance, c1, c2, normal);
+    bool collision =
+        this->shapeDistance(*(this->model1), this->tf1, convex1, convex2,
+                            this->tf2, distance, c1, c2, normal);
 
     FCL_REAL distToCollision = distance - this->request.security_margin;
-    //    if (collision) {
-    //      sqrDistLowerBound = 0;
-    //      if (this->request.num_max_contacts > this->result->numContacts()) {
-    //        this->result->addContact(Contact(this->model1, this->model2,
-    //                                         Contact::NONE, b2, c1, normal,
-    //                                         distance));
-    //        assert(this->result->isCollision());
-    //        return;
-    //      }
-    //    } else
-    if (distToCollision < 0) {
+    if (collision) {
+      sqrDistLowerBound = 0;
+      if (this->request.num_max_contacts > this->result->numContacts()) {
+        this->result->addContact(Contact(this->model1, this->model2,
+                                         Contact::NONE, b2, c1, normal,
+                                         distance));
+        assert(this->result->isCollision());
+        return;
+      }
+    } else if (distToCollision < 0) {
       sqrDistLowerBound = 0;
       if (this->request.num_max_contacts > this->result->numContacts()) {
         this->result->addContact(Contact(this->model1, this->model2,