64 lines
1.5 KiB
C++
64 lines
1.5 KiB
C++
/*
|
|
* This file is part of the Owl Positioning System (OwlPS).
|
|
* OwlPS is a project of the University of Franche-Comté
|
|
* (Université de Franche-Comté), France.
|
|
*/
|
|
|
|
|
|
#include "minmax.hh"
|
|
#include "accesspoint.hh"
|
|
|
|
using std::tr1::unordered_map ;
|
|
|
|
|
|
|
|
Point3D MinMax::multilaterate(
|
|
const unordered_map<AccessPoint*, float> &_ap_distances)
|
|
{
|
|
min = INFINITE ;
|
|
centre = start ;
|
|
ap_distances = &_ap_distances ;
|
|
|
|
for (float x = start.get_x() ; x <= stop.get_x() ; x += step)
|
|
for (float y = start.get_y() ; y <= stop.get_y() ; y += step)
|
|
for (float z = start.get_z() ; z <= stop.get_z() ; z += step)
|
|
iterate(x, y, z) ;
|
|
|
|
return centre ;
|
|
}
|
|
|
|
|
|
Point3D MinMax::multilaterate_2d(
|
|
const unordered_map<AccessPoint*, float> &_ap_distances, float z)
|
|
{
|
|
min = INFINITE ;
|
|
centre = start ;
|
|
ap_distances = &_ap_distances ;
|
|
|
|
for (float x = start.get_x() ; x <= stop.get_x() ; x += step)
|
|
for (float y = start.get_y() ; y <= stop.get_y() ; y += step)
|
|
iterate(x, y, z) ;
|
|
|
|
return centre ;
|
|
}
|
|
|
|
|
|
void MinMax::iterate(float x, float y, float z)
|
|
{
|
|
float d_max = 0 ;
|
|
for (unordered_map<AccessPoint*, float>::const_iterator i =
|
|
ap_distances->begin() ; i != ap_distances->end() ; ++i)
|
|
{
|
|
float dist =
|
|
Point3D(x, y, z).distance_to_sphere(
|
|
i->first->get_coordinates(), i->second) ;
|
|
if (dist > d_max)
|
|
d_max = dist ;
|
|
}
|
|
if (d_max <= min)
|
|
{
|
|
min = d_max ;
|
|
centre.set_coordinates(x, y, z) ;
|
|
}
|
|
}
|