[C++] Geom: remove resetDistance and reset the distanceResult in computeDistance.

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,

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);
     

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