731 lines
21 KiB
C++
731 lines
21 KiB
C++
#include "stock.hh"
|
|
#include "configuration.hh"
|
|
#include "posexcept.hh"
|
|
#include "area.hh"
|
|
|
|
#include <iostream>
|
|
|
|
using namespace std ;
|
|
using std::tr1::unordered_map ;
|
|
using std::tr1::unordered_set ;
|
|
|
|
|
|
|
|
/* *** Attribute definitions *** */
|
|
|
|
unordered_map<string, Building> Stock::buildings ;
|
|
|
|
unordered_map<Point3D, Waypoint> Stock::waypoints ;
|
|
|
|
unordered_map<string, Mobile> Stock::mobiles ;
|
|
|
|
uint32_t Stock::nb_virtual_mobiles = 0 ;
|
|
|
|
unordered_map<string, AccessPoint> Stock::aps ;
|
|
|
|
unordered_set<ReferencePoint,
|
|
boost::hash<ReferencePoint>,
|
|
reference_point_equal_to> Stock::reference_points ;
|
|
|
|
unordered_set<CalibrationRequest> Stock::calibration_requests ;
|
|
|
|
|
|
void Stock::clear()
|
|
{
|
|
buildings.clear() ;
|
|
|
|
// Waypoints are normally deleted in Building::~Building(), so we must
|
|
// not clear waypoints before buildings, but we clear it *after*, just
|
|
// in case.
|
|
waypoints.clear() ;
|
|
|
|
mobiles.clear() ;
|
|
aps.clear() ;
|
|
reference_points.clear() ;
|
|
calibration_requests.clear() ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Building operations *** */
|
|
|
|
|
|
/**
|
|
* The name of the Building is initialised.
|
|
*/
|
|
const Building& Stock::find_create_building(const string &name)
|
|
{
|
|
unordered_map<string, Building>::const_iterator i =
|
|
buildings.find(name) ;
|
|
if (i != buildings.end())
|
|
return i->second ;
|
|
|
|
Building &building = buildings[name] ;
|
|
building.set_name(name) ;
|
|
return building ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param name The name of the Building to search for.
|
|
* It must be a valid name, as no check is performed.
|
|
* @return A const reference to the Building.
|
|
* @throw element_not_found is thrown if the Building corresponding
|
|
* to \em name does not exist.
|
|
*/
|
|
const Building& Stock::get_building(const string &name)
|
|
{
|
|
unordered_map<string, Building>::const_iterator i =
|
|
buildings.find(name) ;
|
|
if (i != buildings.end())
|
|
return i->second ;
|
|
throw element_not_found("No Building with name « " + name + " »!") ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @returns A pointer to the first Area in which \em point was found.
|
|
* @returns NULL if \em point does not belong to any Area.
|
|
*/
|
|
const Area* Stock::in_which_area_is(const Point3D &point)
|
|
{
|
|
for (unordered_map<string, Building>::const_iterator
|
|
b = buildings.begin() ; b != buildings.end() ; ++b)
|
|
{
|
|
const unordered_map<string, Area*> &areas = b->second.get_areas() ;
|
|
for (unordered_map<string, Area*>::const_iterator
|
|
a = areas.begin() ; a != areas.end() ; ++a)
|
|
if (a->second->contains_point(point))
|
|
return a->second ;
|
|
}
|
|
|
|
return NULL ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Waypoint operations *** */
|
|
|
|
|
|
const Waypoint& Stock::
|
|
find_create_waypoint(const Point3D &point)
|
|
{
|
|
unordered_map<Point3D, Waypoint>::const_iterator i =
|
|
waypoints.find(point) ;
|
|
if (i != waypoints.end())
|
|
return i->second ;
|
|
|
|
Waypoint &waypoint = waypoints[point] ;
|
|
waypoint.set_coordinates(point) ;
|
|
return waypoint ;
|
|
}
|
|
|
|
|
|
void Stock::waypoint_remove_building(const Waypoint &point,
|
|
const Building *building)
|
|
{
|
|
unordered_map<Point3D, Waypoint>::iterator i =
|
|
waypoints.find(point) ;
|
|
|
|
Waypoint *waypoint = const_cast<Waypoint*>(&i->second) ;
|
|
waypoint->remove_building(building) ;
|
|
|
|
// If the Waypoint is not linked to any Building any more, we
|
|
// delete it
|
|
if (waypoint->get_1st_building() == NULL)
|
|
waypoints.erase(i) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Mobile operations *** */
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the Mobile to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @return \em true if a mobile with this MAC address exists.
|
|
* @return \em false if this MAC address was not found.
|
|
*/
|
|
bool Stock::mobile_exists(const std::string &mac)
|
|
{
|
|
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
|
|
return i != mobiles.end() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the Mobile to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @return A const reference to the Mobile.
|
|
* @throw element_not_found is thrown if the Mobile corresponding
|
|
* to \em mac does not exist.
|
|
*/
|
|
const Mobile& Stock::get_mobile(const string &mac)
|
|
{
|
|
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
|
|
if (i != mobiles.end())
|
|
return i->second ;
|
|
throw element_not_found("No Mobile with MAC address « " +
|
|
mac + " »!") ;
|
|
}
|
|
|
|
|
|
/**
|
|
* If created, the MAC address of the Mobile is initialised.
|
|
*/
|
|
const Mobile& Stock::find_create_mobile(const string &mac)
|
|
{
|
|
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
|
|
if (i != mobiles.end())
|
|
return i->second ;
|
|
|
|
Mobile &mobile = mobiles[mac] ;
|
|
mobile.set_mac_addr(mac) ;
|
|
return mobile ;
|
|
}
|
|
|
|
|
|
/**
|
|
* If the Mobile already exists, it is replaced by the \em source.
|
|
*/
|
|
const Mobile& Stock::find_create_mobile(const Mobile &source)
|
|
{
|
|
const string &mac = source.get_mac_addr() ;
|
|
unordered_map<string, Mobile>::const_iterator i = mobiles.find(mac) ;
|
|
if (i != mobiles.end())
|
|
return i->second ;
|
|
|
|
mobiles[mac] = source ;
|
|
return mobiles[mac] ;
|
|
}
|
|
|
|
|
|
|
|
/* *** AccessPoint operations *** */
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the AccessPoint to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @return \em true if an AP with this MAC address exists.
|
|
* @return \em false if this MAC address was not found.
|
|
*/
|
|
bool Stock::ap_exists(const std::string &mac)
|
|
{
|
|
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
|
|
return i != aps.end() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param mac The MAC address of the AccessPoint to search for.
|
|
* It must be a valid MAC address, as no check is performed.
|
|
* @return A const reference to the AccessPoint.
|
|
* @throw element_not_found is thrown if the AccessPoint corresponding
|
|
* to \em mac does not exist.
|
|
*/
|
|
const AccessPoint& Stock::get_ap(const string &mac)
|
|
{
|
|
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
|
|
if (i != aps.end())
|
|
return i->second ;
|
|
throw element_not_found("No AccessPoint with MAC address « " +
|
|
mac + " »!") ;
|
|
}
|
|
|
|
|
|
/**
|
|
* The MAC address of the AccessPoint is initialised.
|
|
*/
|
|
const AccessPoint& Stock::find_create_ap(const string &mac)
|
|
{
|
|
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
|
|
if (i != aps.end())
|
|
return i->second ;
|
|
|
|
AccessPoint &ap = aps[mac] ;
|
|
ap.set_mac_addr(mac) ;
|
|
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< "New AP « " << mac
|
|
<< " » (total: " << nb_aps() << " APs).\n" ;
|
|
|
|
return ap ;
|
|
}
|
|
|
|
|
|
/**
|
|
* If the AccessPoint already exists, it is replaced by the \em source.
|
|
*/
|
|
const AccessPoint& Stock::find_create_ap(const AccessPoint &source)
|
|
{
|
|
const string &mac = source.get_mac_addr() ;
|
|
unordered_map<string, AccessPoint>::const_iterator i = aps.find(mac) ;
|
|
if (i != aps.end())
|
|
return i->second ;
|
|
|
|
aps[mac] = source ;
|
|
return aps[mac] ;
|
|
}
|
|
|
|
|
|
void Stock::update_all_friis_indexes()
|
|
{
|
|
for (unordered_map<string, AccessPoint>::iterator ap = aps.begin() ;
|
|
ap != aps.end() ; ++ap)
|
|
{
|
|
string ap_mac = ap->second.get_mac_addr() ;
|
|
double friis_idx_sum = 0 ;
|
|
int nb_friis_idx = 0 ;
|
|
|
|
// Compute main general term, independant from scans
|
|
double const_term = ap->second.friis_constant_term() ;
|
|
|
|
/*
|
|
* Compute indexes for each ReferencePoint. The Friis index for an
|
|
* AP is the average of all indexes in all ReferencePoint.
|
|
*/
|
|
for (unordered_set<ReferencePoint>::const_iterator rp =
|
|
reference_points.begin() ; rp != reference_points.end() ;
|
|
++rp)
|
|
{
|
|
int nb_idx = 0 ;
|
|
float ap_friis_idx_sum =
|
|
rp->friis_indexes_for_ap(ap->second, const_term, nb_idx) ;
|
|
if (nb_idx != 0)
|
|
{
|
|
friis_idx_sum += ap_friis_idx_sum ;
|
|
nb_friis_idx += nb_idx ;
|
|
}
|
|
}
|
|
|
|
if (nb_friis_idx > 0)
|
|
ap->second.set_friis_index(friis_idx_sum / nb_friis_idx) ;
|
|
}
|
|
}
|
|
|
|
|
|
double Stock::ap_matrix_get_ss(const std::string &mac_transmitter,
|
|
const std::string &mac_receiver)
|
|
{
|
|
const AccessPoint &receiver = get_ap(mac_receiver) ;
|
|
const ReferencePoint &rp =
|
|
get_reference_point(receiver.get_coordinates()) ;
|
|
|
|
return rp.average_measurements(mac_transmitter) ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @returns \em true if \em coord are the coordinates of an existing
|
|
* AP, \em false if not.
|
|
*/
|
|
bool Stock::is_ap_coordinate(const Point3D &coord)
|
|
{
|
|
for (unordered_map<string, AccessPoint>::const_iterator ap =
|
|
aps.begin() ; ap != aps.end() ; ++ap)
|
|
if (ap->second.get_coordinates() == coord)
|
|
return true ;
|
|
|
|
return false ;
|
|
}
|
|
|
|
|
|
|
|
/* *** ReferencePoint operations *** */
|
|
|
|
|
|
bool Stock::reference_point_exists(const ReferencePoint &point)
|
|
{
|
|
unordered_set<ReferencePoint>::const_iterator i =
|
|
reference_points.find(point) ;
|
|
return i != reference_points.end() ;
|
|
}
|
|
|
|
|
|
/**
|
|
* @param point Coordinates of the wanted ReferencePoint.
|
|
* @return The ReferencePoint at the given coordinates, if found.
|
|
* @throw element_not_found is thrown if no ReferencePoint exists at the
|
|
* given coordinates.
|
|
*/
|
|
const ReferencePoint& Stock::
|
|
get_reference_point(const ReferencePoint &point)
|
|
{
|
|
unordered_set<ReferencePoint>::const_iterator i =
|
|
reference_points.find(point) ;
|
|
if (i != reference_points.end())
|
|
return *i ;
|
|
throw element_not_found("No ReferencePoint with coordinates " +
|
|
static_cast<string>(point) + "!") ;
|
|
}
|
|
|
|
|
|
const ReferencePoint& Stock::
|
|
find_create_reference_point(const ReferencePoint &point)
|
|
{
|
|
// unordered_set::insert() do all the job: see the documentation at
|
|
// http://boost.org/doc/libs/1_42_0/doc/html/boost/unordered_set.html
|
|
pair<unordered_set<ReferencePoint>::iterator, bool> ret =
|
|
reference_points.insert(point) ;
|
|
return *ret.first ;
|
|
}
|
|
|
|
|
|
const ReferencePoint& Stock::
|
|
closest_reference_point(const Request &request)
|
|
{
|
|
if (reference_points.empty())
|
|
throw element_not_found(
|
|
"Cannot search for the closest reference point: reference points'"
|
|
" list is empty!") ;
|
|
|
|
bool ignore_aps = Configuration::bool_value(
|
|
"positioning.radar-ignore-ap-reference-points") ;
|
|
unordered_set<ReferencePoint>::const_iterator i =
|
|
reference_points.begin() ;
|
|
|
|
if (ignore_aps)
|
|
{
|
|
// Fast-forward to the next non-AP reference point
|
|
while (i != reference_points.end() && is_ap_coordinate(*i))
|
|
++i ;
|
|
|
|
// No non-AP reference point was found, we are forced to consider
|
|
// the AP reference points
|
|
if (i == reference_points.end())
|
|
ignore_aps = false ;
|
|
}
|
|
|
|
float distance = i->ss_square_distance(request) ;
|
|
unordered_set<ReferencePoint>::const_iterator closest = i ;
|
|
|
|
for (++i ; i != reference_points.end() ; ++i)
|
|
{
|
|
if (ignore_aps && is_ap_coordinate(*i))
|
|
continue ;
|
|
|
|
float tmp_distance = i->ss_square_distance(request) ;
|
|
if (tmp_distance < distance)
|
|
{
|
|
distance = tmp_distance ;
|
|
closest = i ;
|
|
}
|
|
}
|
|
|
|
return *closest ;
|
|
}
|
|
|
|
|
|
void Stock::regenerate_reference_points()
|
|
{
|
|
assert(! aps.empty()) ;
|
|
assert(! reference_points.empty()) ;
|
|
|
|
delete_non_ap_calibration_requests() ;
|
|
|
|
if (reference_points.size() < 3)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< reference_points.size() << " true reference points is"
|
|
<< " not enough to generate reference points.\n" ;
|
|
return ;
|
|
}
|
|
|
|
/* Generate RPs */
|
|
if (! Configuration::is_configured("positioning.area-start") ||
|
|
! Configuration::is_configured("positioning.area-stop"))
|
|
throw missing_configuration(
|
|
"You must define the deployment area in order to generate"
|
|
" reference points.") ;
|
|
Point3D start(
|
|
Configuration::string_value("positioning.area-start")) ;
|
|
Point3D stop(
|
|
Configuration::string_value("positioning.area-stop")) ;
|
|
float step_x =
|
|
Configuration::float_value("positioning.generated-meshing-grain-x") ;
|
|
float step_y =
|
|
Configuration::float_value("positioning.generated-meshing-grain-y") ;
|
|
float z = 1 ; // FIXME
|
|
for (float x = start.get_x() ; x <= stop.get_x() ; x += step_x)
|
|
for (float y = start.get_y() ; y <= stop.get_y() ; y += step_y)
|
|
//for (float z = start.get_z() ; z <= stop.get_z() ; z += step_z)
|
|
{
|
|
Point3D current_point(x,y,z) ;
|
|
generate_reference_point(current_point) ;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* The reference point P we want to generate will end-up containing
|
|
* as many calibration requests as the total number of APs.
|
|
* For each of these requests, we consider a transmitter TRX (which is
|
|
* a virtual mobile) and a receiver RX (the current AP).
|
|
* To generate the signal strength (SS) received in P by RX from TRX,
|
|
* we first select two reference APs REF1 and REF2. Then, we average
|
|
* the real measurements REF1->RX and REF2->RX, weighting in function
|
|
* of the angles REF-RX-TRX and the distance from RX to TRX to compute
|
|
* the SS.
|
|
* Hope this is clear enough…
|
|
*/
|
|
void Stock::generate_reference_point(const Point3D &point)
|
|
{
|
|
// If the point is the coordinates of an existing AP, we don't
|
|
// need to generate it
|
|
if (is_ap_coordinate(point))
|
|
return ;
|
|
|
|
/* Get/create the reference point */
|
|
const ReferencePoint ¤t_rp =
|
|
find_create_reference_point(point) ;
|
|
|
|
/* Create the timestamp */
|
|
Timestamp time_sent ;
|
|
time_sent.now() ;
|
|
|
|
/* Create the measurement list */
|
|
unordered_map<string, Measurement> measurements ;
|
|
|
|
/* Prepare the virtual mobile */
|
|
string vmob_mac(PosUtil::int_to_mac(nb_virtual_mobiles++)) ;
|
|
// The gain and trx power of the mobile will be the average of
|
|
// those of all the known APs (could be better, probably)
|
|
double vmob_gain = 0 ;
|
|
double vmob_pow = 0 ;
|
|
|
|
for (unordered_map<string, AccessPoint>::const_iterator
|
|
rx = aps.begin() ; rx != aps.end() ; ++rx)
|
|
{
|
|
/* Update the mobile's attributes */
|
|
vmob_gain += rx->second.get_antenna_gain() / aps.size() ;
|
|
vmob_pow += rx->second.get_trx_power() / aps.size() ;
|
|
|
|
/* Choose the 2 nearest APs in angle */
|
|
const Point3D &rx_coord =
|
|
rx->second.get_coordinates() ;
|
|
multimap<double,
|
|
unordered_map<string, AccessPoint>::const_iterator>
|
|
sorted_angles ;
|
|
for (unordered_map<string, AccessPoint>::const_iterator
|
|
ref = aps.begin() ; ref != aps.end() ; ++ref)
|
|
{
|
|
if (ref == rx)
|
|
continue ;
|
|
const Point3D &ref_coord =
|
|
ref->second.get_coordinates() ;
|
|
// Skip the AP if the associated reference point does
|
|
// not exist yet:
|
|
if (! reference_point_exists(ref_coord))
|
|
continue ;
|
|
double angle =
|
|
rx_coord.angle(point, ref_coord) ;
|
|
pair<double,
|
|
unordered_map<string, AccessPoint>::const_iterator>
|
|
angle_ap(angle, ref) ;
|
|
sorted_angles.insert(angle_ap) ;
|
|
}
|
|
assert(sorted_angles.size() > 1) ;
|
|
|
|
/* Compute the angle weight of the 2 reference APs */
|
|
map<double,
|
|
unordered_map<string, AccessPoint>::const_iterator>
|
|
::const_iterator s = sorted_angles.begin() ;
|
|
// Angle REF1-RX-P
|
|
double ref1_angle = s->first ;
|
|
const AccessPoint &ref1 = s->second->second ;
|
|
++s ;
|
|
const AccessPoint &ref2 = s->second->second ;
|
|
|
|
// Angle REF1-RX-REF2
|
|
const Point3D &ref1_coord = ref1.get_coordinates() ;
|
|
const Point3D &ref2_coord = ref2.get_coordinates() ;
|
|
double reference_angle =
|
|
rx_coord.angle(ref1_coord, ref2_coord) ;
|
|
double ref1_percent, ref2_percent ;
|
|
if (reference_angle == 0)
|
|
ref1_percent = ref2_percent = 50 ;
|
|
else
|
|
{
|
|
ref1_percent = ref1_angle * 100 / reference_angle ;
|
|
if (ref1_percent > 100)
|
|
ref1_percent = 100 ;
|
|
ref2_percent = 100 - ref1_percent ;
|
|
}
|
|
|
|
/* Compute the SS that would be received from a mobile at
|
|
* distance of RX-P, in the direction of each reference AP */
|
|
// Distance RX-P
|
|
float point_dst =
|
|
rx_coord.distance(point) ;
|
|
// Constant term
|
|
double common_ss = rx->second.friis_constant_term() ;
|
|
// SS for REF1
|
|
const ReferencePoint &ref1_rp =
|
|
get_reference_point(ref1_coord) ;
|
|
double friis_index =
|
|
ref1_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
|
|
double ref1_ss =
|
|
common_ss +
|
|
ref1.get_trx_power() +
|
|
ref1.get_antenna_gain() -
|
|
10 * friis_index * log10(point_dst) ;
|
|
// SS for REF2
|
|
const ReferencePoint &ref2_rp =
|
|
get_reference_point(ref2_coord) ;
|
|
friis_index =
|
|
ref2_rp.friis_index_for_ap(rx->second.get_mac_addr()) ;
|
|
double ref2_ss =
|
|
common_ss +
|
|
ref2.get_trx_power() +
|
|
ref2.get_antenna_gain() -
|
|
10 * friis_index * log10(point_dst) ;
|
|
|
|
/* Compute the SS */
|
|
ref1_ss = ref1_ss * ref1_percent / 100 ;
|
|
ref2_ss = ref2_ss * ref2_percent / 100 ;
|
|
double rx_ss = ref1_ss + ref2_ss ;
|
|
|
|
/* Create the measurement, add it to the list */
|
|
Measurement m(&rx->second) ;
|
|
m.add_ss(rx_ss) ;
|
|
measurements[rx->second.get_mac_addr()] = m ;
|
|
}
|
|
|
|
/* Create the virtual mobile */
|
|
Mobile vmob("", vmob_mac, vmob_gain, vmob_pow) ;
|
|
const Mobile &mobile = find_create_mobile(vmob) ;
|
|
|
|
/* Create the calibration request */
|
|
CalibrationRequest cr(OWL_REQUEST_GENERATED) ;
|
|
cr.set_time_sent(time_sent) ;
|
|
cr.set_mobile(&mobile) ;
|
|
cr.set_measurements(measurements) ;
|
|
cr.set_reference_point(¤t_rp) ;
|
|
|
|
store_calibration_request(cr) ;
|
|
}
|
|
|
|
|
|
|
|
/* *** CalibrationRequest operations *** */
|
|
|
|
|
|
/**
|
|
* @returns The CalibrationRequest created or found.
|
|
*/
|
|
const CalibrationRequest& Stock::
|
|
store_calibration_request(const CalibrationRequest &request)
|
|
{
|
|
const CalibrationRequest &calibration_request =
|
|
find_create_calibration_request(request) ;
|
|
calibration_request.reference_point_backward_link() ;
|
|
return calibration_request ;
|
|
}
|
|
|
|
|
|
void Stock::
|
|
delete_calibration_request(const CalibrationRequest &request)
|
|
{
|
|
#ifndef NDEBUG
|
|
unordered_set<CalibrationRequest>::const_iterator found =
|
|
calibration_requests.find(request) ;
|
|
if (found == calibration_requests.end())
|
|
{
|
|
cerr
|
|
<< "Error! Cannot delete an unstored CalibrationRequest: "
|
|
<< request << endl ;
|
|
return ;
|
|
}
|
|
#endif // NDEBUG
|
|
|
|
calibration_requests.erase(request) ;
|
|
}
|
|
|
|
|
|
const CalibrationRequest& Stock::
|
|
find_create_calibration_request(const CalibrationRequest &request)
|
|
{
|
|
pair<unordered_set<CalibrationRequest>::iterator, bool> ret =
|
|
calibration_requests.insert(request) ;
|
|
return *ret.first ;
|
|
}
|
|
|
|
|
|
const CalibrationRequest& Stock::
|
|
closest_calibration_request(const Request &request)
|
|
{
|
|
if (calibration_requests.empty())
|
|
throw element_not_found(
|
|
"Cannot search for the closest calibration request: calibration"
|
|
" requests' list is empty!") ;
|
|
|
|
bool ignore_aps = Configuration::bool_value(
|
|
"positioning.radar-ignore-ap-reference-points") ;
|
|
unordered_set<CalibrationRequest>::const_iterator i =
|
|
calibration_requests.begin() ;
|
|
|
|
if (ignore_aps)
|
|
{
|
|
// Fast-forward to the next non-AP reference point
|
|
while (i != calibration_requests.end() &&
|
|
is_ap_coordinate(*i->get_reference_point()))
|
|
++i ;
|
|
|
|
// No non-AP reference point was found, we are forced to consider
|
|
// the AP reference points
|
|
if (i == calibration_requests.end())
|
|
ignore_aps = false ;
|
|
}
|
|
|
|
float distance = i->ss_square_distance(request) ;
|
|
unordered_set<CalibrationRequest>::const_iterator closest = i ;
|
|
|
|
for (++i ; i != calibration_requests.end() ; ++i)
|
|
{
|
|
if (ignore_aps && is_ap_coordinate(*i->get_reference_point()))
|
|
continue ;
|
|
|
|
float tmp_distance = i->ss_square_distance(request) ;
|
|
if (tmp_distance < distance)
|
|
{
|
|
distance = tmp_distance ;
|
|
closest = i ;
|
|
}
|
|
}
|
|
|
|
return *closest ;
|
|
}
|
|
|
|
|
|
void Stock::delete_non_ap_calibration_requests()
|
|
{
|
|
unordered_set<ReferencePoint>::iterator rp = reference_points.begin() ;
|
|
while (rp != reference_points.end())
|
|
{
|
|
ReferencePoint rp_copy(*rp) ;
|
|
if (rp_copy.delete_generated_requests()) // rp_copy was modified
|
|
{
|
|
rp = reference_points.erase(rp) ;
|
|
// Reinsert the modified copy if it still contains at least
|
|
// one CalibrationRequest:
|
|
if (! rp_copy.get_requests().empty())
|
|
reference_points.insert(rp_copy) ;
|
|
/* __Note on the above insert__
|
|
* From the boost::unordered documentation:
|
|
* "insert is only allowed to invalidate iterators when the
|
|
* insertion causes the load factor to be greater than or
|
|
* equal to the maximum load factor."
|
|
* In our case, we always remove an element prior to insert,
|
|
* so the load factor will never change; therefore insert()
|
|
* should be safe here.
|
|
*/
|
|
}
|
|
else // rp_copy was not modified
|
|
++rp ;
|
|
}
|
|
}
|