2009-05-29 14:07:05 +02:00
# 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.
2009-05-29 17:02:20 +02:00
int direction ;
2009-05-29 14:07:05 +02:00
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 ) ;
2009-05-29 17:02:20 +02:00
direction = from_string < int > ( infos [ 3 ] ) ;
2009-05-29 14:07:05 +02:00
2009-05-29 17:02:20 +02:00
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,
2009-05-29 14:07:05 +02:00
{
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 ( ) ;
}
}
2009-05-29 17:02:20 +02:00
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 )
{
2009-05-29 14:07:05 +02:00
2009-05-29 17:02:20 +02:00
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 )
{
2009-05-29 14:07:05 +02:00
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)
2009-05-29 17:02:20 +02:00
2009-05-29 14:07:05 +02:00
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 ;
}
}
}
}
2009-05-29 17:02:20 +02:00
2009-05-29 14:07:05 +02:00
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 ) ) ;
2009-05-29 17:02:20 +02:00
return areas ;
2009-05-29 14:07:05 +02:00
}
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
2010-05-21 10:35:15 +02:00
{
return pointExists ( reference_point_list , Point ( x , y , z ) ) ;
}
2009-05-29 14:07:05 +02:00
inline bool Positioning : : pointExists ( const Point & p ) const
2010-05-21 10:35:15 +02:00
{
return pointExists ( reference_point_list , p ) ;
}
2009-05-29 14:07:05 +02:00
inline bool Positioning : : pointExists ( const vector < ReferencePoint > & point_list , const float & x , const float & y , const float & z ) const
2010-05-21 10:35:15 +02:00
{
return pointExists ( point_list , Point ( x , y , z ) ) ;
}
2009-05-29 14:07:05 +02:00
inline bool Positioning : : pointExists ( const vector < ReferencePoint > & point_list , const Point & p ) const
2010-05-21 10:35:15 +02:00
{
return ( pointIndex ( point_list , p ) ! = - 1 ) ;
}
2009-05-29 14:07:05 +02:00
inline int Positioning : : pointIndex ( const Point & p ) const
2010-05-21 10:35:15 +02:00
{
return ( unsigned int ) pointIndex ( reference_point_list , p ) ;
}
2009-05-29 14:07:05 +02:00
inline int Positioning : : pointIndex ( const vector < ReferencePoint > & point_list , const float & x , const float & y , const float & z ) const
2010-05-21 10:35:15 +02:00
{
return ( unsigned int ) pointIndex ( point_list , Point ( x , y , z ) ) ;
}
2009-05-29 14:07:05 +02:00
inline int Positioning : : pointIndex ( const vector < ReferencePoint > & point_list , const Point & p ) const
2010-05-21 10:35:15 +02:00
{
unsigned int i ;
2009-05-29 14:07:05 +02:00
2010-05-21 10:35:15 +02:00
for ( i = 0 ; i < point_list . size ( ) ; i + + )
if ( p = = point_list [ i ] . getCoordinates ( ) )
{
return i ;
}
2009-05-29 14:07:05 +02:00
2010-05-21 10:35:15 +02:00
return - 1 ;
}
2009-05-29 14:07:05 +02:00
/* 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
2010-05-21 10:35:15 +02:00
{
for ( unsigned int i = 0 ; i < size ; i + + )
if ( point_tab [ i ] = = p )
return i ;
2009-05-29 14:07:05 +02:00
2010-05-21 10:35:15 +02:00
return - 1 ;
}
2009-05-29 14:07:05 +02:00
/* 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
2010-05-21 10:35:15 +02:00
{
2009-05-29 14:07:05 +02:00
2010-05-21 10:35:15 +02:00
for ( unsigned int i = 0 ; i < point_list . size ( ) ; i + + )
if ( point_list [ i ] = = p )
{
return i ;
}
2009-05-29 14:07:05 +02:00
2010-05-21 10:35:15 +02:00
return - 1 ;
}
2009-05-29 14:07:05 +02:00
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
2010-05-21 10:35:15 +02:00
{
for ( unsigned int i = 0 ; i < access_point_list . size ( ) ; i + + )
cout < < access_point_list [ i ] < < endl ;
}
2009-05-29 17:02:20 +02:00
void Positioning : : printAreatList ( )
{
for ( map < string , Area > : : iterator it = area_list . begin ( ) ; it ! = area_list . end ( ) ; it + + )
cout < < it - > second < < endl ;
}