diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 08fec67c861e652918ca827ec68646606ddbcecf..298aa945492cb8528a4c54d397f1871334580948 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -64,12 +64,12 @@ namespace se3
   {
     assert( pairId < geomModel.collisionPairs.size() );
     const CollisionPair & pair = geomModel.collisionPairs[pairId];
-    fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
 
-    assert( pairId      < geomData.distanceResults.size() );
+    assert( pairId      < geomData.collisionResults.size() );
     assert( pair.first  < geomData.collisionObjects.size() );
     assert( pair.second < geomData.collisionObjects.size() );
 
+    fcl::CollisionResult& collisionResult = geomData.collisionResults[pairId];
     collisionResult.clear();
     fcl::collide (&geomData.collisionObjects[pair.first],
                   &geomData.collisionObjects[pair.second],
@@ -127,7 +127,8 @@ namespace se3
     assert( pairId      < geomData.distanceResults.size() );
     assert( pair.first  < geomData.collisionObjects.size() );
     assert( pair.second < geomData.collisionObjects.size() );
-    
+
+    geomData.distanceResults[pairId].clear();
     fcl::distance ( &geomData.collisionObjects[pair.first],
                     &geomData.collisionObjects[pair.second],
                     geomData.distanceRequest,
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index dfb37c2f470daa7aea0a5507a3d9b7c1ae8dbd37..239b4d229cbf35660cb8010f021240d6ea6b978f 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -258,11 +258,6 @@ namespace se3
     /// \sa activateCollisionPair
     void deactivateCollisionPair(const PairIndex pairId);
 
-    /// Reset the vector distanceResults.
-    /// TODO: should this be called automatically before calling computeDistance?
-    /// TODO: should we implement the same for collisions?
-    void resetDistances();
-
 #endif //WITH_HPP_FCL
     friend std::ostream & operator<<(std::ostream & os, const GeometryData & geomData);
     
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 3fd502c4313cba53a0e2cf8be754766d661afad8..619730cfad177fb5a41880b8537f4657c9c306e5 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -238,10 +238,6 @@ namespace se3
     activeCollisionPairs[pairId] = false;
   }
 
-  inline void GeometryData::resetDistances()
-  {
-    std::fill(distanceResults.begin(), distanceResults.end(), fcl::DistanceResult() );
-  }
 #endif //WITH_HPP_FCL
 } // namespace se3