737 lines
23 KiB
C++
737 lines
23 KiB
C++
#include "positioning.hh"
|
|
|
|
template <class T>
|
|
inline T from_string(const string &s)
|
|
{
|
|
|
|
T t;
|
|
istringstream iss(s);
|
|
|
|
iss >> t;
|
|
|
|
return t;
|
|
}
|
|
|
|
void Positioning::makeReferencePointListFromFile(const string &filename, const bool uniq_point)
|
|
{
|
|
|
|
ifstream input_file ; // Flux d'entrée du fichier.
|
|
string cpp_buffer ; // Buffer au format string.
|
|
ReferencePoint rp;
|
|
Point tmp_point;
|
|
float x, y, z ; // Coordonnées des points.
|
|
unsigned int pt_idx = 0 ; // Position du point lu dans la liste.
|
|
int direction;
|
|
vector<string> infos ; // Liste des informations lues dans une ligne du fichier.
|
|
|
|
input_file.open(filename.c_str()) ;
|
|
if (input_file.fail())
|
|
{
|
|
cerr << "Error opening input file « " << filename << " » !" << endl ;
|
|
return ;
|
|
}
|
|
|
|
while (!input_file.eof())
|
|
{
|
|
getline(input_file, cpp_buffer);
|
|
|
|
if ((input_file.rdstate() & ifstream::eofbit) == 0)
|
|
{
|
|
if (cpp_buffer.size() == 0) // Ignorer une ligne vide
|
|
continue ;
|
|
|
|
infos = extractReferencePointInfoFromBuffer(cpp_buffer);
|
|
x = from_string<float>(infos[0]);
|
|
y = from_string<float>(infos[1]);
|
|
z = from_string<float>(infos[2]);
|
|
|
|
tmp_point.setX(x);
|
|
tmp_point.setY(y);
|
|
tmp_point.setZ(z);
|
|
direction = from_string<int>(infos[3]);
|
|
|
|
if (!uniq_point || !pointExists(reference_point_list, tmp_point)) // Si on ne veut pas de points unique, ou que le point n'existe pas encore,
|
|
{
|
|
rp.setCoordinates(tmp_point);
|
|
reference_point_list.push_back(rp); // on le crée.
|
|
pt_idx = reference_point_list.size() - 1; // Le point que l'on vient d'ajouter est le dernier du vector.
|
|
}
|
|
else // Le point existe déjà :
|
|
{
|
|
pt_idx = pointIndex(reference_point_list, tmp_point) ; // On recherche le point auquel on veut ajouter les informations.
|
|
}
|
|
|
|
for (unsigned int i = 4; i < infos.size(); i++)
|
|
{
|
|
if (i+1 < infos.size())
|
|
{
|
|
reference_point_list[pt_idx].addMeasurement(infos[i], from_string<int>(infos[i+1]));
|
|
i++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
input_file.close();
|
|
infos.clear() ;
|
|
}
|
|
|
|
void Positioning::makeApListFromFile(const string &filename)
|
|
{
|
|
|
|
ifstream input_file;
|
|
string buffer;
|
|
vector<string> ap_infos;
|
|
AccessPoint tmp_ap;
|
|
|
|
input_file.open(filename.c_str());
|
|
if (input_file.fail())
|
|
{
|
|
cerr << "Error opening input file « " << filename << " » !" << endl ;
|
|
return ;
|
|
}
|
|
|
|
while (!input_file.eof())
|
|
{
|
|
getline(input_file, buffer);
|
|
|
|
if ((input_file.rdstate() & ifstream::eofbit) == 0)
|
|
{
|
|
if (buffer[0] == '\0' || buffer[0] == '#') // ligne vide ou ligne commençant par #
|
|
continue;
|
|
|
|
ap_infos = explode(buffer, ';');
|
|
tmp_ap.setApAddr(ap_infos[0]);
|
|
tmp_ap.setCoordinates(from_string<float>(ap_infos[1]), from_string<float>(ap_infos[2]), from_string<float>(ap_infos[3]));
|
|
tmp_ap.setFrequency(from_string<unsigned int>(ap_infos[4]));
|
|
tmp_ap.setAntennaGain(from_string<float>(ap_infos[5]));
|
|
tmp_ap.setOutputPower(from_string<float>(ap_infos[6]));
|
|
access_point_list.push_back(tmp_ap);
|
|
ap_infos.clear();
|
|
}
|
|
}
|
|
|
|
input_file.close();
|
|
}
|
|
|
|
void Positioning::makeTopologyFromFile(const string &filename)
|
|
{
|
|
|
|
ifstream input_file;
|
|
string buffer;
|
|
vector<string> infos;
|
|
|
|
input_file.open(filename.c_str()) ;
|
|
if (input_file.fail())
|
|
{
|
|
cerr << "Error opening input file « " << filename << " » !" << endl ;
|
|
return ;
|
|
}
|
|
|
|
while (!input_file.eof())
|
|
{
|
|
getline(input_file, buffer) ;
|
|
|
|
if ((input_file.rdstate() & ifstream::eofbit) == 0)
|
|
{
|
|
/* Traitement basique des commentaires */
|
|
if (buffer[0] == '\0' || buffer[0] == '#') // ligne vide ou ligne commençant par #
|
|
continue ; // ignorer cette ligne.
|
|
|
|
infos = explode(buffer, ';') ;
|
|
if (area_list.find(infos[0]) != area_list.end()) // Si la pièce existe déjà,
|
|
cerr << "Erreur ! Déclaration multiple de la zone « " << infos[0] << " »." << endl ; // on le signale ;
|
|
else // sinon on l'ajoute :
|
|
area_list[infos[0]] = Area(infos[0], from_string<float>(infos[1]), from_string<float>(infos[3]),
|
|
from_string<float>(infos[2]), from_string<float>(infos[4]),
|
|
from_string<float>(infos[5]), from_string<float>(infos[6])) ;
|
|
|
|
infos.clear() ;
|
|
}
|
|
}
|
|
|
|
input_file.close() ;
|
|
|
|
}
|
|
|
|
void Positioning::makeWaypointListFromFile(const string &filename)
|
|
{
|
|
|
|
ifstream input_file ;
|
|
string buffer;
|
|
vector<string> infos ;
|
|
set<Point> point_list ;
|
|
|
|
input_file.open(filename.c_str());
|
|
if (input_file.fail())
|
|
{
|
|
cerr << "Error opening input file « " << filename << " » !" << endl ;
|
|
return ;
|
|
}
|
|
|
|
while (!input_file.eof())
|
|
{
|
|
getline(input_file, buffer);
|
|
|
|
if ((input_file.rdstate() & ifstream::eofbit) == 0)
|
|
{
|
|
if (buffer[0] == '\0' || buffer[0] == '#') // ligne vide ou ligne commençant par #
|
|
continue;
|
|
|
|
infos = explode(buffer, ';') ;
|
|
|
|
Point tmp_pt(from_string<float>(infos[0]), from_string<float>(infos[1]), from_string<float>(infos[2])) ;
|
|
if (! point_list.insert(tmp_pt).second)
|
|
cerr << "Point " << tmp_pt << " non ajouté ! Peut-être est-il dupliqué ?" << endl ;
|
|
|
|
infos.clear() ;
|
|
}
|
|
}
|
|
input_file.close(); // Lecture du fichier terminée
|
|
|
|
makeWaypointMatrix(point_list);
|
|
}
|
|
|
|
void Positioning::makeReferencePointListFromDb(PGconn *conn, const bool uniq)
|
|
{
|
|
|
|
PGresult *res;
|
|
Point tmp_point;
|
|
ReferencePoint rp;
|
|
|
|
ostringstream oss;
|
|
string buf;
|
|
|
|
buf = "SELECT * FROM ref_point;";
|
|
|
|
res = PQexec(conn, buf.c_str());
|
|
if (PQresultStatus(res) != PGRES_TUPLES_OK)
|
|
{
|
|
cerr << "Commande incorrecte: " << PQerrorMessage(conn);
|
|
}
|
|
else
|
|
{
|
|
int nbRes = PQntuples(res);
|
|
for (int i = 0; i < nbRes; i++)
|
|
{
|
|
tmp_point.setX(from_string<float>(PQgetvalue(res, i, 1)));
|
|
tmp_point.setY(from_string<float>(PQgetvalue(res, i, 2)));
|
|
tmp_point.setZ(from_string<float>(PQgetvalue(res, i, 3)));
|
|
|
|
if (!uniq || !pointExists(reference_point_list, tmp_point)) // Si on ne veut pas de points unique, ou que le point n'existe pas encore,
|
|
{
|
|
rp.setCoordinates(tmp_point);
|
|
reference_point_list.push_back(rp); // on le crée.
|
|
}
|
|
}
|
|
PQclear(res);
|
|
|
|
for (unsigned int i = 0; i < reference_point_list.size(); i++)
|
|
{
|
|
|
|
tmp_point = reference_point_list[i].getCoordinates();
|
|
|
|
oss << "SELECT ap.ap_addr, n.power ";
|
|
oss << "FROM ap, ref_packet n, ref_request r, ref_point p ";
|
|
oss << "WHERE ap.id_ap = n.id_ap ";
|
|
oss << "AND n.id_ref_request = r.id_ref_request ";
|
|
oss << "AND r.id_ref_point = p.id_ref_point ";
|
|
oss << "AND p.x = " << tmp_point.getX() << " ";
|
|
oss << "AND p.y = " << tmp_point.getY() << " ";
|
|
oss << "AND p.z = " << tmp_point.getZ() << ";";
|
|
|
|
buf = oss.str();
|
|
oss.str("");
|
|
|
|
res = PQexec(conn, buf.c_str());
|
|
|
|
if (PQresultStatus(res) != PGRES_TUPLES_OK)
|
|
{
|
|
cerr << "Commande incorrecte: " << PQerrorMessage(conn);
|
|
}
|
|
else
|
|
{
|
|
nbRes = PQntuples(res);
|
|
for (int j = 0; j < nbRes; j++)
|
|
reference_point_list[i].addMeasurement(PQgetvalue(res, j, 0), from_string<int>(PQgetvalue(res, j, 1)));
|
|
}
|
|
|
|
PQclear(res);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void Positioning::makeApListFromDb(PGconn *conn)
|
|
{
|
|
|
|
PGresult *res;
|
|
AccessPoint tmp_ap;
|
|
string buf;
|
|
|
|
buf = "SELECT * FROM ap;";
|
|
|
|
res = PQexec(conn, buf.c_str());
|
|
if (PQresultStatus(res) != PGRES_TUPLES_OK)
|
|
{
|
|
cerr << "Commande incorrecte: " << PQerrorMessage(conn);
|
|
}
|
|
else
|
|
{
|
|
int nbRes = PQntuples(res);
|
|
for (int i = 0; i < nbRes; i++)
|
|
{
|
|
tmp_ap.setApAddr(PQgetvalue(res, i, 1));
|
|
tmp_ap.setCoordinates(from_string<float>(PQgetvalue(res, i, 2)), from_string<float>(PQgetvalue(res, i, 3)), from_string<float>(PQgetvalue(res, i, 4)));
|
|
tmp_ap.setFriisIndex(from_string<float>(PQgetvalue(res, i, 5)));
|
|
tmp_ap.setOutputPower(from_string<float>(PQgetvalue(res, i, 8)));
|
|
tmp_ap.setAntennaGain(from_string<float>(PQgetvalue(res, i, 7)));
|
|
tmp_ap.setFrequency(from_string<unsigned int>(PQgetvalue(res, i, 6)));
|
|
access_point_list.push_back(tmp_ap);
|
|
}
|
|
}
|
|
|
|
PQclear(res);
|
|
}
|
|
|
|
void Positioning::makeTopologyFromDb(PGconn *conn)
|
|
{
|
|
|
|
PGresult *res;
|
|
string buf;
|
|
|
|
buf = "SELECT * FROM area;";
|
|
|
|
res = PQexec(conn, buf.c_str());
|
|
if (PQresultStatus(res) != PGRES_TUPLES_OK)
|
|
{
|
|
cerr << "Commande incorrecte: " << PQerrorMessage(conn);
|
|
}
|
|
else
|
|
{
|
|
int nbRes = PQntuples(res);
|
|
for (int i = 0; i < nbRes; i++)
|
|
{
|
|
if (area_list.find(PQgetvalue(res, i, 1)) != area_list.end()) // Si la pièce existe déjà,
|
|
cerr << "Erreur ! Déclaration multiple de la zone « " << PQgetvalue(res, i, 1) << " »." << endl ; // on le signale ;
|
|
else
|
|
area_list[PQgetvalue(res, i, 1)] = Area(PQgetvalue(res, i, 1), from_string<float>(PQgetvalue(res, i, 2)), from_string<float>(PQgetvalue(res, i, 5)),
|
|
from_string<float>(PQgetvalue(res, i, 3)), from_string<float>(PQgetvalue(res, i, 6)),
|
|
from_string<float>(PQgetvalue(res, i, 4)), from_string<float>(PQgetvalue(res, i, 7)));
|
|
}
|
|
}
|
|
PQclear(res);
|
|
}
|
|
|
|
void Positioning::makeWaypointListFromDb(PGconn *conn)
|
|
{
|
|
|
|
PGresult *res;
|
|
string buf;
|
|
set<Point> point_list ;
|
|
|
|
buf = "SELECT * FROM waypoint;";
|
|
|
|
res = PQexec(conn, buf.c_str());
|
|
if (PQresultStatus(res) != PGRES_TUPLES_OK)
|
|
{
|
|
cerr << "Commande incorrecte: " << PQerrorMessage(conn);
|
|
}
|
|
else
|
|
{
|
|
int nbRes = PQntuples(res);
|
|
for (int i = 0; i < nbRes; i++)
|
|
{
|
|
Point tmp_pt(from_string<float>(PQgetvalue(res, i, 0)), from_string<float>(PQgetvalue(res, i, 1)), from_string<float>(PQgetvalue(res, i, 2)));
|
|
if (! point_list.insert(tmp_pt).second)
|
|
cerr << "Point " << tmp_pt << " non ajouté ! Peut-être est-il dupliqué ?" << endl ;
|
|
}
|
|
makeWaypointMatrix(point_list);
|
|
}
|
|
|
|
PQclear(res);
|
|
}
|
|
|
|
void Positioning::makeWaypointMatrix(set<Point> point_list)
|
|
{
|
|
|
|
unsigned int nb_pts = point_list.size() ; // Nombre de points
|
|
unsigned int cur_idx = 0 ; // Itérateur sur l'indice de la matrice.
|
|
waypoint_list.reserve(nb_pts) ; // Vector contenant tous les points (attribut du serveur)
|
|
waypoint_matrix = new float*[nb_pts] ; // Matrice (carrée) des distances entre les points de passage (attribut du serveur)
|
|
|
|
for (unsigned int i = 0 ; i < nb_pts ; i++) // Initialisation de la matrice :
|
|
{
|
|
waypoint_matrix[i] = new float[nb_pts] ;
|
|
for (unsigned int k = 0 ; k < nb_pts ; k++) // Initialisation des distances à -1
|
|
waypoint_matrix[i][k] = -1 ;
|
|
}
|
|
|
|
for (set<Point>::iterator it = point_list.begin() ; it != point_list.end() ; it++, cur_idx++) // Pour chaque point enregistré
|
|
{
|
|
waypoint_list.push_back(*it) ; // Ajout du point dans waypoint_list
|
|
waypoint_matrix[cur_idx][cur_idx] = 0 ; // La distance du point à lui-même est nulle
|
|
|
|
for (unsigned int k = 0 ; k < cur_idx ; k++) // Pour chacun des point précédent
|
|
{
|
|
if (inTheSameArea(waypoint_list[cur_idx], waypoint_list[k])) // Si le point est dans la même zone,
|
|
{
|
|
float dist = it->distance(waypoint_list[k]) ; // on calcule la distance
|
|
waypoint_matrix[cur_idx][k] = dist ; // et on l'insère dans le tableau,
|
|
waypoint_matrix[k][cur_idx] = dist ; // dans les deux sens.
|
|
}
|
|
}
|
|
}
|
|
|
|
bool comput_done = true ;
|
|
while (comput_done)
|
|
{
|
|
comput_done = false ;
|
|
for (unsigned int i = 0 ; i < nb_pts ; i++)
|
|
for (unsigned int j = 0 ; j < nb_pts ; j++)
|
|
if (waypoint_matrix[i][j] > 0)
|
|
for (unsigned int k = 0 ; k < nb_pts ; k++)
|
|
if (waypoint_matrix[j][k] > 0)
|
|
{
|
|
float tmp_dist = round_float((waypoint_matrix[j][k] + waypoint_matrix[i][j]), 2);
|
|
if (tmp_dist < round_float(waypoint_matrix[i][k], 2) || waypoint_matrix[i][k] == -1)
|
|
{
|
|
waypoint_matrix[i][k] = tmp_dist ;
|
|
comput_done = true ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
map<string, Area> Positioning::inWhichAreas(const Point &p)
|
|
{
|
|
|
|
map<string, Area> areas ;
|
|
|
|
for (map<string, Area>::iterator it = area_list.begin() ; it != area_list.end() ; it++)
|
|
if (it->second.containsPoint(p))
|
|
areas.insert(make_pair(it->first, it->second)) ;
|
|
|
|
return areas;
|
|
|
|
}
|
|
|
|
bool Positioning::inTheSameArea(const Point &p1, const Point &p2)
|
|
{
|
|
|
|
// Liste des zones comportant les points p1 et p2 :
|
|
map<string, Area> zones1(inWhichAreas(p1));
|
|
map<string, Area> zones2(inWhichAreas(p2));
|
|
|
|
if (zones1.size() == 0 || zones2.size() == 0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for (map<string, Area>::iterator it1 = zones1.begin() ; it1 != zones1.end() ; it1++) // Parcours des zones :
|
|
for (map<string, Area>::iterator it2 = zones2.begin(); it2 != zones2.end(); it2++)
|
|
if (it1->second == it2->second)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false ;
|
|
}
|
|
|
|
float Positioning::distanceTopology(const Point &p1, const Point &p2)
|
|
{
|
|
|
|
map<string, Area> zones1(inWhichAreas(p1));
|
|
map<string, Area> zones2(inWhichAreas(p2));
|
|
|
|
if (zones1.size() == 0 || zones2.size() == 0) // Si aucune zone ne contient p1 ou p2,
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
Area z1(zones1.begin() -> second); // Première zone d'appartenance du point p1. // FIXME : vérifier toutes les zones
|
|
Area z2(zones2.begin() -> second);
|
|
vector<Point> connection;
|
|
|
|
|
|
/* *** Cas de la même zone *** */
|
|
if (z1 == z2) // Si les points sont dans la même zone,
|
|
{
|
|
return p1.distance(p2) ; // la distance entre eux est la distance euclidienne.
|
|
}
|
|
|
|
|
|
/* *** Cas de deux zones voisines (connectées directement) *** */
|
|
if ((connection = areaConnection(z1, z2)).size() != 0) // Si les points sont dans deux zones voisines,
|
|
{
|
|
// la distance entre eux est la distance euclidienne de chacun jusqu'au plus proche point de passage entre les deux zones.
|
|
float dist = p1.distance(connection[0]) + p2.distance(connection[0]) ;
|
|
for (unsigned int i = 1 ; i < connection.size() ; i++) // Recherche du point de passage offrant la plus courte distance
|
|
{
|
|
float tmp_dist = p1.distance(connection[i]) + p2.distance(connection[i]) ;
|
|
if (tmp_dist < dist)
|
|
dist = tmp_dist ;
|
|
}
|
|
|
|
return dist ;
|
|
}
|
|
|
|
|
|
/* *** Les points sont dans des zones non-voisines *** */
|
|
// Liste des points de passage appartenant respectivement à z1 et à z2.
|
|
vector<Point> wp_z1(areaConnection(z1));
|
|
vector<Point> wp_z2(areaConnection(z2));
|
|
|
|
// Indices des points de passage dans la liste :
|
|
int wp_z1_idx;
|
|
int wp_z2_idx = -1 ; // z2 ne peut pas être utilisé non initialisé, contrairement à ce que dit GCC, car dans le cas où il n'est pas initialisé on quitte la fonction avant de l'utiliser (cf. "return -1" un peu plus bas);
|
|
|
|
if (wp_z1.size() == 0 || wp_z2.size() == 0) // Si l'une des deux zones ne comporte aucun point de passage,
|
|
{
|
|
return -1 ; // on quitte (ceci ne devrait jamais se produire).
|
|
}
|
|
|
|
/* Première itération (voir explication de l'algorithme plus bas) */
|
|
unsigned int i1, i2 ;
|
|
for (i1 = 0 ; i1 < wp_z1.size() ; i1++) // Recherche des premiers indices valides (où il existe un chemin entre les points de passage)
|
|
{
|
|
wp_z1_idx = pointIndex(waypoint_list, wp_z1[i1]) ;
|
|
for (i2 = 0 ; i2 < wp_z2.size() ; i2++)
|
|
{
|
|
wp_z2_idx = pointIndex(waypoint_list, wp_z2[i2]) ;
|
|
if (wp_z2_idx < 0)
|
|
continue ;
|
|
|
|
if (waypoint_matrix[wp_z1_idx][wp_z2_idx] != -1) // Si c'est bon,
|
|
break ; // on arrête le parcours.
|
|
}
|
|
if (waypoint_matrix[wp_z1_idx][wp_z2_idx] != -1) // Si c'est bon,
|
|
break ; // on arrête le parcours.
|
|
}
|
|
|
|
if (i1 >= wp_z1.size()) // Si on a parcouru tout le tableau sans trouver de chemin,
|
|
{
|
|
return -1 ; // on quitte.
|
|
}
|
|
|
|
float dist = p1.distance(wp_z1[i1]) + waypoint_matrix[wp_z1_idx][wp_z2_idx] + p2.distance(wp_z2[i2]);
|
|
float tmp_dist ;
|
|
|
|
for (unsigned int j = i2 + 1 ; j < wp_z2.size() ; j++) // Fin du parcours :
|
|
{
|
|
wp_z2_idx = pointIndex(waypoint_list, wp_z2[j]) ;
|
|
if (waypoint_matrix[wp_z1_idx][wp_z2_idx] == -1)
|
|
continue ;
|
|
tmp_dist = p1.distance(wp_z1[i1]) + waypoint_matrix[wp_z1_idx][wp_z2_idx] + p2.distance(wp_z2[j]) ;
|
|
if (tmp_dist < dist)
|
|
dist = tmp_dist ;
|
|
}
|
|
|
|
/* Itérations suivantes */
|
|
for (unsigned int i = i1 + 1 ; i < wp_z1.size() ; i++)
|
|
for (unsigned int j = 0 ; j < wp_z2.size() ; j++)
|
|
{
|
|
// Indice des points de passage wp_z1[i] et wp_z2[j] dans la liste des points de passage :
|
|
wp_z1_idx = pointIndex(waypoint_list, wp_z1[i]) ;
|
|
wp_z2_idx = pointIndex(waypoint_list, wp_z2[j]) ;
|
|
|
|
if (waypoint_matrix[wp_z1_idx][wp_z2_idx] == -1) // S'il n'existe aucun chemin entre les deux points de passage
|
|
continue ;
|
|
|
|
tmp_dist = p1.distance(wp_z1[i]) + waypoint_matrix[wp_z1_idx][wp_z2_idx] + p2.distance(wp_z2[j]) ;
|
|
if (tmp_dist < dist)
|
|
dist = tmp_dist ;
|
|
}
|
|
|
|
return dist ;
|
|
}
|
|
|
|
vector<Point> Positioning::areaConnection(const Area &z1, const Area &z2)
|
|
{
|
|
vector<Point> points ;
|
|
|
|
for (unsigned int i = 0 ; i < waypoint_list.size() ; i++)
|
|
{
|
|
map<string, Area> areas(inWhichAreas(waypoint_list[i])) ;
|
|
if (areas.find(z1.getName()) != areas.end() && areas.find(z2.getName()) != areas.end())
|
|
points.push_back(waypoint_list[i]) ;
|
|
}
|
|
|
|
return points ;
|
|
}
|
|
|
|
|
|
|
|
vector<Point> Positioning::areaConnection(const Area &z)
|
|
{
|
|
vector<Point> points ;
|
|
|
|
for (unsigned int i = 0 ; i < waypoint_list.size() ; i++)
|
|
{
|
|
map<string, Area> areas(inWhichAreas(waypoint_list[i])) ;
|
|
if (areas.find(z.getName()) != areas.end())
|
|
points.push_back(waypoint_list[i]) ;
|
|
}
|
|
|
|
return points ;
|
|
}
|
|
|
|
void Positioning::makeReferencePointDistances()
|
|
{
|
|
|
|
unsigned int nb_pts = reference_point_list.size() ; // Nombre de points à traiter.
|
|
|
|
reference_point_matrix = new float*[nb_pts] ; // Allocation de la première dimension.
|
|
for (unsigned int i = 0 ; i < nb_pts ; i++) // Initialisation de la matrice :
|
|
{
|
|
reference_point_matrix[i] = new float[nb_pts] ;
|
|
for (unsigned int k = 0; k < nb_pts; k++) // Initialisation des distances à -1
|
|
reference_point_matrix[i][k] = -1 ;
|
|
}
|
|
|
|
for (unsigned int cur_idx = 0 ; cur_idx < nb_pts ; cur_idx++)
|
|
{
|
|
reference_point_matrix[cur_idx][cur_idx] = 0 ; // La distance du point à lui-même est nulle
|
|
|
|
for (unsigned int k = 0 ; k < cur_idx ; k++) // Pour chacun des point précédent
|
|
{
|
|
float dist = distanceTopology((Point) reference_point_list[cur_idx], (Point) reference_point_list[k]) ; // On calcule la distance
|
|
reference_point_matrix[cur_idx][k] = dist ; // et on l'insère dans le tableau,
|
|
reference_point_matrix[k][cur_idx] = dist ; // dans les deux sens.
|
|
}
|
|
}
|
|
}
|
|
|
|
bool Positioning::checkTopology()
|
|
{
|
|
|
|
for (unsigned int i = 0 ; i < waypoint_list.size() ; i++)
|
|
{
|
|
map<string, Area> areas(inWhichAreas(waypoint_list[i])) ;
|
|
if (areas.empty())
|
|
{
|
|
return false ;
|
|
}
|
|
}
|
|
|
|
for (unsigned int i = 0 ; i < reference_point_list.size() ; i++)
|
|
{
|
|
map<string, Area> areas(inWhichAreas((Point)reference_point_list[i])) ;
|
|
if (areas.empty())
|
|
{
|
|
return false ; // À modifier si on veut permettre aux points de référence de n'appartenir à aucune zone.
|
|
}
|
|
}
|
|
|
|
|
|
set<string> connected_areas ; // Liste des zones connectées à au moins une autre zone (on n'enregistre que le nom des zones, dupliquer toutes les zones étant inutile).
|
|
|
|
for (map<string, Area>::iterator it1 = area_list.begin() ; it1 != area_list.end() ; it1++)
|
|
for (map<string, Area>::iterator it2 = area_list.begin() ; it2 != area_list.end() ; it2++)
|
|
{
|
|
if (it1 == it2)
|
|
continue ;
|
|
|
|
vector<Point> connect(areaConnection(it1->second, it2->second)) ;
|
|
if (!connect.empty())
|
|
connected_areas.insert(it1->first) ; // Ajout de la zone à la liste des zones connectées.
|
|
}
|
|
|
|
if (connected_areas.size() != area_list.size())
|
|
{
|
|
return false ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
inline bool Positioning::pointExists(const float &x, const float &y, const float &z) const
|
|
{
|
|
return pointExists(reference_point_list, Point(x, y, z));
|
|
}
|
|
|
|
inline bool Positioning::pointExists(const Point &p) const
|
|
{
|
|
return pointExists(reference_point_list, p);
|
|
}
|
|
|
|
inline bool Positioning::pointExists(const vector<ReferencePoint> &point_list, const float &x, const float &y, const float &z) const
|
|
{
|
|
return pointExists(point_list, Point(x, y, z));
|
|
}
|
|
|
|
inline bool Positioning::pointExists(const vector<ReferencePoint> &point_list, const Point &p) const
|
|
{
|
|
return (pointIndex(point_list, p) != -1);
|
|
}
|
|
|
|
inline int Positioning::pointIndex(const Point &p) const
|
|
{
|
|
return (unsigned int) pointIndex(reference_point_list, p);
|
|
}
|
|
|
|
inline int Positioning::pointIndex(const vector<ReferencePoint> &point_list, const float &x, const float &y, const float &z) const
|
|
{
|
|
return (unsigned int) pointIndex(point_list, Point(x, y, z)) ;
|
|
}
|
|
|
|
inline int Positioning::pointIndex(const vector<ReferencePoint> &point_list, const Point &p) const
|
|
{
|
|
unsigned int i;
|
|
|
|
for (i = 0 ; i < point_list.size() ; i++)
|
|
if (p == point_list[i].getCoordinates())
|
|
{
|
|
return i;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
/* Retourne la position du Point "p" dans le tableau à une dimension "point_tab" de taille "size", ou -1 en cas d'échec. */
|
|
inline int Positioning::pointIndex(const Point *point_tab, unsigned int &size, const Point &p) const
|
|
{
|
|
for (unsigned int i = 0 ; i < size ; i++)
|
|
if (point_tab[i] == p)
|
|
return i;
|
|
|
|
return -1;
|
|
}
|
|
|
|
/* Retourne la position du Point "p" dans le vector "point_list", ou -1 en cas d'échec. */
|
|
inline int Positioning::pointIndex(const vector<Point> &point_list, const Point &p) const
|
|
{
|
|
|
|
for (unsigned int i = 0 ; i < point_list.size() ; i++)
|
|
if (point_list[i] == p)
|
|
{
|
|
return i;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
void Positioning::printReferencePointList()
|
|
{
|
|
printPointList(reference_point_list) ;
|
|
}
|
|
|
|
void Positioning::printPointList(vector<ReferencePoint> &point_list)
|
|
{
|
|
for (unsigned int i = 0 ; i < point_list.size() ; i++)
|
|
cout << point_list[i] << endl ;
|
|
}
|
|
|
|
void Positioning::printAccessPointList() const
|
|
{
|
|
for (unsigned int i = 0; i < access_point_list.size(); i++)
|
|
cout << access_point_list[i] << endl;
|
|
}
|
|
|
|
void Positioning::printAreatList()
|
|
{
|
|
for (map<string, Area>::iterator it = area_list.begin() ; it != area_list.end() ; it++)
|
|
cout << it -> second << endl;
|
|
}
|