diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 1c0d831e5f1d4da24f9bedf54e6a0521d5eae283..a85f7fae3131b7333d88401a4168af161bdaf42d 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -101,6 +101,7 @@ namespace fcl
     }
 
     //// @brief intersection checking between one shape and a triangle with transformation
+    /// \return true if the shape are colliding.
     template<typename S>
     bool shapeTriangleInteraction
     (const S& s, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index b919ac08e6b5e244186d90dd7bdb451ff0220ae3..9c81ed71866feb64d9e72af8be1452ba1d5aa8ce 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -187,13 +187,10 @@ public:
   bool BVTesting(int b1, int /*b2*/) const
   {
     if(this->enable_statistics) this->num_bv_tests++;
-    bool res;
     if (RTIsIdentity)
-      res = !this->model1->getBV(b1).bv.overlap(this->model2_bv);
+      return !this->model1->getBV(b1).bv.overlap(this->model2_bv);
     else
-      res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
-    assert (!res || sqrDistLowerBound > 0);
-    return res;
+      return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
   }
 
   /// test between BV b1 and shape
@@ -204,12 +201,15 @@ public:
   bool BVTesting(int b1, int /*b2*/, FCL_REAL& sqrDistLowerBound) const
   {
     if(this->enable_statistics) this->num_bv_tests++;
+    bool res;
     if (RTIsIdentity)
-      return !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
+      res = !this->model1->getBV(b1).bv.overlap(this->model2_bv, this->request, sqrDistLowerBound);
     else
-      return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
+      res = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
                       this->model2_bv, this->model1->getBV(b1).bv,
                       this->request, sqrDistLowerBound);
+    assert (!res || sqrDistLowerBound > 0);
+    return res;
   }
 
   /// @brief Intersection testing between leaves (one triangle and one shape)
@@ -232,33 +232,35 @@ public:
 
     bool collision;
     if (RTIsIdentity) {
+      static const Transform3f Id;
       collision =
         nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
-                                          Transform3f (), distance, c1, c2,
-                                          normal);
+                                          Id       , distance, c1, c2, normal);
     } else {
-      collision = 
+      collision =
         nsolver->shapeTriangleInteraction(*(this->model2), this->tf2, p1, p2, p3,
-                                          this->tf1     , distance, c1, c2, normal);
+                                          this->tf1, distance, c1, c2, normal);
     }
 
     if(collision) {
       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 ());
-      return;
+        assert (this->result->isCollision ());
+        return;
+      }
     }
     sqrDistLowerBound = distance * distance;
     assert (distance > 0);
-    if (this->request.security_margin > 0) {
-      if (distance <= this->request.security_margin) {
-        this->result->addContact(Contact(this->model1, this->model2,
-                                         primitive_id, Contact::NONE,
-                                         .5 * (c1+c2), (c2-c1).normalized (),
-                                         -distance));
-      }
+    if (   this->request.security_margin > 0
+        && distance <= this->request.security_margin)
+    {
+      this->result->addContact(Contact(this->model1, this->model2,
+                                       primitive_id, Contact::NONE,
+                                       .5 * (c1+c2), (c2-c1).normalized (),
+                                       -distance));
     }
     assert (!this->result->isCollision () || sqrDistLowerBound > 0);
   }
@@ -271,7 +273,7 @@ public:
 
   Vec3f* vertices;
   Triangle* tri_indices;
-  
+
   const NarrowPhaseSolver* nsolver;
 };