se3::computeDistances return the index of the shortest distance.

src/algorithm/geometry.hpp

@@ -70,14 +70,14 @@ namespace se3
                                 const bool stopAtFirstCollision = false
                                 );
 
-  inline void computeDistances(GeometryData & data_geom);
+  inline std::size_t computeDistances(GeometryData & data_geom);
 
-  inline void computeDistances(const Model & model,
-                              Data & data,
-                              const GeometryModel & model_geom,
-                              GeometryData & data_geom,
-                              const Eigen::VectorXd & q
-                              );
+  inline std::size_t computeDistances(const Model & model,
+                                      Data & data,
+                                      const GeometryModel & model_geom,
+                                      GeometryData & data_geom,
+                                      const Eigen::VectorXd & q
+                                      );
 
   inline void computeBodyRadius(const Model &         model,
                                 const GeometryModel & geomModel,

src/algorithm/geometry.hxx

@@ -79,15 +79,23 @@ namespace se3
     return computeCollisions(data_geom, stopAtFirstCollision);
   }
   
-  
-  inline void computeDistances(GeometryData & data_geom)
+  inline std::size_t computeDistances(GeometryData & data_geom)
   {
+    double min_dist = std::numeric_limits<double>::infinity();
+    std::size_t min_index = data_geom.collision_pairs.size();
     for (std::size_t cpt = 0; cpt < data_geom.collision_pairs.size(); ++cpt)
+    {
       data_geom.distance_results[cpt] = data_geom.computeDistance(data_geom.collision_pairs[cpt].first,
                                                                   data_geom.collision_pairs[cpt].second);
+      if (data_geom.distance_results[cpt].distance() < min_dist) {
+        min_index = cpt;
+        min_dist = data_geom.distance_results[cpt].distance();
+      }
+    }
+    return min_index;
   }
   
-  inline void computeDistances(const Model & model,
+  inline std::size_t computeDistances(const Model & model,
                                Data & data,
                                const GeometryModel & model_geom,
                                GeometryData & data_geom,
@@ -95,7 +103,7 @@ namespace se3
                                )
   {
     updateGeometryPlacements (model, data, model_geom, data_geom, q);
-    computeDistances(data_geom);
+    return computeDistances(data_geom);
   }
 
   /// Given p1..3 being either "min" or "max", return one of the corners of the