[Positioner] Add Stock::distance_from_closest_cp()

master
Matteo Cypriani 10 years ago
parent 9d3c079b45
commit 36cf7c13cb

@ -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 *** */

@ -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 */

Loading…
Cancel
Save