Commit 06b284af authored by jben's avatar jben

maj route

parent 2b6d3a89
......@@ -2,7 +2,7 @@
# this lists the binaries to produce, the (non-PHONY, binary) targets in
# the previous manual Makefile
bin_PROGRAMS = rv_route
rv_route_SOURCES = database.cc main.cc model.cc node.cc node_parcouru.cc queue.h route.h database.h Makefile.am model.h node.h node_parcouru.h route.cc edge.h
rv_route_SOURCES = database.cc main.cc model.cc node.cc node_parcouru.cc queue.h route.h database.h Makefile.am model.h node.h node_parcouru.h route.cc instant.h
rv_route_CPPFLAGS = $(LIBPQXX_CFLAGS)
rv_route_LDADD = $(LIBPQXX_LIBS)
......@@ -24,38 +24,151 @@ node database::get_node(const unsigned long long & ull_id)
return(n_result);
}
unsigned long long database::find_node(const double & d_lon, const double & d_lat)
std::list<unsigned long long> database::find_nodes(std::list< std::pair<double,double> > & l_points)
{
unsigned long long ull_id_node=0;
double diam = 0.015;
// On trouve la cc
unsigned long long ull_begin, ull_end;
unsigned long long ull_begin_cc, ull_end_cc;
double dist_begin, dist_end;
unsigned long long ull_cc;
if(!find_one_node(*(l_points.begin()),false,0,ull_begin,ull_begin_cc,dist_begin))
abort();
if(!find_one_node(*(l_points.rbegin()),false,0,ull_end,ull_end_cc,dist_end))
abort();
do
if(ull_begin_cc==ull_end_cc)
{
pqxx::work txn(db);
char z_req[1024];
ull_cc=ull_begin_cc;
}
else
{
unsigned long long ull_begin_oth, ull_end_oth;
double dist_begin_oth, dist_end_oth;
unsigned long long ull_cc_poubelle;
bool b1, b2;
b1=find_one_node(*(l_points.begin()),true,ull_end_cc,ull_begin_oth,ull_cc_poubelle,dist_begin_oth);
b2=find_one_node(*(l_points.rbegin()),true,ull_begin_cc,ull_end_oth,ull_cc_poubelle,dist_end_oth);
if(b1)
{
if(b2)
{
if(dist_begin+dist_end_oth < dist_begin_oth+dist_end)
{
ull_cc = ull_begin_cc;
ull_end = ull_end_oth;
}
else
{
ull_cc = ull_end_cc;
ull_begin = ull_begin_oth;
}
}
else
{
ull_cc = ull_end_cc;
ull_begin = ull_begin_oth;
}
}
else
{
if(b2)
{
ull_cc = ull_begin_cc;
ull_end = ull_end_oth;
}
else
{
abort();
}
}
}
snprintf(z_req,1024,"SELECT id FROM rv_nodes WHERE geom && ST_Expand('SRID=4326;POINT(%.8f %.8f)'::geometry,%f) ORDER BY ST_Distance(geom,'POINT(%.8f %.8f)'::geography) LIMIT 1;",d_lon,d_lat,diam,d_lon,d_lat);
pqxx::result res = txn.exec(z_req);
std::list<unsigned long long> lull_id;
if(res.size()>0)
ull_id_node = res[0][0].as<unsigned long long>();
else
diam*=2;
} while(ull_id_node==0);
std::list< std::pair<double,double> >::iterator it=l_points.begin();
while(it!=l_points.end())
{
unsigned long long id;
std::pair<double,double> & P = *it;
bool ok=false;
if(it==l_points.begin())
{
id=ull_begin;
ok=true;
}
it++;
if(it==l_points.end())
{
id=ull_end;
ok=true;
}
if(!ok)
{
double dist_poubelle;
unsigned long long ull_cc_poubelle;
if(!find_one_node(P,true,ull_cc,id,ull_cc_poubelle,dist_poubelle))
abort();
}
fprintf(stderr,"%llu\n",ull_id_node);
return(ull_id_node);
lull_id.push_back(id);
}
return(lull_id);
}
bool database::find_one_node(std::pair<double,double> P, bool respect_cc, unsigned long long ull_cc, unsigned long long & ull_result_id, unsigned long long & ull_result_cc, double & dist)
{
double diam = 0.013;
double & d_lon = P.first;
double & d_lat = P.second;
pqxx::work txn(db);
char z_req[1024];
if(respect_cc)
snprintf(z_req,1024,"SELECT id,cc,ST_Distance(geom,'POINT(%.8f %.8f)'::geography) AS d FROM rv_nodes WHERE geom && ST_Expand('SRID=4326;POINT(%.8f %.8f)'::geometry,%f) AND cc=%llu ORDER BY d LIMIT 1;",d_lon,d_lat,d_lon,d_lat,diam,ull_cc);
else
snprintf(z_req,1024,"SELECT id,cc,ST_Distance(geom,'POINT(%.8f %.8f)'::geography) AS d FROM rv_nodes WHERE geom && ST_Expand('SRID=4326;POINT(%.8f %.8f)'::geometry,%f) ORDER BY d LIMIT 1;",d_lon,d_lat,d_lon,d_lat,diam);
pqxx::result res = txn.exec(z_req);
if(res.size()>0)
{
ull_result_id = res[0][0].as<unsigned long long>();
ull_result_cc = res[0][1].as<unsigned long long>();
dist = res[0][2].as<double>();
return true;
}
return false;
}
edge database::get_edge(const unsigned long long & ull_from, const unsigned long long & ull_to)
std::pair<unsigned long long, unsigned int> database::get_edge(const unsigned long long & ull_from, const unsigned long long & ull_to)
{
pqxx::work txn(db);
char z_req[1024];
snprintf(z_req,1024,"SELECT way_id FROM rv_edges WHERE from_id=%lld AND to_id=%lld;",ull_from,ull_to);
snprintf(z_req,1024,"SELECT way_id,version FROM rv_edges INNER JOIN ways ON ways.id=rv_edges.way_id WHERE from_id=%lld AND to_id=%lld;",ull_from,ull_to);
pqxx::result r=txn.exec(z_req);
if(r.size()==0)
......@@ -66,9 +179,9 @@ edge database::get_edge(const unsigned long long & ull_from, const unsigned long
unsigned long long ull_id_way = r[0][0].as<unsigned long long>();
unsigned int ui_rev = r[0][1].as<unsigned int>();
edge E_result(ull_from,ull_to,ull_id_way);
return(E_result);
return(std::pair<unsigned long long, unsigned int>(ull_id_way,ui_rev));
}
std::list<unsigned long long> database::get_adjoining(const unsigned long long & ull_id)
......
......@@ -10,7 +10,6 @@
#include <list>
#include <math.h>
#include "node.h"
#include "edge.h"
#define PREC(N) std::setiosflags(std::ios::fixed) << std::setprecision(N) <<
......@@ -24,15 +23,20 @@ class database
node get_node(const unsigned long long & ull_id);
edge get_edge(const unsigned long long & ull_from, const unsigned long long & ull_to);
std::pair<unsigned long long, unsigned int> get_edge(const unsigned long long & ull_from, const unsigned long long & ull_to);
std::list<unsigned long long> get_adjoining(const unsigned long long & ull_id);
unsigned long long find_node(const double & d_lon, const double & d_lat);
std::list<unsigned long long> find_nodes(const std::list< std::pair<double,double> > & l_points);
double get_height_s(const int & lon_s, const int & lat_s);
double get_height(const double & lon, const double & lat);
bool find_one_node(std::pair<double,double> P, bool respect_cc, unsigned long long ull_cc, unsigned long long & ull_result_id, unsigned long long & ull_result_cc, double & dist);
std::list<unsigned long long> find_nodes(std::list< std::pair<double,double> > & l_points);
};
#endif
#ifndef H_EDGE
#define H_EDGE 1
class edge
{
public:
unsigned long long ull_from;
unsigned long long ull_to;
unsigned long long ull_id_way;
node n_from;
node n_to;
double d_dist;
double d_pente;
edge(const unsigned long long & ull_from_i, const unsigned long long & ull_to_i, const unsigned long long & ull_id_way_i) :
ull_from(ull_from_i),
ull_to(ull_to_i),
ull_id_way(ull_id_way_i),
d_dist(0),
d_pente(0),
n_from(0,0,0,0),
n_to(0,0,0,0)
{}
edge(const edge & E_orig) :
ull_from(E_orig.ull_from),
ull_to(E_orig.ull_to),
ull_id_way(E_orig.ull_id_way),
d_dist(E_orig.d_dist),
d_pente(E_orig.d_pente),
n_from(E_orig.n_from),
n_to(E_orig.n_to)
{}
};
#endif
#ifndef H_INSTANT
#define H_INSTANT 1
class instant
{
public:
unsigned long long ull_id;
unsigned long long ull_way_from_id;
unsigned int ull_way_from_rev;
double d_lon;
double d_lat;
double d_height;
double d_dist;
double d_V;
double d_ET;
double d_deniv_pos;
instant():
ull_id(0),
ull_way_from_id(0),
ull_way_from_rev(0),
d_height(0),
d_dist(0),
d_V(0),
d_ET(0),
d_deniv_pos(0)
{}
};
#endif
#include "route.h"
#include "model.h"
#include "database.h"
#include <getopt.h>
int main(int argc, char* argv[])
{
if(argc<14)
int c;
std::string s_db;
double d_VplatKMH=20;
double d_masse=80;
double d_SCx=.45;
double d_Cr=.01;
double d_rho_air=1.204;
double d_vent_from=0;
double d_vent_VKMH=0;
char mode='e';
while (1)
{
static struct option long_options[] = {
{"database",required_argument, 0, 'd' },
{"speedref",required_argument, 0, 's' },
{"mass", required_argument, 0, 'm' },
{"cr", required_argument, 0, 'C' },
{"scx", required_argument, 0, 'S' },
{"rho", required_argument, 0, 'r' },
{"winddir", required_argument, 0, 'w' },
{"windspeed",required_argument,0, 'W' },
{"criterion",required_argument,0, 'c' },
{0, 0, 0, 0 }
};
int option_index = 0;
c = getopt_long(argc, argv, "d:s:m:C:S:r:w:W:c:",
long_options, &option_index);
if (c == -1)
break;
switch (c)
{
case 0:
if (long_options[option_index].flag != 0)
break;
case 'd':
s_db = optarg;
break;
case 's':
d_VplatKMH = atof(optarg);
if(d_VplatKMH<=0)
abort();
break;
case 'm':
d_masse = atof(optarg);
if(d_masse<=0)
abort();
break;
case 'C':
d_Cr = atof(optarg);
if(d_Cr<0)
abort();
break;
case 'S':
d_SCx = atof(optarg);
if(d_SCx<0)
abort();
break;
case 'r':
d_rho_air = atof(optarg);
if(d_rho_air<0)
abort();
break;
case 'w':
d_vent_from = atof(optarg);
break;
case 'W':
d_vent_VKMH = atof(optarg);
break;
case 'c':
if(std::string(optarg)==std::string("energy"))
mode='e';
else if(std::string(optarg)==std::string("distance"))
mode='d';
else
abort();
break;
}
}
std::list< std::pair<double,double> > l_points;
bool lonoulat=false;
double lon,lat;
if (optind < argc)
{
fprintf(stderr,"Usage:\n");
fprintf(stderr,"%s \"database connection string\" VplatKMH masse Cr SCx rho_air vent_from vent_VKMH mode lon1 lat1 lon2 lat2\n",argv[0]);
while (optind < argc)
{
if(lonoulat)
{
lat=atof(argv[optind]);
l_points.push_back(std::pair<double,double>(lon,lat));
}
else
{
lon=atof(argv[optind]);
}
lonoulat=!lonoulat;
optind++;
}
}
if(l_points.size()<2)
{
fprintf(stderr,"Must have at least 2 points\n");
abort();
}
std::string s_db(argv[1]);
double d_lon1,d_lon2,d_lat1,d_lat2;
double d_VplatKMH;
double d_masse;
double d_SCx;
double d_Cr;
double d_rho_air;
double d_vent_from, d_vent_VKMH;
char c;
d_VplatKMH=atof(argv[2]); // on peut mettre 20 km/h by default
d_masse=atof(argv[3]); // attention cycliste + velo + bagages
d_Cr=atof(argv[4]); // coef de roulement, 0.01 voir moins avec des pneus fins et une route lisse
d_SCx=atof(argv[5]); // 0.45 : cyclotoriste avec une position confortable
d_rho_air=atof(argv[6]); // 1.204 a 20 degré celsius
d_vent_from=atof(argv[7]); // en degré p/r au nord (exemple 0=nord, 90=est, 180=sud, 270=ouest, 360=nord)
d_vent_VKMH=atof(argv[8]); // vitesse du vent en km/h
sscanf(argv[9],"%c",&c); // mode (e=energy, p=pente, d=dist)
d_lon1=atof(argv[10]);
d_lat1=atof(argv[11]);
d_lon2=atof(argv[12]);
d_lat2=atof(argv[13]);
database D_db(s_db);
std::list<unsigned long long> l_id=D_db.find_nodes(l_points);
model m_jben(c,d_VplatKMH,d_masse,d_Cr,d_SCx,d_rho_air,d_vent_from,d_vent_VKMH);
//for(std::list<unsigned long long>::iterator it=l_id.begin();it!=l_id.end();it++)
//{
// printf("%llu\n",*it);
//}
route r_route(s_db,m_jben,d_lon1,d_lat1,d_lon2,d_lat2);
r_route.go();
model m_model(mode,d_VplatKMH,d_masse,d_Cr,d_SCx,d_rho_air,d_vent_from,d_vent_VKMH);
std::list<edge> & lE_result = r_route.get_result();
unsigned long long previous=0;
etat_cycliste e_courant;
int troncon=0;
/*
for(std::list<edge>::iterator itE = lE_result.begin(); itE != lE_result.end(); itE++)
{
fprintf(stdout,"%10llu\t%10llu\t%10llu\t%f\t%f\t%f\t%f\t%f\t%f\n",
itE->ull_from,
itE->ull_to,
itE->ull_id_way,
itE->n_from.d_lat,
itE->n_from.d_lon,
itE->n_to.d_lat,
itE->n_to.d_lon,
itE->d_dist,
itE->d_pente
);
}
*/
fprintf(stdout,"<?xml version=\"1.0\"?>\n");
double pdist=r_route.get_dist_total();
double pdeniv=r_route.get_deniv();
double penergie=r_route.get_energie();
double ppui=m_jben.d_P;
double ptps=penergie/ppui;
double pvmoy=pdist/ptps;
fprintf(stdout,"\n");
fprintf(stdout,"<!-- Distance : %f m, %f km -->\n",pdist,pdist/1000);
fprintf(stdout,"<!-- Denivele positif : %f m -->\n",pdeniv);
fprintf(stdout,"<!-- Energie : %f J -->\n",penergie);
fprintf(stdout,"<!-- Puissance constante : %f W -->\n",ppui);
fprintf(stdout,"<!-- Temps total : %f s, %f min -->\n",ptps,ptps/60);
fprintf(stdout,"<!-- Vitesse moyenne : %f m/s, %f km/h -->\n",pvmoy,pvmoy*3.6);
fprintf(stdout,"\n");
fprintf(stdout,"<gpx version=\"1.0\" xmlns=\"http://www.topografix.com/GPX/1/0\">\n");
fprintf(stdout," <trk>\n");
fprintf(stdout," <trkseg>\n");
fprintf(stdout," <trkpt lat=\"%f\" lon=\"%f\"/>\n",lE_result.begin()->n_from.d_lat,lE_result.begin()->n_from.d_lon);
for(std::list<edge>::iterator itE = lE_result.begin(); itE != lE_result.end(); itE++)
printf("#troncon\tnode_id\tlon\tlat\theight\tway_from\tway_from_rev\tvitesse\tdistance\tdeniv\tenergie\ttps\n");
for(std::list<unsigned long long>::iterator it_id=l_id.begin();it_id!=l_id.end();it_id++)
{
fprintf(stdout," <trkpt lat=\"%f\" lon=\"%f\"/>\n",itE->n_to.d_lat,itE->n_to.d_lon);
}
if(it_id==l_id.begin())
{
previous=*it_id;
continue;
}
route r_route(s_db,m_model,previous,*it_id);
fprintf(stdout," </trkseg>\n");
fprintf(stdout," </trk>\n");
fprintf(stdout,"</gpx>\n");
r_route.set_etat_init(e_courant);
r_route.go();
e_courant=r_route.get_etat_final();
std::list<instant> & lI_result = r_route.get_result();
for(std::list<instant>::iterator it=lI_result.begin();it!=lI_result.end();it++)
{
printf("%lu\t",troncon);
printf("%llu\t",it->ull_id);
printf("%f\t%f\t%f\t",it->d_lon,it->d_lat,it->d_height);
printf("%llu\t%lu\t",it->ull_way_from_id,it->ull_way_from_rev);
printf("%f\t%f\t%f\t%f\t%f",it->d_V,it->d_dist,it->d_deniv_pos,it->d_ET,it->d_ET/m_model.d_P);
printf("\n");
}
previous=*it_id;
troncon++;
}
return(0);
}
......@@ -20,6 +20,13 @@ class etat_cycliste
etat_cycliste() : d_ET(0),d_V(0),d_dist(0),d_pente(0) {}
etat_cycliste(const double & d_ET_i, const double & d_V_i, const double & d_dist_i, const double & d_pente_i) : d_ET(d_ET_i), d_V(d_V_i), d_dist(d_dist_i), d_pente(d_pente_i) {}
etat_cycliste(const etat_cycliste & orig) :
d_ET(orig.d_ET),
d_V(orig.d_V),
d_dist(orig.d_dist),
d_pente(orig.d_pente)
{}
};
class model
......@@ -66,7 +73,6 @@ class model
etat_cycliste etat_depart() {return etat_cycliste(0,0,0,0); }
};
......
......@@ -6,6 +6,7 @@ void node_parcouru::set(unsigned long long ull_id, database & D_db, const node &
lull_adj = D_db.get_adjoining(ull_id);
d_cout_minimal_arrive = m_model.cout_minimal(n_node,n_arrivee);
d_dist_min_arrive = m_model.dist_node(n_node,n_arrivee);
b_frozen = false;
......
......@@ -20,6 +20,7 @@ class node_parcouru
double d_cout_minimal;
double d_cout_minimal_arrive;
double d_dist_min_arrive;
node_parcouru() : b_defined(false),b_frozen(false),n_node(0,0,0,0) {}
......
......@@ -15,10 +15,13 @@ void route::go()
P_depart.set(ull_depart,D_db,n_arrivee,m_model);
P_depart.e_etat_cycliste_optimal = m_model.etat_depart();
P_depart.e_etat_cycliste_optimal = e_init;
P_depart.d_cout_minimal = m_model.cout_etat(P_depart.e_etat_cycliste_optimal);
P_depart.ull_from=0;
double d_dist_min_totale = P_depart.d_dist_min_arrive;
double d_status;
S_queue.push(node_in_queue(ull_depart,P_depart.d_cout_minimal + P_depart.d_cout_minimal_arrive));
// Dans la queue on a que le noeud de départ et dans la map aussi.
......@@ -34,6 +37,13 @@ void route::go()
P_en_cours.b_frozen=true;
// fprintf(stderr,"%10llu ->%10llu\t%f\t%f\n",P_en_cours.ull_from,ull_en_cours,P_en_cours.d_cout_minimal,P_en_cours.d_cout_minimal_arrive);
double d_nstatus=1-P_en_cours.d_dist_min_arrive/d_dist_min_totale;
if(d_nstatus>d_status)
{
d_status=d_nstatus;
printf("### status %.3f\n",d_nstatus);
}
// Si on vient de geler le noeud de fin, on s'arrete la.
if(ull_en_cours == ull_arrivee)
{
......@@ -106,20 +116,31 @@ void route::go()
unsigned long long ull_previous_node;
for(std::list<unsigned long long>::iterator it=lull_chemin.begin();it!=lull_chemin.end();it++)
{
instant I;
node_parcouru & P_node = M_map[*it];
node & n_node = P_node.n_node;
etat_cycliste & e_node = P_node.e_etat_cycliste_optimal;
I.ull_id=*it;
I.d_lon=n_node.d_lon;
I.d_lat=n_node.d_lat;
I.d_height=n_node.d_height;
I.d_dist=e_node.d_dist;
I.d_V=e_node.d_V;
I.d_ET=e_node.d_ET;
I.d_deniv_pos=e_node.d_pente;
if(it!=lull_chemin.begin())
{
edge E_edge = D_db.get_edge(ull_previous_node,*it);
node & n_node1 = M_map[ull_previous_node].n_node;
node & n_node2 = M_map[*it].n_node;
E_edge.n_from = n_node1;
E_edge.n_to = n_node2;
E_edge.d_dist = m_model.dist_node(n_node1,n_node2);
E_edge.d_pente = (n_node2.d_height - n_node1.d_height)/E_edge.d_dist;
lE_result.push_back(E_edge);
std::pair<unsigned long long,unsigned int> pair = D_db.get_edge(ull_previous_node,*it);
I.ull_way_from_id=pair.first;
I.ull_way_from_rev=pair.second;
}
ull_previous_node=*it;
lI_result.push_back(I);
}
......@@ -129,6 +150,7 @@ void route::go()
d_dist_total = m_model.cout_etat_dist(P_last_node.e_etat_cycliste_optimal);
d_deniv = m_model.cout_etat_pente(P_last_node.e_etat_cycliste_optimal);
d_energie = m_model.cout_etat_energie(P_last_node.e_etat_cycliste_optimal);
e_final = P_last_node.e_etat_cycliste_optimal;
}
......
......@@ -7,7 +7,7 @@
#include "queue.h"
#include "database.h"
#include<map>
#include "edge.h"
#include "instant.h"
class route
{
......@@ -18,27 +18,31 @@ class route
database D_db;
std::list<edge> lE_result;
std::list<instant> lI_result;
double d_dist_total;
double d_deniv;
double d_energie;
etat_cycliste e_init;
etat_cycliste e_final;
public:
route(const std::string & s_db, const model & m_model_i, double & d_lon1, double & d_lat1, double & d_lon2, double & d_lat2) :
route(const std::string & s_db, const model & m_model_i, const unsigned long long & ull_id_1, const unsigned long long & ull_id_2) :
m_model(m_model_i),
D_db(s_db) ,
d_dist_total(0),
d_deniv(0),
d_energie(0)
{
ull_depart = D_db.find_node(d_lon1,d_lat1);
ull_arrivee = D_db.find_node(d_lon2,d_lat2);
}
d_energie(0),
ull_depart(ull_id_1),
ull_arrivee(ull_id_2)
{}
std::list<edge> & get_result() { return lE_result;}
std::list<instant> & get_result() { return lI_result;}
double get_dist_total() { return d_dist_total; }
double get_deniv() { return d_deniv; }
double get_energie() { return d_energie; }
etat_cycliste get_etat_final() { return e_final; }
void set_etat_init(const etat_cycliste & e_init_i) {e_init=e_init_i;}
void go();
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment