37 lines
996 B
C++
37 lines
996 B
C++
#include "minmax.hh"
|
|
#include "accesspoint.hh"
|
|
|
|
using std::tr1::unordered_map ;
|
|
|
|
|
|
|
|
Point3D MinMax::multilaterate(
|
|
const unordered_map<AccessPoint*, float> &ap_distances)
|
|
{
|
|
float min = INFINITE ;
|
|
Point3D centre(start) ;
|
|
|
|
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)
|
|
{
|
|
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) ;
|
|
}
|
|
}
|
|
|
|
return centre ;
|
|
}
|