Commit 26d70e0f authored by Jean-Benoist Leger's avatar Jean-Benoist Leger

update

parent 5c59a8bd
...@@ -46,9 +46,9 @@ void rv_do_with_velocity( ...@@ -46,9 +46,9 @@ void rv_do_with_velocity(
energy = delta_kinetic_energy + delta_gravitational_potential_energy + work_of_rolling_resistance + work_of_air_resistance; energy = delta_kinetic_energy + delta_gravitational_potential_energy + work_of_rolling_resistance + work_of_air_resistance;
time = 2*distance_AB/(velocityA+velocityB); time = 2*distance_AB/(velocityA+velocityB);
power = energy / time;
if(energy<0) // In fact we brake if(energy<0) // In fact we brake
energy=0; energy=0;
power = energy / time;
} }
void rv_do_with_null_power( void rv_do_with_null_power(
...@@ -86,7 +86,8 @@ void rv_do_walk( ...@@ -86,7 +86,8 @@ void rv_do_walk(
double work_of_walking_resistance = parameters.mass * RV_G * parameters.Cw * distance_AB; double work_of_walking_resistance = parameters.mass * RV_G * parameters.Cw * distance_AB;
energy = delta_gravitational_potential_energy + work_of_walking_resistance; energy = delta_gravitational_potential_energy + work_of_walking_resistance;
time = energy / parameters.power_walk; time = energy / parameters.power_walk;
velocityB = distance_AB / time; velocityB = 0;
power = parameters.power_walk;
} }
void order3_poly(double & a, double & b, double & c, double & d, double & root3) void order3_poly(double & a, double & b, double & c, double & d, double & root3)
...@@ -151,12 +152,30 @@ double rv_constant_power_biking( ...@@ -151,12 +152,30 @@ double rv_constant_power_biking(
return(velocityB); return(velocityB);
} }
double rv_constant_speed_with_constant_power(
parameters_t & parameters,
double & distance,
double & delta_elevation)
{
double velocity;
double a = -(RV_RHO_AIR*parameters.SCx)/2; // kg.m^(-1)
double b = 0;
double c = -(delta_elevation*RV_G*parameters.mass)/distance-parameters.Cr*RV_G*parameters.mass; // kg.m.s^(-2)
double d = parameters.power; // kg.m^2.s^(-3)
order3_poly(a,b,c,d,velocity);
return(velocity);
}
double rv_heuristic( double rv_heuristic(
parameters_t & parameters, parameters_t & parameters,
node_info_fixed_t *nodeA, node_info_fixed_t *nodeA,
node_info_fixed_t *nodeF, node_info_fixed_t *nodeF,
status_t & statusA) status_t & statusA)
{ {
if(parameters.criterion == RV_CRITERION_ENERGY_D || parameters.criterion == RV_CRITERION_TIME_D)
return(statusA.criterion);
double distance_AF = rv_distance( double distance_AF = rv_distance(
nodeA->lon, nodeA->lon,
nodeA->lat, nodeA->lat,
...@@ -187,6 +206,20 @@ double rv_heuristic( ...@@ -187,6 +206,20 @@ double rv_heuristic(
return(statusA.criterion + distance_AF); return(statusA.criterion + distance_AF);
} }
if(parameters.criterion == RV_CRITERION_ENERGY_APPROX || parameters.criterion == RV_CRITERION_TIME_APPROX)
{
double delta_elevation = nodeF->elevation - nodeA->elevation;
double velocity = rv_constant_speed_with_constant_power(parameters, distance_AF, delta_elevation);
double min_energy = parameters.mass * RV_G * (parameters.Cr * distance_AF + delta_elevation) + .5*RV_RHO_AIR*parameters.SCx*velocity*velocity*distance_AF;
if(min_energy<0)
min_energy = 0;
if(parameters.criterion == RV_CRITERION_ENERGY_APPROX)
return(statusA.criterion + min_energy);
else
return(statusA.criterion + min_energy/parameters.power);
}
// What the hell are we doing here // What the hell are we doing here
return(0); return(0);
} }
...@@ -202,7 +235,7 @@ void rv_step( ...@@ -202,7 +235,7 @@ void rv_step(
status_t & statusB) status_t & statusB)
{ {
double distance_AB, distance_BC, distance_AC; double distance_AB, distance_BC, distance_AC;
double curvature_maximum_velocity; double curvature_maximum_velocity=0;
distance_AB = rv_distance( distance_AB = rv_distance(
nodeA->lon, nodeA->lon,
...@@ -228,7 +261,7 @@ void rv_step( ...@@ -228,7 +261,7 @@ void rv_step(
nodeC->lat, nodeC->lat,
nodeC->elevation); nodeC->elevation);
double angle = std::acos((distance_AB*distance_AB + distance_BC*distance_BC - distance_AC*distance_AC)/(2*distance_AB*distance_AC)); double angle = std::acos((distance_AB*distance_AB + distance_BC*distance_BC - distance_AC*distance_AC)/(2*distance_AB*distance_BC));
double way_width = (way_kindAB == RV_WK_CYCLEWAY) ? RV_WIDTH_CYCLEWAY : RV_WIDTH_OTHERS; double way_width = (way_kindAB == RV_WK_CYCLEWAY) ? RV_WIDTH_CYCLEWAY : RV_WIDTH_OTHERS;
if(distance_AB<2*way_width) if(distance_AB<2*way_width)
way_width = distance_AB/2; way_width = distance_AB/2;
...@@ -241,6 +274,7 @@ void rv_step( ...@@ -241,6 +274,7 @@ void rv_step(
radius_of_curvature = way_width / (1.0 - std::sin(.5*angle)); radius_of_curvature = way_width / (1.0 - std::sin(.5*angle));
curvature_maximum_velocity = std::sqrt(parameters.lateral_acceleration * RV_G * radius_of_curvature); curvature_maximum_velocity = std::sqrt(parameters.lateral_acceleration * RV_G * radius_of_curvature);
} }
double & velocityA = statusA.velocity; double & velocityA = statusA.velocity;
...@@ -263,7 +297,7 @@ void rv_step( ...@@ -263,7 +297,7 @@ void rv_step(
velocityB = parameters.velocity_nopower; velocityB = parameters.velocity_nopower;
rv_do_with_velocity(parameters, velocityA, velocityB, delta_elevation, distance_AB, energy, power, time); rv_do_with_velocity(parameters, velocityA, velocityB, delta_elevation, distance_AB, energy, power, time);
if(power < 0) // In fact we don't need power to have this velocity if(power <= 0) // In fact we don't need power to have this velocity
{ {
rv_do_with_null_power(parameters, velocityA, velocityB, delta_elevation, distance_AB, energy, power, time); rv_do_with_null_power(parameters, velocityA, velocityB, delta_elevation, distance_AB, energy, power, time);
...@@ -292,10 +326,11 @@ void rv_step( ...@@ -292,10 +326,11 @@ void rv_step(
statusB.energy = statusA.energy + energy; statusB.energy = statusA.energy + energy;
statusB.time = statusA.time + time; statusB.time = statusA.time + time;
statusB.distance = statusA.distance + distance_AB; statusB.distance = statusA.distance + distance_AB;
statusB.power = power;
if(parameters.criterion == RV_CRITERION_ENERGY) if(parameters.criterion == RV_CRITERION_ENERGY || parameters.criterion == RV_CRITERION_ENERGY_APPROX || parameters.criterion == RV_CRITERION_ENERGY_D)
statusB.criterion = statusA.criterion + energy * penalty; statusB.criterion = statusA.criterion + energy * penalty;
else if(parameters.criterion == RV_CRITERION_TIME) else if(parameters.criterion == RV_CRITERION_TIME || parameters.criterion == RV_CRITERION_TIME_APPROX || parameters.criterion == RV_CRITERION_TIME_D)
statusB.criterion = statusA.criterion + time * penalty; statusB.criterion = statusA.criterion + time * penalty;
else if(parameters.criterion == RV_CRITERION_DISTANCE) else if(parameters.criterion == RV_CRITERION_DISTANCE)
statusB.criterion = statusA.criterion + distance_AB * penalty; statusB.criterion = statusA.criterion + distance_AB * penalty;
......
...@@ -57,6 +57,7 @@ std::list<route_point_t> get_route_between_nids( ...@@ -57,6 +57,7 @@ std::list<route_point_t> get_route_between_nids(
status.velocity=0; status.velocity=0;
status.energy=0; status.energy=0;
status.time=0; status.time=0;
status.power=0;
status.walk=false; status.walk=false;
status.from_edge=null_edge; status.from_edge=null_edge;
status.freezed=false; status.freezed=false;
...@@ -201,8 +202,8 @@ std::list<route_point_t> get_route_between_nids( ...@@ -201,8 +202,8 @@ std::list<route_point_t> get_route_between_nids(
routing_point.energy = status.energy; routing_point.energy = status.energy;
routing_point.time = status.time; routing_point.time = status.time;
routing_point.walk = status.walk; routing_point.walk = status.walk;
routing_point.power = status.power;
ret.push_front(routing_point); ret.push_front(routing_point);
printf("%8lu\t%8lu\t%8f\t%8f\n",edge.A,edge.B,status.distance,status.energy);
if(edge.A == nid_initial) if(edge.A == nid_initial)
break; break;
......
#include "get_route.h" #include "get_route.h"
int main() int main(int argc, char** argv)
{ {
// if(argc<4)
// abort();
route_db_t route_db("/home/rv/tmpfs/route_db"); route_db_t route_db("/home/rv/tmpfs/route_db");
parameters_t parameters; parameters_t parameters;
parameters.mass = 90; parameters.mass = 90;
parameters.power = 47.6; parameters.power = 140;
parameters.SCx = .45; parameters.SCx = .45;
parameters.Cr = .008; parameters.Cr = .008;
parameters.velocity_nopower = 11.1; parameters.velocity_nopower = 9.7;
parameters.velocity_brake = 16.7; parameters.velocity_brake = 13.9;
parameters.velocity_equilibrium = 1.4; parameters.velocity_equilibrium = 1.4;
parameters.criterion = RV_CRITERION_ENERGY; parameters.criterion = RV_CRITERION_ENERGY_APPROX;
parameters.power_walk = 36.7; parameters.power_walk = 140;
parameters.lateral_acceleration = .5; parameters.lateral_acceleration = .5;
parameters.walk_penalty = 1; parameters.walk_penalty = 1;
parameters.Cw = .03; parameters.Cw = .03;
//std::pair<double,double> ptA(-78.39191437,40.51849719); //std::pair<double,double> ptA(-78.39191437,40.51849719);
//std::pair<double,double> ptB(-77.86422729,40.79197912); //std::pair<double,double> ptB(-77.86422729,40.79197912);
std::pair<double,double> ptA(-76.79992676,40.88444794); // std::pair<double,double> ptA(-76.79992676,40.88444794);
std::pair<double,double> ptB(-79.96948242,40.43858587); // std::pair<double,double> ptB(-79.96948242,40.43858587);
std::pair<double,double> ptA(atof(argv[1]),atof(argv[2]));
std::pair<double,double> ptB(atof(argv[3]),atof(argv[4]));
std::list<std::pair<double,double> > geos; std::list<std::pair<double,double> > geos;
geos.push_back(ptA); geos.push_back(ptA);
...@@ -34,9 +38,9 @@ int main() ...@@ -34,9 +38,9 @@ int main()
res_it!=res.end(); res_it!=res.end();
res_it++) res_it++)
{ {
printf("# track %u\n", track); // printf("# track %u\n", track);
printf("#nid \tlongitud\tlatitude\televatio\tvelocity\tdistance\tenergy \ttime \twalk\n"); // printf("#nid \tlongitud\tlatitude\televatio\tvelocity\tdistance\tenergy \ttime \twalk\n");
std::list<route_point_t> & l = *res_it; std::list<route_point_t> & l = *res_it;
...@@ -46,7 +50,7 @@ int main() ...@@ -46,7 +50,7 @@ int main()
{ {
nid_t nid = l_it->nid; nid_t nid = l_it->nid;
node_info_t* pni = route_db.get_node(nid); node_info_t* pni = route_db.get_node(nid);
printf("%8lu\t%8f\t%8f\t%8f\t%8f\t%8f\t%8f\t%8f\t%s\n", printf("%8lu\t%8f\t%8f\t%8f\t%8f\t%8f\t%8f\t%8f\t%8f\t%s\n",
nid, nid,
pni->fixed->lon, pni->fixed->lon,
pni->fixed->lat, pni->fixed->lat,
...@@ -54,8 +58,9 @@ int main() ...@@ -54,8 +58,9 @@ int main()
l_it->velocity, l_it->velocity,
l_it->distance, l_it->distance,
l_it->energy, l_it->energy,
l_it->power,
l_it->time, l_it->time,
(l_it->walk)?"yes":"no"); (l_it->walk)?"1":"0");
} }
track++; track++;
} }
......
...@@ -9,12 +9,15 @@ route_db_t::route_db_t(const char* db_location) ...@@ -9,12 +9,15 @@ route_db_t::route_db_t(const char* db_location)
snprintf(zfilename,RV_ZCHAR_LENGTH,"%s/%s",db_location,RV_LOOKUP_DB_RELATIVE_PATH); snprintf(zfilename,RV_ZCHAR_LENGTH,"%s/%s",db_location,RV_LOOKUP_DB_RELATIVE_PATH);
lookup_db = new lookup_db_t(zfilename); lookup_db = new lookup_db_t(zfilename);
count=0;
} }
route_db_t::~route_db_t() route_db_t::~route_db_t()
{ {
delete nodes_db; delete nodes_db;
delete lookup_db; delete lookup_db;
fprintf(stderr,"number of nodes : %u\n",count);
} }
int route_db_t::lookup( int route_db_t::lookup(
...@@ -32,6 +35,7 @@ node_info_t* route_db_t::get_node( ...@@ -32,6 +35,7 @@ node_info_t* route_db_t::get_node(
if( ! pni->fixed) if( ! pni->fixed)
{ {
nodes_db->get_node(nid, pni); nodes_db->get_node(nid, pni);
count++;
} }
return(pni); return(pni);
......
...@@ -24,7 +24,7 @@ class route_db_t ...@@ -24,7 +24,7 @@ class route_db_t
node_info_t* get_node(nid_t & nid); node_info_t* get_node(nid_t & nid);
unsigned int count;
}; };
......
...@@ -58,7 +58,11 @@ enum criterion_t ...@@ -58,7 +58,11 @@ enum criterion_t
{ {
RV_CRITERION_ENERGY, RV_CRITERION_ENERGY,
RV_CRITERION_TIME, RV_CRITERION_TIME,
RV_CRITERION_DISTANCE RV_CRITERION_DISTANCE,
RV_CRITERION_ENERGY_APPROX,
RV_CRITERION_TIME_APPROX,
RV_CRITERION_ENERGY_D,
RV_CRITERION_TIME_D
}; };
typedef enum criterion_t criterion_t; typedef enum criterion_t criterion_t;
...@@ -109,6 +113,7 @@ struct status_t ...@@ -109,6 +113,7 @@ struct status_t
edge_t from_edge; edge_t from_edge;
bool freezed; bool freezed;
bool initialized; bool initialized;
double power;
status_t() : freezed(false), initialized(false) {} status_t() : freezed(false), initialized(false) {}
}; };
...@@ -122,6 +127,7 @@ struct route_point_t ...@@ -122,6 +127,7 @@ struct route_point_t
double distance; double distance;
double energy; double energy;
double time; double time;
double power;
bool walk; bool walk;
}; };
......
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