98 lines
3.1 KiB
C++
98 lines
3.1 KiB
C++
/*
|
|
* This file is part of the Owl Positioning System (OwlPS).
|
|
* OwlPS is a project of the University of Franche-Comte
|
|
* (Université de Franche-Comté), France.
|
|
*
|
|
* Copyright © Université de Franche-Comté 2007-2012.
|
|
*
|
|
* Corresponding author: Matteo Cypriani <mcy@lm7.fr>
|
|
*
|
|
***********************************************************************
|
|
*
|
|
* This software is governed by the CeCILL license under French law and
|
|
* abiding by the rules of distribution of free software. You can use,
|
|
* modify and/or redistribute the software under the terms of the CeCILL
|
|
* license as circulated by CEA, CNRS and INRIA at the following URL:
|
|
* http://www.cecill.info
|
|
*
|
|
* As a counterpart to the access to the source code and rights to copy,
|
|
* modify and redistribute granted by the license, users are provided
|
|
* only with a limited warranty and the software's authors, the holder
|
|
* of the economic rights, and the successive licensors have only
|
|
* limited liability.
|
|
*
|
|
* In this respect, the user's attention is drawn to the risks
|
|
* associated with loading, using, modifying and/or developing or
|
|
* reproducing the software by the user in light of its specific status
|
|
* of free software, that may mean that it is complicated to manipulate,
|
|
* and that also therefore means that it is reserved for developers and
|
|
* experienced professionals having in-depth computer knowledge. Users
|
|
* are therefore encouraged to load and test the software's suitability
|
|
* as regards their requirements in conditions enabling the security of
|
|
* their systems and/or data to be ensured and, more generally, to use
|
|
* and operate it in the same conditions as regards security.
|
|
*
|
|
* The fact that you are presently reading this means that you have had
|
|
* knowledge of the CeCILL license and that you accept its terms.
|
|
*
|
|
***********************************************************************
|
|
*/
|
|
|
|
|
|
#include "minmax.hh"
|
|
#include "accesspoint.hh"
|
|
|
|
using std::tr1::unordered_map ;
|
|
|
|
|
|
|
|
Point3D MinMax::trilaterate(
|
|
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::trilaterate_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) ;
|
|
}
|
|
}
|