diff --git a/src/Makefile b/src/Makefile index dfd8d63..bc1870d 100644 --- a/src/Makefile +++ b/src/Makefile @@ -71,7 +71,8 @@ PLANETSPLITTER_OBJ=planetsplitter.o \ files.o logging.o logerror.o errorlogx.o \ results.o queue.o sorting.o \ xmlparse.o tagging.o \ - uncompress.o osmxmlparse.o osmpbfparse.o osmo5mparse.o osmparser.o + uncompress.o osmxmlparse.o osmpbfparse.o osmo5mparse.o osmparser.o \ + srtmHgtReader.o planetsplitter : $(PLANETSPLITTER_OBJ) $(LD) $(PLANETSPLITTER_OBJ) -o $@ $(LDFLAGS) @@ -84,7 +85,8 @@ PLANETSPLITTER_SLIM_OBJ=planetsplitter-slim.o \ files.o logging.o logerror-slim.o errorlogx-slim.o \ results.o queue.o sorting.o \ xmlparse.o tagging.o \ - uncompress.o osmxmlparse.o osmpbfparse.o osmo5mparse.o osmparser.o + uncompress.o osmxmlparse.o osmpbfparse.o osmo5mparse.o osmparser.o \ + srtmHgtReader.o planetsplitter-slim : $(PLANETSPLITTER_SLIM_OBJ) $(LD) $(PLANETSPLITTER_SLIM_OBJ) -o $@ $(LDFLAGS) diff --git a/src/optimiser.c b/src/optimiser.c index 0f22fae..41d52c2 100644 --- a/src/optimiser.c +++ b/src/optimiser.c @@ -33,7 +33,7 @@ /*+ To help when debugging +*/ -#define DEBUG 0 +//#define DEBUG 0 /* Global variables */ @@ -170,7 +170,15 @@ Results *FindNormalRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r /* must obey one-way restrictions (unless profile allows) */ if(profile->oneway && IsOnewayTo(segmentp,node1)) - goto endloop; + { + if (profile->allow != Transports_Bicycle) + goto endloop; + wayp=LookupWay(ways,segmentp->way,1); + + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; + printf(" FindNormalRoute(...,start_node=%"Pindex_t" prev_segment=%"Pindex_t" finish_node=%"Pindex_t") props=%d DoubleSens=%d \n",start_node,prev_segment,finish_node,wayp->props, Properties_DoubleSens); + } if(IsFakeNode(node1) || IsFakeNode(node2)) { @@ -247,7 +255,7 @@ Results *FindNormalRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r if(option_quickest==0) segment_score=(score_t)DISTANCE(segmentp->distance)/segment_pref; else - segment_score=(score_t)Duration(segmentp,wayp,profile)/segment_pref; + segment_score=(score_t)Duration(node1,segmentp,wayp,profile)/segment_pref; cumulative_score=result1->score+segment_score; @@ -358,7 +366,7 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r int force_uturn=0; #if DEBUG - printf(" FindMiddleRoute(...,[begin has %d nodes],[end has %d nodes])\n",begin->number,end->number); + printf(" FindMiddleRoute(...,[begin has %d nodes],[end has %d nodes] finish_node=%"Pindex_t " )\n",begin->number,end->number,end->finish_node); #endif #if !DEBUG @@ -463,6 +471,10 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r index_t node1,seg1; index_t turnrelation=NO_RELATION; +#if DEBUG + printf("A Pop result1->node=%"Pindex_t" seg1=%"Pindex_t" score=%f finish_score=%f \n",result1->node,result1->segment,result1->score,finish_score); +#endif + /* score must be better than current best score */ if(result1->score>=finish_score) continue; @@ -494,7 +506,16 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r /* must obey one-way restrictions (unless profile allows) */ if(profile->oneway && IsOnewayTo(segmentp,node1)) - goto endloop; + { + if (profile->allow != Transports_Bicycle) + goto endloop; + wayp=LookupWay(ways,segmentp->way,1); + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; +#if DEBUG + printf("B FindMiddleRoute() node1=%"Pindex_t" node2=%"Pindex_t" props=%d DoubleSens=%d \n",segmentp->node1,segmentp->node2,wayp->props, Properties_DoubleSens); +#endif + } seg2=IndexSegment(segments,segmentp); /* segment cannot be a fake segment (must be a super-segment) */ @@ -559,14 +580,20 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r if(option_quickest==0) segment_score=(score_t)DISTANCE(segmentp->distance)/segment_pref; else - segment_score=(score_t)Duration(segmentp,wayp,profile)/segment_pref; + segment_score=(score_t)Duration(node1,segmentp,wayp,profile)/segment_pref; cumulative_score=result1->score+segment_score; - +#if DEBUG + printf("BTestsok node1=%"Pindex_t" node2=%"Pindex_t" seg2=%"Pindex_t" dist=%08x segment_pref=%f segment_score=%f cumulative_score=%f finish_score=%f\n",node1,node2,seg2,DISTANCE(segmentp->distance),segment_pref,segment_score,cumulative_score,finish_score); +#endif /* score must be better than current best score */ if(cumulative_score>=finish_score) + { +#if DEBUG + printf("Bcum>finish segmentp->node1=%"Pindex_t" node2=%"Pindex_t" seg1=%"Pindex_t" seg2=%"Pindex_t"\n",segmentp->node1,segmentp->node2,seg1,seg2); +#endif goto endloop; - + } result2=FindResult(results,node2,seg2); if(!result2) /* New end node/segment pair */ @@ -581,15 +608,25 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r result2->score=cumulative_score; } else + { +#if DEBUG + printf("Bcum>=score prec result2->node=%"Pindex_t" seg=%"Pindex_t" result2->score=%f cumulative_score=%f\n",result2->node,result2->segment,result2->score,cumulative_score); +#endif goto endloop; - + } if((result3=FindResult(end,node2,seg2))) { +#if DEBUG + printf("Cend result3->node=%"Pindex_t" seg=%"Pindex_t" score=%f result2->node=%"Pindex_t "seg=%"Pindex_t" score=%f finish_score=%f\n",result3->node,result3->segment,result3->score,result2->node,result2->segment,result2->score,finish_score); +#endif if((result2->score+result3->score)score+result3->score; finish_result=result2; - } +#if DEBUG + printf("Cend finish_result->node=%"Pindex_t" seg=%"Pindex_t" finish_score=%f\n",finish_result->node,finish_result->segment,finish_score); +#endif + } } else { @@ -605,6 +642,9 @@ Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *r potential_score=result2->score+(score_t)direct/profile->max_pref; else potential_score=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref; +#if DEBUG + printf("Cpot result2->node=%"Pindex_t "seg=%"Pindex_t" potential_score=%f node,result2->segment,potential_score,finish_score); +#endif if(potential_scoreoneway && IsOnewayTo(segmentp,node1)) - goto endloop; + { + if (profile->allow != Transports_Bicycle) + goto endloop; + wayp=LookupWay(ways,segmentp->way,1); + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; + printf(" FindStartRoutes(...,start_node=%"Pindex_t" prev_segment=%"Pindex_t" finish_node=%"Pindex_t") props=%d DoubleSens=%d\n",start_node,prev_segment,finish_node,wayp->props,Properties_DoubleSens); + } if(IsFakeNode(node1) || IsFakeNode(node2)) { @@ -1046,7 +1096,7 @@ Results *FindStartRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations *r if(option_quickest==0) segment_score=(score_t)DISTANCE(segmentp->distance)/segment_pref; else - segment_score=(score_t)Duration(segmentp,wayp,profile)/segment_pref; + segment_score=(score_t)Duration(node1,segmentp,wayp,profile)/segment_pref; cumulative_score=result1->score+segment_score; @@ -1268,7 +1318,14 @@ Results *ExtendStartRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations /* must obey one-way restrictions (unless profile allows) */ if(profile->oneway && IsOnewayTo(segmentp,node1)) - goto endloop; + { + if (profile->allow != Transports_Bicycle) + goto endloop; + wayp=LookupWay(ways,segmentp->way,1); + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; +printf(" ExtendStartRoutes(...,[begin has %d nodes],finish_node=%"Pindex_t") props=%d DoubleSens=%d\n",begin->number,finish_node, wayp->props,Properties_DoubleSens); + } if(IsFakeNode(node1) || IsFakeNode(node2)) { @@ -1334,7 +1391,7 @@ Results *ExtendStartRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations if(option_quickest==0) segment_score=(score_t)DISTANCE(segmentp->distance)/segment_pref; else - segment_score=(score_t)Duration(segmentp,wayp,profile)/segment_pref; + segment_score=(score_t)Duration(node1,segmentp,wayp,profile)/segment_pref; cumulative_score=result1->score+segment_score; @@ -1492,7 +1549,14 @@ Results *FindFinishRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations * /* must obey one-way restrictions (unless profile allows) */ if(profile->oneway && IsOnewayFrom(segmentp,node1)) /* working backwards => disallow oneway *from* node1 */ - goto endloop; + { + if (profile->allow != Transports_Bicycle) + goto endloop; + wayp=LookupWay(ways,segmentp->way,1); + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; + printf(" FindFinishRoutes(...,finish_node=%"Pindex_t") props=%d DoubleSens=%d\n",finish_node,wayp->props,Properties_DoubleSens); + } node2=OtherNode(segmentp,node1); @@ -1565,7 +1629,7 @@ Results *FindFinishRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations * if(option_quickest==0) segment_score=(score_t)DISTANCE(segmentp->distance)/segment_pref; else - segment_score=(score_t)Duration(segmentp,wayp,profile)/segment_pref; + segment_score=(score_t)Duration(node2,segmentp,wayp,profile)/segment_pref; cumulative_score=result1->score+segment_score; diff --git a/src/osmparser.c b/src/osmparser.c index a310326..698a858 100644 --- a/src/osmparser.c +++ b/src/osmparser.c @@ -69,6 +69,7 @@ static node_t relation_via=NO_NODE_ID; static double parse_speed(way_t id,const char *k,const char *v); static double parse_weight(way_t id,const char *k,const char *v); static double parse_length(way_t id,const char *k,const char *v); +static double parse_incline(way_t id,const char *k,const char *v); /*++++++++++++++++++++++++++++++++++++++ @@ -525,6 +526,15 @@ void ProcessWayTags(TagList *tags,int64_t way_id,int mode) break; + case 'c': + if(!strcmp(k,"cycleway")) + { + if(!strcmp(v,"opposite_lane")) + way.props|=Properties_DoubleSens; + recognised=1; break; + } + break; + case 'f': if(!strcmp(k,"foot")) { @@ -582,6 +592,16 @@ void ProcessWayTags(TagList *tags,int64_t way_id,int mode) break; + case 'i': + if(!strcmp(k,"incline")) + { +/* logerror("Way %"Pway_t" has an 'incline' = '%s' \n",logerror_way(id),v); */ + way.incline=pourcent_to_incline(parse_incline(id,k,v)); + recognised=1; break; + } + break; + + case 'l': if(!strcmp(k,"lanes")) { @@ -1140,3 +1160,43 @@ static double parse_length(way_t id,const char *k,const char *v) return(0); } + +/*++++++++++++++++++++++++++++++++++++++ + Convert a string containing an inclination into a double precision. + + double parse_incline Returns the inclination in % if it can be parsed. + + way_t id The way being processed. + + const char *k The tag key. + + const char *v The tag value. + ++++++++++++++++++++++++++++++++++++++*/ + +static double parse_incline(way_t id,const char *k,const char *v) +{ + char *ev; + double value=strtod(v,&ev); + + if(v==ev) + logerror("Way %"Pway_t" has an unrecognised tag '%s' = '%s' (after tagging rules); ignoring it.\n",logerror_way(id),k,v); + else + { + while(isspace(*ev)) ev++; + + if(!strcmp(ev,"°")) + { + if (value > 0) + return(100*tan(value)); + else + return(-100*tan(-value)); + } + + if(*ev==0 || !strcmp(ev,"%")) + return(value); + + logerror("Way %"Pway_t" has an un-parseable tag '%s' = '%s' (after tagging rules); ignoring it.\n",logerror_way(id),k,v); + } + + return(0); +} diff --git a/src/output.c b/src/output.c index c2481e9..42b4163 100644 --- a/src/output.c +++ b/src/output.c @@ -383,8 +383,11 @@ void PrintRoute(Results **results,int nresults,Nodes *nodes,Segments *segments,W resultwayp=LookupWay(ways,resultsegmentp->way,1); seg_distance+=DISTANCE(resultsegmentp->distance); - seg_duration+=Duration(resultsegmentp,resultwayp,profile); - + /* pour calcul en fonction de incline */ + if (result->node == resultsegmentp->node1) + seg_duration+=Duration(resultsegmentp->node2,resultsegmentp,resultwayp,profile); + else + seg_duration+=Duration(resultsegmentp->node1,resultsegmentp,resultwayp,profile); /* Calculate the cumulative distance/duration */ junc_distance+=seg_distance; diff --git a/src/prunex.c b/src/prunex.c index a7d1d11..9ccc252 100644 --- a/src/prunex.c +++ b/src/prunex.c @@ -1288,10 +1288,20 @@ static void modify_segment(SegmentsX *segmentsx,SegmentX *segmentx,index_t newno if(segmentx->distance&(ONEWAY_2TO1|ONEWAY_1TO2)) segmentx->distance^=ONEWAY_2TO1|ONEWAY_1TO2; + if(segmentx->distance&(INCLINEUP_2TO1|INCLINEUP_1TO2)) + segmentx->distance^=INCLINEUP_2TO1|INCLINEUP_1TO2; temp=newnode1; newnode1=newnode2; newnode2=temp; + + float tmp; + tmp=segmentx->ascent; + segmentx->ascent=segmentx->descent; + segmentx->descent = tmp; + tmp=segmentx->ascentOn; + segmentx->ascentOn=segmentx->descentOn; + segmentx->descentOn=tmp; } if(newnode1!=segmentx->node1) diff --git a/src/results.c b/src/results.c index ad456ff..d2ba837 100644 --- a/src/results.c +++ b/src/results.c @@ -221,7 +221,11 @@ Result *InsertResult(Results *results,index_t node,index_t segment) result->score=0; result->sortby=0; - + result->ascent=0; + result->descent=0; + result->ascentOn=0; + result->descentOn=0; + result->queued=NOT_QUEUED; return(result); diff --git a/src/results.h b/src/results.h index bda8be2..9b88ae2 100644 --- a/src/results.h +++ b/src/results.h @@ -49,6 +49,11 @@ struct _Result score_t score; /*+ The best actual weighted distance or duration score from the start to the node. +*/ score_t sortby; /*+ The best possible weighted distance or duration score from the start to the finish. +*/ + + float ascent; + float descent; + float ascentOn; + float descentOn; uint32_t queued; /*+ The position of this result in the queue. +*/ diff --git a/src/segments.c b/src/segments.c index 1656412..299b7ea 100644 --- a/src/segments.c +++ b/src/segments.c @@ -144,8 +144,8 @@ index_t FindClosestSegmentHeading(Nodes *nodes,Segments *segments,Ways *ways,ind if(!IsNormalSegment(segmentp)) goto endloop; - if(profile->oneway && IsOnewayFrom(segmentp,node1)) - goto endloop; +// if(profile->oneway && IsOnewayFrom(segmentp,node1)) +// goto endloop; if(IsFakeNode(node1) || IsFakeNode(node2)) seg2=IndexFakeSegment(segmentp); @@ -153,10 +153,23 @@ index_t FindClosestSegmentHeading(Nodes *nodes,Segments *segments,Ways *ways,ind seg2=IndexSegment(segments,segmentp); wayp=LookupWay(ways,segmentp->way,1); - + if(!(wayp->allow&profile->allow)) goto endloop; + if(profile->oneway && IsOnewayFrom(segmentp,node1)) + { + if (profile->allow != Transports_Bicycle) + goto endloop; + if (!(wayp->props & Properties_DoubleSens)) + goto endloop; + #if DEBUG + printf(" FindClosestSegmentHeading(...,node1=%"Pindex_t",node2=%"Pindex_t") props=%d DoubleSens=%d\n",node1,node2,wayp->props,Properties_DoubleSens); +#endif + + } + + bearing=BearingAngle(nodes,segmentp,node1); difference=(heading-bearing); @@ -228,6 +241,8 @@ distance_t Distance(double lat1,double lon1,double lat2,double lon2) Calculate the duration of travel on a segment. duration_t Duration Returns the duration of travel. + + Node *node first node of the route Segment *segmentp The segment to traverse. @@ -236,10 +251,11 @@ distance_t Distance(double lat1,double lon1,double lat2,double lon2) Profile *profile The profile of the transport being used. ++++++++++++++++++++++++++++++++++++++*/ -duration_t Duration(Segment *segmentp,Way *wayp,Profile *profile) +duration_t Duration(index_t node, Segment *segmentp,Way *wayp,Profile *profile) { speed_t speed1=wayp->speed; speed_t speed2=profile->speed[HIGHWAY(wayp->type)]; + speed_t speedresult, speedcalc; distance_t distance=DISTANCE(segmentp->distance); if(speed1==0) @@ -247,17 +263,74 @@ duration_t Duration(Segment *segmentp,Way *wayp,Profile *profile) if(speed2==0) return(hours_to_duration(10)); else - return distance_speed_to_duration(distance,speed2); + speedresult=speed2; } else /* if(speed1!=0) */ { if(speed2==0) - return distance_speed_to_duration(distance,speed1); + speedresult=speed1; else if(speed1<=speed2) - return distance_speed_to_duration(distance,speed1); + speedresult=speed1; + else + speedresult=speed2; + } + if (profile->allow != Transports_Bicycle || (wayp->incline == 0 && segmentp->ascentOn == 0 && segmentp->descentOn == 0)) + return distance_speed_to_duration(distance,speedresult); + +#if DEBUG + printf(" incline=%d node=%"Pindex_t" seg->node1=%"Pindex_t" seg->node2=%"Pindex_t" ascentOn=%0.4f descentOn=%04f descent=%f distx=%08x\n",wayp->incline,node,segmentp->node1,segmentp->node2,segmentp->ascentOn,segmentp->descentOn,segmentp->descent,segmentp->distance ); +#endif + if (wayp->incline != 0) + { + if (segmentp->node1 == node && segmentp->distance & INCLINEUP_2TO1) + return distance_speed_to_duration(distance,speedresult); + + if (segmentp->node2 == node && segmentp->distance & INCLINEUP_1TO2) + return distance_speed_to_duration(distance,speedresult); + + if (abs(wayp->incline) < 50) + return distance_speed_to_duration(distance,speedresult); + + if (abs(wayp->incline) < 100) + speedcalc = 20 - abs(wayp->incline)/10; else - return distance_speed_to_duration(distance,speed2); + if (abs(wayp->incline) < 160) + speedcalc = 18 - abs(wayp->incline)/10; + else + speedcalc = 2; + +#if DEBUG + printf("Duration incline=%d speedcalc=%d result=%d\n",wayp->incline,speedcalc,speedresult ); +#endif } + else + { + float percent=0; + if (segmentp->node1 == node && segmentp->ascentOn > 0) + percent = segmentp->ascent/segmentp->ascentOn*100; + if (segmentp->node2 == node && segmentp->descentOn > 0) + percent = segmentp->descent/segmentp->descentOn*100; + + if(percent < 5) + return distance_speed_to_duration(distance,speedresult); + + if(percent < 10) + speedcalc = 20 - percent; + + if(percent < 16) + speedcalc = 18 - percent; + else + speedcalc = 2; + +#if DEBUG + printf("Duration percent: %0.2f speedcalc=%d result=%d\n",percent,speedcalc,speedresult ); +#endif + } + + if (speedcalc < speedresult) speedresult=speedcalc; + + return distance_speed_to_duration(distance,speedresult); + } diff --git a/src/segments.h b/src/segments.h index ab0572a..1b01d10 100644 --- a/src/segments.h +++ b/src/segments.h @@ -48,6 +48,11 @@ struct _Segment index_t way; /*+ The index of the way associated with the segment. +*/ distance_t distance; /*+ The distance between the nodes. +*/ + + float ascent; + float descent; + float ascentOn; + float descentOn; }; @@ -95,7 +100,7 @@ index_t FindClosestSegmentHeading(Nodes *nodes,Segments *segments,Ways *ways,ind distance_t Distance(double lat1,double lon1,double lat2,double lon2); -duration_t Duration(Segment *segmentp,Way *wayp,Profile *profile); +duration_t Duration(index_t node,Segment *segmentp,Way *wayp,Profile *profile); double TurnAngle(Nodes *nodes,Segment *segment1p,Segment *segment2p,index_t node); double BearingAngle(Nodes *nodes,Segment *segmentp,index_t node); diff --git a/src/segmentsx.c b/src/segmentsx.c index e70bb46..681103f 100644 --- a/src/segmentsx.c +++ b/src/segmentsx.c @@ -37,6 +37,8 @@ #include "logging.h" #include "sorting.h" +#include "srtmHgtReader.h" + /* Global variables */ @@ -131,7 +133,7 @@ void FreeSegmentList(SegmentsX *segmentsx) distance_t distance The distance between the nodes (or just the flags). ++++++++++++++++++++++++++++++++++++++*/ -void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t node2,distance_t distance) +void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t node2,distance_t distance, float ascent, float descent, float ascentOn, float descentOn) { SegmentX segmentx; @@ -145,6 +147,16 @@ void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t no if(distance&(ONEWAY_2TO1|ONEWAY_1TO2)) distance^=ONEWAY_2TO1|ONEWAY_1TO2; + if(distance&(INCLINEUP_2TO1|INCLINEUP_1TO2)) + distance^=INCLINEUP_2TO1|INCLINEUP_1TO2; + + float tmp; + tmp=ascent; + ascent=descent; + descent = tmp; + tmp=ascentOn; + ascentOn=descentOn; + descentOn=tmp; } segmentx.node1=node1; @@ -153,6 +165,12 @@ void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t no segmentx.way=way; segmentx.distance=distance; + segmentx.ascent=ascent; + segmentx.descent=descent; + segmentx.ascentOn=ascentOn; + segmentx.descentOn=descentOn; + + WriteFileBuffered(segmentsx->fd,&segmentx,sizeof(SegmentX)); segmentsx->number++; @@ -454,6 +472,18 @@ void ProcessSegments(SegmentsX *segmentsx,NodesX *nodesx,WaysX *waysx) segmentx.distance=DISTANCE(DistanceX(nodex1,nodex2))|DISTFLAG(segmentx.distance); segmentx.distance&=~SEGMENT_AREA; + + /* Compute the ascent descent */ + TSrtmAscentDescent ad; + ad = srtmGetAscentDescent( + radians_to_degrees(latlong_to_radians(nodex1->latitude)), radians_to_degrees(latlong_to_radians(nodex1->longitude)), + radians_to_degrees(latlong_to_radians(nodex2->latitude)), radians_to_degrees(latlong_to_radians(nodex2->longitude)), + (int)DISTANCE(segmentx.distance)); + + segmentx.ascent = ad.ascent; + segmentx.descent = ad.descent; + segmentx.ascentOn = ad.ascentOn; + segmentx.descentOn = ad.descentOn; /* Write the modified segment */ @@ -893,6 +923,16 @@ static int geographically_index(SegmentX *segmentx,index_t index) if(segmentx->distance&(ONEWAY_2TO1|ONEWAY_1TO2)) segmentx->distance^=ONEWAY_2TO1|ONEWAY_1TO2; + if(segmentx->distance&(INCLINEUP_2TO1|INCLINEUP_1TO2)) + segmentx->distance^=INCLINEUP_2TO1|INCLINEUP_1TO2; + + float tmp; + tmp=segmentx->ascent; + segmentx->ascent=segmentx->descent; + segmentx->descent = tmp; + tmp=segmentx->ascentOn; + segmentx->ascentOn=segmentx->descentOn; + segmentx->descentOn=tmp; } return(1); @@ -941,6 +981,11 @@ void SaveSegmentList(SegmentsX *segmentsx,const char *filename) segment.way =segmentx.way; segment.distance=segmentx.distance; + segment.ascent =segmentx.ascent; + segment.descent =segmentx.descent; + segment.ascentOn=segmentx.ascentOn; + segment.descentOn=segmentx.descentOn; + if(IsSuperSegment(&segment)) super_number++; if(IsNormalSegment(&segment)) diff --git a/src/segmentsx.h b/src/segmentsx.h index df04c62..bebfbec 100644 --- a/src/segmentsx.h +++ b/src/segmentsx.h @@ -47,6 +47,11 @@ struct _SegmentX index_t way; /*+ The WayX index of the way. +*/ distance_t distance; /*+ The distance between the nodes. +*/ + + float ascent; + float descent; + float ascentOn; + float descentOn; }; @@ -85,7 +90,7 @@ struct _SegmentsX SegmentsX *NewSegmentList(void); void FreeSegmentList(SegmentsX *segmentsx); -void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t node2,distance_t distance); +void AppendSegmentList(SegmentsX *segmentsx,index_t way,index_t node1,index_t node2,distance_t distance, float ascent, float descent, float ascentOn, float descentOn); void FinishSegmentList(SegmentsX *segmentsx); SegmentX *FirstSegmentX(SegmentsX *segmentsx,index_t nodeindex,int position); diff --git a/src/srtmHgtReader.c b/src/srtmHgtReader.c new file mode 100644 index 0000000..1b747dd --- /dev/null +++ b/src/srtmHgtReader.c @@ -0,0 +1,230 @@ +/* + * File: srtmHgtReader.cpp + * Author: Pavel Zbytovský + * + * Created on April 28, 2013, 12:01 AM + */ + +//#define SRTMSLIM 1 + +#include +#include //exit +#include //int16_t +#include + +#include "srtmHgtReader.h" //fmod + +//const int secondsPerPx = 1; //arc seconds per pixel (3 equals cca 90m) +//const int totalPx = 3601; +//const char* folder = "aster"; + +const int secondsPerPx = 3; //arc seconds per pixel (3 equals cca 90m) +const int totalPx = 1201; +const char* folder = "srtm"; + +FILE* srtmFd = NULL; + +int srtmLat = 255; //default never valid +int srtmLon = 255; + +unsigned char * srtmTile = NULL; + + +/** Prepares corresponding file if not opened */ + +void srtmLoadTile(int latDec, int lonDec){ + + if(srtmLat != latDec || srtmLon != lonDec) { + srtmLat = latDec; + srtmLon = lonDec; + + if(srtmFd != NULL){ + fclose(srtmFd); + } + + char filename[20]; + sprintf(filename, "%s/N%dE0%02d.hgt", folder, latDec, lonDec); + printf("Opening %s\n", filename); + + srtmFd = fopen(filename, "r"); + + if(srtmFd == NULL) { + printf("Error opening %s\n", filename); + exit(1); + } + +#if !SRTMSLIM + + if(srtmTile == NULL){ + srtmTile = (unsigned char*) malloc(totalPx * totalPx * 2); //allocate only once + } + + //read the whole tile + fread(srtmTile, 1, (2 * totalPx * totalPx), srtmFd); +#endif + } +} + + + +void srtmClose(){ + if(srtmFd != NULL){ + fclose(srtmFd); + } + +#if !SRTMSLIM + if(srtmTile != NULL){ + free(srtmTile); + } +#endif + +} + +/** Pixel idx from left bottom corner (0-1200) */ + +void srtmReadPx(int y, int x, int* height){ + + int row = (totalPx-1) - y; + int col = x; + int pos = (row * totalPx + col) * 2; + +#if SRTMSLIM + //seek and read 2 bytes short + + unsigned char buff[2];// = {0xFF, 0xFB}; //-5 (bigendian) + fseek(srtmFd, pos, SEEK_SET); + fread(&buff, 2, 1, srtmFd); +#else + //set correct buff pointer + unsigned char * buff = & srtmTile[pos]; +#endif + + //solve endianity (using int16_t) + int16_t hgt = 0 | (buff[0] << 8) | (buff[1] << 0); + + if(hgt == -32768) { + printf("ERROR: Void pixel found on xy(%d,%d) in latlon(%d,%d) tile.\n", x,y, srtmLat, srtmLon); + exit(1); + } + + *height = (int) hgt; +} + + +/** Returns interpolated height from four nearest points */ + +float srtmGetElevation(float lat, float lon){ + + int latDec = (int)lat; + int lonDec = (int)lon; + + float secondsLat = (lat-latDec) * 60 * 60; + float secondsLon = (lon-lonDec) * 60 * 60; + + srtmLoadTile(latDec, lonDec); + + //X coresponds to x/y values, + //everything easter/norhter (< S) is rounded to X. + // + // y ^ + // 3 | | S + // +-------+------- + // 0 | X | + // +-------+--------> + // (sec) 0 3 x (lon) + + //both values are 0-1199 (1200 reserved for interpolating) + + int y = secondsLat/secondsPerPx; + int x = secondsLon/secondsPerPx; + + //get norther and easter points + + int height[4]; + srtmReadPx(y, x, &height[2]); + srtmReadPx(y+1, x, &height[0]); + srtmReadPx(y, x+1, &height[3]); + srtmReadPx(y+1, x+1, &height[1]); + + //ratio where X lays + float dy = fmod(secondsLat, secondsPerPx) / secondsPerPx; + float dx = fmod(secondsLon, secondsPerPx) / secondsPerPx; + + // Bilinear interpolation + // h0------------h1 + // | + // |--dx-- . + // | | + // | dy + // | | + // h2------------h3 + + return height[0] * dy * (1 - dx) + + height[1] * dy * (dx) + + height[2] * (1 - dy) * (1 - dx) + + height[3] * (1 - dy) * dx; + +} + + +/** Returns amount of ascent and descent between points */ + +TSrtmAscentDescent srtmGetAscentDescent(float lat1, float lon1, float lat2, float lon2, float dist){ + + TSrtmAscentDescent ret = {0}; + + //segment we need to devide in "pixels" + double latDiff = lat2 - lat1; + double lonDiff = lon2 - lon1; + + //how many pixels there are both in y and x axis + double latSteps = latDiff * (3600 / 3); // 1/pixelDistance = cca 0.00083 + double lonSteps = lonDiff * (3600 / 3); + + //we use the max of both + int steps = fmax(fabs(latSteps), fabs(lonSteps)); + + //just in case both points are inside one pixel (we need interpolation!) + if(steps == 0) steps = 1; + + //set the delta of each step + double latStep = latDiff / steps; + double lonStep = lonDiff / steps; + double distStep = dist/steps; + + //printf("steps %d: %f %f %f\n", steps, latStep, lonStep, distStep); + + int i; + double lat = lat1, lon = lon1; + float height, lastHeight, eleDiff; + + //get first elevation -> we need eleDiff then + height = srtmGetElevation(lat, lon); + + //printf("first: %f %f hgt:%f\n", lat, lon, height); + + for(i=0; i 0){ + ret.ascent += eleDiff; + ret.ascentOn += distStep; + } + else{ + ret.descent += -eleDiff; + ret.descentOn += distStep; + } + + //printf("LL(%d): %f %f hgt: %0.1f, diff %0.1f\n", i, lat, lon, height, eleDiff); + } + + // printf("last: %f %f\n", i, lat, lon); == printf("ll2: %f %f\n", i, lat2, lon2); + + return ret; + +} diff --git a/src/srtmHgtReader.h b/src/srtmHgtReader.h new file mode 100644 index 0000000..c8ff189 --- /dev/null +++ b/src/srtmHgtReader.h @@ -0,0 +1,32 @@ +/* + * File: srtmHgtReader.h + * Author: Pavel Zbytovský + * + * Created on April 28, 2013, 6:44 PM + */ + +#ifndef SRTMHGTREADER_H + +#define SRTMHGTREADER_H + +void srtmLoadTile(int latDec, int lonDec); + +void srtmReadPx(int y, int x, int* height); + +float srtmGetElevation(float lat, float lon); + +void srtmClose(); + +struct _SrtmAscentDescent { + float ascent; + float descent; + float ascentOn; + float descentOn; +}; + +typedef struct _SrtmAscentDescent TSrtmAscentDescent; + +TSrtmAscentDescent srtmGetAscentDescent(float lat1, float lon1, float lat2, float lon2, float dist); + +#endif /* SRTMHGTREADER_H */ + diff --git a/src/superx.c b/src/superx.c index 81483b7..8b4d9f3 100644 --- a/src/superx.c +++ b/src/superx.c @@ -291,10 +291,34 @@ SegmentsX *CreateSuperSegments(NodesX *nodesx,SegmentsX *segmentsx,WaysX *waysx) { if(IsBitSet(nodesx->super,result->node) && result->segment!=NO_SEGMENT) { + distance_t segment_flags=0; if(wayx->way.type&Highway_OneWay && result->node!=i) - AppendSegmentList(supersegmentsx,segmentx->way,i,result->node,DISTANCE((distance_t)result->score)|ONEWAY_1TO2); - else - AppendSegmentList(supersegmentsx,segmentx->way,i,result->node,DISTANCE((distance_t)result->score)); + segment_flags|=ONEWAY_1TO2; + + if (wayx->way.incline != 0 && result->node!=i) + { + SegmentX *segmentxres; + segmentxres=LookupSegmentX(segmentsx,result->segment,2); + if (segmentxres->node1==i) + segment_flags|= (segmentxres->distance & (INCLINEUP_1TO2|INCLINEUP_2TO1)) ; + else if (segmentxres->node2==i) + { + segment_flags= INCLINEUP_1TO2|INCLINEUP_2TO1; + segment_flags^=(segmentxres->distance & (INCLINEUP_1TO2|INCLINEUP_2TO1)) ; + } + else if (segmentxres->node2==result->node) + segment_flags|= (segmentxres->distance & (INCLINEUP_1TO2|INCLINEUP_2TO1)) ; + else if (segmentxres->node1==result->node) + { + segment_flags= INCLINEUP_1TO2|INCLINEUP_2TO1; + segment_flags^=(segmentxres->distance & (INCLINEUP_1TO2|INCLINEUP_2TO1)) ; + } +#ifdef DEBUG + printf("createsupersegments-INCLINE=%d node1=%"Pindex_t" node2=%"Pindex_t" segxnode1=%"Pindex_t" segxnode2=%"Pindex_t" way=%"Pindex_t" segx=%"Pindex_t" segdist=%08x calcflags=%08x\n",wayx->way.incline,i,result->node,segmentxres->node1,segmentxres->node2,segmentxres->way,result->segment,segmentxres->distance,segment_flags); +#endif + } + + AppendSegmentList(supersegmentsx,segmentx->way,i,result->node,DISTANCE((distance_t)result->score)|segment_flags, result->ascent, result->descent, result->ascentOn, result->descentOn); ss++; } @@ -409,7 +433,7 @@ SegmentsX *MergeSuperSegments(SegmentsX *segmentsx,SegmentsX *supersegmentsx) (segmentx.node1>supersegmentx.node1)) { /* mark as super-segment */ - AppendSegmentList(mergedsegmentsx,supersegmentx.way,supersegmentx.node1,supersegmentx.node2,supersegmentx.distance|SEGMENT_SUPER); + AppendSegmentList(mergedsegmentsx,supersegmentx.way,supersegmentx.node1,supersegmentx.node2,supersegmentx.distance|SEGMENT_SUPER,supersegmentx.ascent, supersegmentx.descent, supersegmentx.ascentOn, supersegmentx.descentOn); added++; j++; } @@ -421,9 +445,9 @@ SegmentsX *MergeSuperSegments(SegmentsX *segmentsx,SegmentsX *supersegmentsx) } if(super) - AppendSegmentList(mergedsegmentsx,segmentx.way,segmentx.node1,segmentx.node2,segmentx.distance|SEGMENT_SUPER|SEGMENT_NORMAL); + AppendSegmentList(mergedsegmentsx,segmentx.way,segmentx.node1,segmentx.node2,segmentx.distance|SEGMENT_SUPER|SEGMENT_NORMAL, segmentx.ascent, segmentx.descent, segmentx.ascentOn, segmentx.descentOn); else - AppendSegmentList(mergedsegmentsx,segmentx.way,segmentx.node1,segmentx.node2,segmentx.distance|SEGMENT_NORMAL); + AppendSegmentList(mergedsegmentsx,segmentx.way,segmentx.node1,segmentx.node2,segmentx.distance|SEGMENT_NORMAL, segmentx.ascent, segmentx.descent, segmentx.ascentOn, segmentx.descentOn); if(!((i+1)%10000)) printf_middle("Merging Segments: Segments=%"Pindex_t" Super=%"Pindex_t" Merged=%"Pindex_t" Added=%"Pindex_t,i+1,j,merged,added); @@ -534,7 +558,20 @@ static Results *FindSuperRoutes(NodesX *nodesx,SegmentsX *segmentsx,WaysX *waysx result2=InsertResult(results,node2,seg2); result2->prev=result1; result2->score=cumulative_distance; - + if (node1 == segmentx->node1) + { + result2->ascent = result1->ascent + segmentx->ascent; + result2->descent = result1->descent + segmentx->descent; + result2->ascentOn = result1->ascentOn + segmentx->ascentOn; + result2->descentOn = result1->descentOn + segmentx->descentOn; + } + else + { + result2->ascent = result1->ascent + segmentx->descent; + result2->descent = result1->descent + segmentx->ascent; + result2->ascentOn = result1->ascentOn + segmentx->descentOn; + result2->descentOn = result1->descentOn + segmentx->ascentOn; + } /* don't route beyond a super-node. */ if(!IsBitSet(nodesx->super,node2)) InsertInQueue(queue,result2,cumulative_distance); @@ -544,6 +581,21 @@ static Results *FindSuperRoutes(NodesX *nodesx,SegmentsX *segmentsx,WaysX *waysx result2->prev=result1; result2->score=cumulative_distance; + if (node1 == segmentx->node1) + { + result2->ascent = result1->ascent + segmentx->ascent; + result2->descent = result1->descent + segmentx->descent; + result2->ascentOn = result1->ascentOn + segmentx->ascentOn; + result2->descentOn = result1->descentOn + segmentx->descentOn; + } + else + { + result2->ascent = result1->ascent + segmentx->descent; + result2->descent = result1->descent + segmentx->ascent; + result2->ascentOn = result1->ascentOn + segmentx->descentOn; + result2->descentOn = result1->descentOn + segmentx->ascentOn; + } + /* don't route beyond a super-node. */ if(!IsBitSet(nodesx->super,node2)) InsertInQueue(queue,result2,cumulative_distance); diff --git a/src/types.c b/src/types.c index 9538486..5b1cc11 100644 --- a/src/types.c +++ b/src/types.c @@ -163,6 +163,11 @@ Property PropertyType(const char *property) return(Property_Bridge); break; + case 'd': + if(!strcmp(property,"doublesens")) + return(Property_DoubleSens); + break; + case 'f': if(!strcmp(property,"footroute")) return(Property_FootRoute); @@ -325,6 +330,9 @@ const char *PropertyName(Property property) case Property_BicycleRoute: return("bicycleroute"); + case Property_DoubleSens: + return("doublesens"); + case Property_Count: ; } diff --git a/src/types.h b/src/types.h index 69ea57f..fd38fdb 100644 --- a/src/types.h +++ b/src/types.h @@ -103,11 +103,17 @@ /*+ A flag to mark a segment as a normal segment. +*/ #define SEGMENT_NORMAL ((distance_t)0x08000000) +/*+ A flag to mark a segment as incline up from node1 to node2. +*/ +#define INCLINEUP_1TO2 ((distance_t)0x04000000) + +/*+ A flag to mark a segment as incline up from node2 to node1. +*/ +#define INCLINEUP_2TO1 ((distance_t)0x02000000) + /*+ The real distance ignoring the other flags. +*/ -#define DISTANCE(xx) ((distance_t)((xx)&(~(SEGMENT_AREA|ONEWAY_1TO2|ONEWAY_2TO1|SEGMENT_SUPER|SEGMENT_NORMAL)))) +#define DISTANCE(xx) ((distance_t)((xx)&(~(SEGMENT_AREA|ONEWAY_1TO2|ONEWAY_2TO1|SEGMENT_SUPER|SEGMENT_NORMAL|INCLINEUP_1TO2|INCLINEUP_2TO1)))) /*+ The distance flags selecting only the flags. +*/ -#define DISTFLAG(xx) ((distance_t)((xx)&(SEGMENT_AREA|ONEWAY_1TO2|ONEWAY_2TO1|SEGMENT_SUPER|SEGMENT_NORMAL))) +#define DISTFLAG(xx) ((distance_t)((xx)&(SEGMENT_AREA|ONEWAY_1TO2|ONEWAY_2TO1|SEGMENT_SUPER|SEGMENT_NORMAL|INCLINEUP_1TO2|INCLINEUP_2TO1))) /*+ A very large almost infinite distance. +*/ @@ -331,8 +337,8 @@ typedef enum _Property Property_Tunnel = 4, Property_FootRoute = 5, Property_BicycleRoute = 6, - - Property_Count = 7 /* One more than the number of property types. */ + Property_DoubleSens = 7, + Property_Count = 8 /* One more than the number of property types. */ } Property; @@ -353,7 +359,7 @@ typedef enum _Properties Properties_Tunnel = PROPERTIES(Property_Tunnel ), Properties_FootRoute = PROPERTIES(Property_FootRoute ), Properties_BicycleRoute = PROPERTIES(Property_BicycleRoute), - + Properties_DoubleSens = PROPERTIES(Property_DoubleSens ), Properties_ALL = PROPERTIES(Property_Count )-1 } Properties; @@ -374,6 +380,9 @@ typedef uint8_t width_t; /*+ The maximum length of a way, measured in multiples of 0.1 metres. +*/ typedef uint8_t length_t; +/*+ The maximum inclination of a way, measured in multiples of 0.1 %. +*/ +typedef int16_t incline_t; + /*+ Conversion of km/hr to speed_t. +*/ #define kph_to_speed(xxx) (speed_t)(xxx) @@ -405,6 +414,11 @@ typedef uint8_t length_t; /*+ Conversion of length_t to metres. +*/ #define length_to_metres(xxx) ((double)(xxx)/10.0) +/*+ Conversion of pourcent to incline_t. +*/ +#define pourcent_to_incline(xxx) (incline_t)((xxx)*10) + +/*+ Conversion of incline_t to % . +*/ +#define incline_to_pourcent(xxx) ((double)(xxx)/10.0) /* Data structures */ diff --git a/src/ways.c b/src/ways.c index 60cb08d..2c08c91 100644 --- a/src/ways.c +++ b/src/ways.c @@ -137,5 +137,8 @@ int WaysCompare(Way *way1p,Way *way2p) if(way1p->length!=way2p->length) return((int)way1p->length - (int)way2p->length); + if(way1p->incline!=way2p->incline) + return((int)way1p->incline - (int)way2p->incline); + return(0); } diff --git a/src/ways.h b/src/ways.h index b4f4066..c58e7a8 100644 --- a/src/ways.h +++ b/src/ways.h @@ -52,6 +52,7 @@ struct _Way height_t height; /*+ The defined maximum height of traffic on the way. +*/ width_t width; /*+ The defined maximum width of traffic on the way. +*/ length_t length; /*+ The defined maximum length of traffic on the way. +*/ + incline_t incline; /*+ The maximum inclination of the way +*/ }; diff --git a/src/waysx.c b/src/waysx.c index d7d737d..adaa27e 100644 --- a/src/waysx.c +++ b/src/waysx.c @@ -460,8 +460,19 @@ SegmentsX *SplitWays(WaysX *waysx,NodesX *nodesx,int keep) if(wayx.way.type&Highway_Area) segment_flags|=SEGMENT_AREA; + + if (wayx.way.incline != 0) + { + if (wayx.way.incline > 0) + segment_flags|=INCLINEUP_1TO2; + else + segment_flags|=INCLINEUP_2TO1; +#ifdef DEBUG + printf("splitways-INCLINE=%d node1=%"Pindex_t" node2=%"Pindex_t" way=%"Pindex_t" segflags=%08x\n",wayx.way.incline,previndex,index,i,segment_flags); +#endif + } - AppendSegmentList(segmentsx,i,previndex,index,segment_flags); + AppendSegmentList(segmentsx,i,previndex,index,segment_flags,0,0,0,0); } prevnode=node; @@ -480,6 +491,12 @@ SegmentsX *SplitWays(WaysX *waysx,NodesX *nodesx,int keep) WriteFileBuffered(fd,&wayx,sizeof(WayX)); size+=sizeof(index_t); +#ifdef DEBUG + if (wayx.way.incline != 0) + { + printf("splitways-INCLINE way=%"Pindex_t" name=%s\n",i,name); + } +#endif WriteFileBuffered(nfd,&size,FILESORT_VARSIZE); WriteFileBuffered(nfd,&i,sizeof(index_t));