70 lines
1.9 KiB
C++
70 lines
1.9 KiB
C++
/*
|
|
* This file is part of the Owl Positioning System (OwlPS) project.
|
|
* It is subject to the copyright notice and license terms in the
|
|
* COPYRIGHT.t2t file found in the top-level directory of this
|
|
* distribution and at
|
|
* https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
|
|
* No part of the OwlPS Project, including this file, may be copied,
|
|
* modified, propagated, or distributed except according to the terms
|
|
* contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
|
|
* distributed along with this file, either separately or by replacing
|
|
* this notice by the COPYRIGHT.t2t file's contents.
|
|
*/
|
|
|
|
|
|
#include "minmax.hh"
|
|
#include "capturepoint.hh"
|
|
|
|
using namespace std ;
|
|
|
|
|
|
|
|
Point3D MinMax::trilaterate(
|
|
const unordered_map<const CapturePoint*, float> &_cp_distances)
|
|
{
|
|
min = INFINITE ;
|
|
centre = start ;
|
|
cp_distances = &_cp_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<const CapturePoint*, float> &_cp_distances, float z)
|
|
{
|
|
min = INFINITE ;
|
|
centre = start ;
|
|
cp_distances = &_cp_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(const float x, const float y, const float z)
|
|
{
|
|
float d_max = 0 ;
|
|
for (auto i = cp_distances->begin() ; i != cp_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) ;
|
|
}
|
|
}
|