[Positioner] Add Stock::distance_from_closest_cp()

owlps-positioner/stock.cc

@@ -21,6 +21,7 @@
 #include "autocalibrationline.hh"
 
 #include <iostream>
+#include <cmath>
 
 using namespace std ;
 
@@ -335,6 +336,32 @@ const CapturePoint* Stock::is_cp_coordinate(const Point3D &coord)
 }
 
 
+/**
+ * @returns The distance from `location` to the closest CP.
+ * @returns -1 if there are no CPs in stock.
+ */
+float Stock::distance_from_closest_cp(const Point3D &location)
+{
+  auto cp = cps.begin() ;
+  if (cp == cps.end())
+    return -1 ;
+
+  float min_dist = location.square_distance(cp->second.get_coordinates()) ;
+  ++cp ;
+
+  while (cp != cps.end())
+    {
+      float tmp_dist =
+        location.square_distance(cp->second.get_coordinates()) ;
+      if (tmp_dist < min_dist)
+        min_dist = tmp_dist ;
+      ++cp ;
+    }
+
+  return sqrtf(min_dist) ;
+}
+
+
 
 /* *** ReferencePoint operations *** */
 

owlps-positioner/stock.hh

@@ -156,6 +156,8 @@ public:
                                  const std::string &mac_receiver) ;
   /// Checks if a Point3D is the coordinate of an existing CP
   static const CapturePoint* is_cp_coordinate(const Point3D &coord) ;
+  /// Returns the distance from `location` to the closest CP
+  static float distance_from_closest_cp(const Point3D &location) ;
   //@}
 
   /** @name ReferencePoint operations */