Make computeDistances templated functions

src/algorithm/geometry.hpp

@@ -70,8 +70,18 @@ namespace se3
                                 const bool stopAtFirstCollision = false
                                 );
 
+  /// Compute the distances of all collision pairs
+  ///
+  /// \param ComputeShortest default to true.
+  /// \param data_geom
+  /// \return When ComputeShortest is true, the index of the collision pair which has the shortest distance.
+  ///         When ComputeShortest is false, the number of collision pairs.
+  template <bool ComputeShortest>
   inline std::size_t computeDistances(GeometryData & data_geom);
 
+  /// Compute the forward kinematics, update the goemetry placements and
+  /// calls computeDistances(GeometryData&).
+  template <bool ComputeShortest>
   inline std::size_t computeDistances(const Model & model,
                                       Data & data,
                                       const GeometryModel & model_geom,

src/algorithm/geometry.hxx

@@ -78,7 +78,14 @@ namespace se3
     
     return computeCollisions(data_geom, stopAtFirstCollision);
   }
+
+  // Required to have a default template argument on templated free function
+  inline std::size_t computeDistances(GeometryData & data_geom)
+  {
+    return computeDistances<true>(data_geom);
+  }
   
+  template <bool ComputeShortest>
   inline std::size_t computeDistances(GeometryData & data_geom)
   {
     double min_dist = std::numeric_limits<double>::infinity();
@@ -87,7 +94,7 @@ namespace se3
     {
       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) {
+      if (ComputeShortest && data_geom.distance_results[cpt].distance() < min_dist) {
         min_index = cpt;
         min_dist = data_geom.distance_results[cpt].distance();
       }
@@ -95,6 +102,18 @@ namespace se3
     return min_index;
   }
   
+  // Required to have a default template argument on templated free function
+  inline std::size_t computeDistances(const Model & model,
+                               Data & data,
+                               const GeometryModel & model_geom,
+                               GeometryData & data_geom,
+                               const Eigen::VectorXd & q
+                               )
+  {
+    return computeDistances<true>(model, data, model_geom, data_geom, q);
+  }
+
+  template <bool ComputeShortest>
   inline std::size_t computeDistances(const Model & model,
                                Data & data,
                                const GeometryModel & model_geom,
@@ -103,7 +122,7 @@ namespace se3
                                )
   {
     updateGeometryPlacements (model, data, model_geom, data_geom, q);
-    return computeDistances(data_geom);
+    return computeDistances<ComputeShortest>(data_geom);
   }
 
   /// Given p1..3 being either "min" or "max", return one of the corners of the