[C++][Geom] distances results for computeDistances is not always cleared but...

[C++][Geom] distances results for computeDistances is not always cleared but replaced by a vector with empty results. The initialization of collision pairs is now  always done in constructor.

benchmark/timings.cpp

@@ -158,7 +158,6 @@ int main(int argc, const char ** argv)
   se3::GeometryModel romeo_model_geom = romeo.second;
   Data romeo_data(romeo_model);
   GeometryData romeo_data_geom(romeo_data, romeo_model_geom);
-  romeo_data_geom.fillAllPairsAsCollisions();
 
   VectorXd q_romeo = VectorXd::Random(romeo_model.nq);
   VectorXd qdot_romeo = VectorXd::Random(romeo_model.nv);
@@ -204,7 +203,7 @@ int main(int argc, const char ** argv)
   {
     romeo_data_geom.isColliding();
   }
-  std::cout << "Collision Test for all collision pairs = \t"; timer.toc(std::cout,NBT);
+  std::cout << "Collision Test : is the robot colliding ? = \t"; timer.toc(std::cout,NBT);
 
 
   timer.tic();

src/multibody/geometry.hpp

@@ -131,6 +131,7 @@ namespace se3
     std::vector<fcl::Transform3f> oMg_fcl;
 
     std::vector < CollisionPair_t > collision_pairs;
+    std::size_t nCollisionPairs;
     std::vector < DistanceResult > distances;
 
     GeometryData(Data& data, GeometryModel& model_geom)
@@ -139,8 +140,12 @@ namespace se3
         , oMg(model_geom.ngeom)
         , oMg_fcl(model_geom.ngeom)
         , collision_pairs()
+        , nCollisionPairs(0)
         , distances()
     {
+      initializeListOfCollisionPairs();
+      distances.resize(nCollisionPairs, DistanceResult( fcl::DistanceResult(), 0, 0) );
+
     }
 
     ~GeometryData() {};
@@ -152,11 +157,14 @@ namespace se3
     bool isCollisionPair (Index co1, Index co2);
     bool isCollisionPair (const CollisionPair_t& pair);
     void fillAllPairsAsCollisions();
+    void desactivateCollisionPairs();
+    void initializeListOfCollisionPairs();
 
     bool collide(Index co1, Index co2);
     bool isColliding();
 
     fcl::DistanceResult computeDistance(Index co1, Index co2);
+    void resetDistances();
     void computeDistances ();
 
     std::vector < DistanceResult > distanceResults(); //TODO : to keep or not depending of public or not for distances
@@ -252,6 +260,7 @@ namespace se3
     assert(pair.first < pair.second);
     assert(pair.second < model_geom.ngeom);
     collision_pairs.push_back(pair);
+    nCollisionPairs++;
   }
 
   inline void GeometryData::removeCollisionPair (Index co1, Index co2)
@@ -270,6 +279,7 @@ namespace se3
     assert(isCollisionPair(pair));
 
     collision_pairs.erase(std::remove(collision_pairs.begin(), collision_pairs.end(), pair), collision_pairs.end());
+    nCollisionPairs--;
   }
 
   inline bool GeometryData::isCollisionPair (Index co1, Index co2)
@@ -295,6 +305,20 @@ namespace se3
     }
   }
 
+  // TODO :  give a srdf file as argument, read it, and remove corresponding
+  // pairs from list collision_pairs
+  inline void GeometryData::desactivateCollisionPairs()
+  {
+
+  }
+
+  inline void GeometryData::initializeListOfCollisionPairs()
+  {
+    fillAllPairsAsCollisions();
+    desactivateCollisionPairs();
+    assert(nCollisionPairs == collision_pairs.size());
+  }
+
   inline bool GeometryData::collide(Index co1, Index co2) 
   {
     fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
@@ -332,12 +356,18 @@ namespace se3
     return result;
   }
 
+  inline void GeometryData::resetDistances()
+  {
+    std::fill(distances.begin(), distances.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
+  }
+
   inline void GeometryData::computeDistances ()
   {
-    distances.clear();
+    std::size_t cpt = 0;
     for (std::vector<CollisionPair_t>::iterator it = collision_pairs.begin(); it != collision_pairs.end(); ++it)
     {
-      distances.push_back( DistanceResult(computeDistance(it->first, it->second), it->first, it->second));
+      distances[cpt] = DistanceResult(computeDistance(it->first, it->second), it->first, it->second);
+      cpt++;
     }
   }