Results *FindFinishRoutes(Nodes *nodes,Segments *segments,Ways *ways,index_t finish,Profile *profile)
{
Results *results;
Queue *queue;
index_t node1,node2;
Result *result1,*result2;
Segment *segment;
Way *way;
/* Insert the first node into the queue */
results=NewResultsList(8);
results->finish=finish;
result1=InsertResult(results,finish);
ZeroResult(result1);
queue=NewQueueList();
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
node1=result1->node;
if(IsFakeNode(node1))
segment=FirstFakeSegment(node1);
else
segment=FirstSegment(segments,nodes,node1);
while(segment)
{
score_t segment_pref,segment_score,cumulative_score;
int i;
if(!IsNormalSegment(segment))
goto endloop;
if(profile->oneway && IsOnewayFrom(segment,node1))
goto endloop;
node2=OtherNode(segment,node1);
if(result1->next==node2)
goto endloop;
way=LookupWay(ways,segment->way);
if(!(way->allow&profile->allow))
goto endloop;
if(!profile->highway[HIGHWAY(way->type)])
goto endloop;
if(way->weight && way->weight<profile->weight)
goto endloop;
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
if(segment_pref==0)
goto endloop;
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
result2=FindResult(results,node2);
if(!result2) /* New end node */
{
result2=InsertResult(results,node2);
result2->prev=NO_NODE;
result2->next=node1;
result2->score=cumulative_score;
result2->segment=segment;
if(!IsFakeNode(node2) && !IsSuperNode(nodes,node2))
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New end node is better */
{
result2->next=node1;
result2->score=cumulative_score;
result2->segment=segment;
if(!IsFakeNode(node2) && !IsSuperNode(nodes,node2))
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
endloop:
if(IsFakeNode(node1))
segment=NextFakeSegment(segment,node1);
else
segment=NextSegment(segments,segment,node1);
}
}
FreeQueueList(queue);
/* Check it worked */
if(results->number==1)
{
FreeResultsList(results);
return(NULL);
}
return(results);
}
static Results *FindRoutesWay(NodesX *nodesx,SegmentsX *segmentsx,WaysX *waysx,node_t start,Way *match)
{
Results *results;
Queue *queue;
Result *result1,*result2;
WayX *wayx;
/* Insert the first node into the queue */
results=NewResultsList(4);
queue=NewQueueList();
result1=InsertResult(results,start,NO_SEGMENT);
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
index_t node1;
SegmentX *segmentx;
node1=result1->node;
segmentx=FirstSegmentX(segmentsx,node1,2); /* position 1 is already used */
while(segmentx)
{
index_t node2,seg2;
distance_t cumulative_distance;
/* must not be one-way against the direction of travel */
if(IsOnewayTo(segmentx,node1))
goto endloop;
node2=OtherNode(segmentx,node1);
seg2=IndexSegmentX(segmentsx,segmentx);
/* must not be a u-turn */
if(result1->segment==seg2)
goto endloop;
wayx=LookupWayX(waysx,segmentx->way,2); /* position 1 is already used */
/* must be the right type of way */
if(WaysCompare(&wayx->way,match))
goto endloop;
cumulative_distance=(distance_t)result1->score+DISTANCE(segmentx->distance);
result2=FindResult(results,node2,seg2);
if(!result2) /* New end node */
{
result2=InsertResult(results,node2,seg2);
result2->prev=result1;
result2->score=cumulative_distance;
result2->sortby=cumulative_distance;
/* don't route beyond a super-node. */
if(!IsBitSet(nodesx->super,node2))
InsertInQueue(queue,result2);
}
else if(cumulative_distance<result2->score)
{
result2->prev=result1;
result2->score=cumulative_distance;
result2->sortby=cumulative_distance;
/* don't route beyond a super-node. */
if(!IsBitSet(nodesx->super,node2))
InsertInQueue(queue,result2);
}
endloop:
segmentx=NextSegmentX(segmentsx,segmentx,node1);
}
}
FreeQueueList(queue);
return(results);
}
Results *FindNormalRoute(Nodes *nodes,Segments *segments,Ways *ways,index_t start,index_t finish,Profile *profile)
{
Results *results;
Queue *queue;
index_t node1,node2;
score_t finish_score;
double finish_lat,finish_lon;
Result *result1,*result2;
Segment *segment;
Way *way;
/* Set up the finish conditions */
finish_score=INF_SCORE;
if(IsFakeNode(finish))
GetFakeLatLong(finish,&finish_lat,&finish_lon);
else
GetLatLong(nodes,finish,&finish_lat,&finish_lon);
/* Create the list of results and insert the first node into the queue */
results=NewResultsList(8);
results->start=start;
results->finish=finish;
result1=InsertResult(results,start);
ZeroResult(result1);
queue=NewQueueList();
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
if(result1->score>finish_score)
continue;
node1=result1->node;
if(IsFakeNode(node1))
segment=FirstFakeSegment(node1);
else
segment=FirstSegment(segments,nodes,node1);
while(segment)
{
score_t segment_pref,segment_score,cumulative_score;
int i;
node2=OtherNode(segment,node1); /* need this here because we use node2 later */
if(!IsNormalSegment(segment))
goto endloop;
if(profile->oneway && IsOnewayTo(segment,node1))
goto endloop;
if(result1->prev==node2)
goto endloop;
if(node2!=finish && !IsFakeNode(node2) && IsSuperNode(nodes,node2))
goto endloop;
way=LookupWay(ways,segment->way);
if(!(way->allow&profile->allow))
goto endloop;
if(!profile->highway[HIGHWAY(way->type)])
goto endloop;
if(way->weight && way->weight<profile->weight)
goto endloop;
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
if(segment_pref==0)
goto endloop;
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
if(cumulative_score>finish_score)
goto endloop;
result2=FindResult(results,node2);
if(!result2) /* New end node */
{
result2=InsertResult(results,node2);
result2->prev=node1;
result2->next=NO_NODE;
result2->score=cumulative_score;
result2->segment=segment;
if(node2==finish)
{
finish_score=cumulative_score;
}
else
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New end node is better */
{
result2->prev=node1;
result2->score=cumulative_score;
result2->segment=segment;
if(node2==finish)
{
finish_score=cumulative_score;
}
else
{
result2->sortby=result2->score;
if(result2->score<finish_score)
InsertInQueue(queue,result2);
}
}
endloop:
if(IsFakeNode(node1))
segment=NextFakeSegment(segment,node1);
else if(IsFakeNode(node2))
segment=NULL; /* cannot call NextSegment() with a fake segment */
else
{
segment=NextSegment(segments,segment,node1);
if(!segment && IsFakeNode(finish))
segment=ExtraFakeSegment(node1,finish);
}
}
}
FreeQueueList(queue);
/* Check it worked */
if(!FindResult(results,finish))
{
FreeResultsList(results);
return(NULL);
}
FixForwardRoute(results,finish);
return(results);
}
Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Results *begin,Results *end,Profile *profile)
{
Results *results;
Queue *queue;
index_t node1,node2;
index_t end_prev;
score_t finish_score;
double finish_lat,finish_lon;
Result *result1,*result2,*result3;
Segment *segment;
Way *way;
if(!option_quiet)
{
printf("Routing: Super-Nodes checked = 0");
fflush(stdout);
}
/* Set up the finish conditions */
finish_score=INF_DISTANCE;
end_prev=NO_NODE;
if(IsFakeNode(end->finish))
GetFakeLatLong(end->finish,&finish_lat,&finish_lon);
else
GetLatLong(nodes,end->finish,&finish_lat,&finish_lon);
/* Create the list of results and insert the first node into the queue */
results=NewResultsList(2048);
results->start=begin->start;
results->finish=end->finish;
result1=InsertResult(results,begin->start);
result3=FindResult(begin,begin->start);
*result1=*result3;
queue=NewQueueList();
/* Insert the finish points of the beginning part of the path into the queue */
result3=FirstResult(begin);
while(result3)
{
if(result3->node!=begin->start && !IsFakeNode(result3->node) && IsSuperNode(nodes,result3->node))
{
result2=InsertResult(results,result3->node);
*result2=*result3;
result2->prev=begin->start;
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
result3=NextResult(begin,result3);
}
if(begin->number==1)
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
if(result1->score>finish_score)
continue;
node1=result1->node;
segment=FirstSegment(segments,nodes,node1);
while(segment)
{
score_t segment_pref,segment_score,cumulative_score;
int i;
if(!IsSuperSegment(segment))
goto endloop;
if(profile->oneway && IsOnewayTo(segment,node1))
goto endloop;
node2=OtherNode(segment,node1);
if(result1->prev==node2)
goto endloop;
way=LookupWay(ways,segment->way);
if(!(way->allow&profile->allow))
goto endloop;
if(!profile->highway[HIGHWAY(way->type)])
goto endloop;
if(way->weight && way->weight<profile->weight)
goto endloop;
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
if(segment_pref==0)
goto endloop;
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
if(cumulative_score>finish_score)
goto endloop;
result2=FindResult(results,node2);
if(!result2) /* New end node */
{
result2=InsertResult(results,node2);
result2->prev=node1;
result2->next=NO_NODE;
result2->score=cumulative_score;
result2->segment=segment;
if((result3=FindResult(end,node2)))
{
if((result2->score+result3->score)<finish_score)
{
finish_score=result2->score+result3->score;
end_prev=node2;
}
}
else
{
double lat,lon;
distance_t direct;
GetLatLong(nodes,node2,&lat,&lon);
direct=Distance(lat,lon,finish_lat,finish_lon);
if(option_quickest==0)
result2->sortby=result2->score+(score_t)direct/profile->max_pref;
else
result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New end node is better */
{
result2->prev=node1;
result2->score=cumulative_score;
result2->segment=segment;
if((result3=FindResult(end,node2)))
{
if((result2->score+result3->score)<finish_score)
{
finish_score=result2->score+result3->score;
end_prev=node2;
}
}
else if(result2->score<finish_score)
{
double lat,lon;
distance_t direct;
GetLatLong(nodes,node2,&lat,&lon);
direct=Distance(lat,lon,finish_lat,finish_lon);
if(option_quickest==0)
result2->sortby=result2->score+(score_t)direct/profile->max_pref;
else
result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
InsertInQueue(queue,result2);
}
}
endloop:
if(!option_quiet && !(results->number%10000))
{
printf("\rRouting: Super-Nodes checked = %d",results->number);
fflush(stdout);
}
segment=NextSegment(segments,segment,node1);
}
}
if(!option_quiet)
{
printf("\rRouting: Super-Nodes checked = %d\n",results->number);
fflush(stdout);
}
/* Finish off the end part of the route. */
if(!FindResult(results,end->finish) && end_prev!=NO_NODE)
{
result2=InsertResult(results,end->finish);
result3=FindResult(end,end->finish);
*result2=*result3;
result2->prev=end_prev;
result2->score=finish_score;
}
FreeQueueList(queue);
/* Check it worked */
if(end_prev==NO_NODE)
{
FreeResultsList(results);
return(NULL);
}
FixForwardRoute(results,end->finish);
return(results);
}
Results *FindFinishRoutes(Nodes *nodes,Segments *segments,Ways *ways,Relations *relations,Profile *profile,index_t finish_node)
{
Results *results,*results2;
Queue *queue;
Result *result1,*result2,*result3;
/* Create the results and insert the finish node */
results=NewResultsList(64);
results->finish_node=finish_node;
result1=InsertResult(results,finish_node,NO_SEGMENT);
/* Insert the first node into the queue */
queue=NewQueueList();
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
index_t node1,seg1,seg1r;
Segment *segment;
index_t turnrelation=NO_RELATION;
node1=result1->node;
seg1=result1->segment;
if(IsFakeSegment(seg1))
seg1r=IndexRealSegment(seg1);
else
seg1r=seg1;
/* lookup if a turn restriction applies */
if(profile->turns && !IsFakeNode(node1) && IsTurnRestrictedNode(LookupNode(nodes,node1,1)))
turnrelation=FindFirstTurnRelation1(relations,node1); /* working backwards => turn relation sort order doesn't help */
/* Loop across all segments */
if(IsFakeNode(node1))
segment=FirstFakeSegment(node1);
else
segment=FirstSegment(segments,nodes,node1,1);
while(segment)
{
Way *way;
index_t node2,seg2,seg2r;
score_t segment_pref,segment_score,cumulative_score;
int i;
/* must be a normal segment */
if((IsFakeNode(node1) || !IsSuperNode(LookupNode(nodes,node1,1))) && !IsNormalSegment(segment))
goto endloop;
/* must obey one-way restrictions (unless profile allows) */
if(profile->oneway && IsOnewayFrom(segment,node1)) /* Disallow oneway from node2 *to* node1 */
goto endloop;
node2=OtherNode(segment,node1);
if(IsFakeNode(node1) || IsFakeNode(node2))
{
seg2 =IndexFakeSegment(segment);
seg2r=IndexRealSegment(seg2);
}
else
{
seg2 =IndexSegment(segments,segment);
seg2r=seg2;
}
/* must not perform U-turn (unless profile allows) */
if(profile->turns && (seg1==seg2 || seg1==seg2r || seg1r==seg2 || (seg1r==seg2r && IsFakeUTurn(seg1,seg2))))
goto endloop;
/* must obey turn relations */
if(turnrelation!=NO_RELATION)
{
index_t turnrelation2=FindFirstTurnRelation2(relations,node1,seg2r); /* node2 -> node1 -> result1->next->node */
if(turnrelation2!=NO_RELATION && !IsTurnAllowed(relations,turnrelation2,node1,seg2r,seg1r,profile->allow))
goto endloop;
}
way=LookupWay(ways,segment->way,1);
/* mode of transport must be allowed on the highway */
if(!(way->allow&profile->allow))
goto endloop;
/* must obey weight restriction (if exists) */
if(way->weight && way->weight<profile->weight)
goto endloop;
/* must obey height/width/length restriction (if exists) */
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->file.props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
/* profile preferences must allow this highway */
if(segment_pref==0)
goto endloop;
/* mode of transport must be allowed through node2 */
if(!IsFakeNode(node2))
{
Node *node=LookupNode(nodes,node2,2);
if(!(node->allow&profile->allow))
goto endloop;
}
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
result2=FindResult(results,node2,seg2);
if(!result2) /* New end node */
{
result2=InsertResult(results,node2,seg2);
result2->next=result1; /* working backwards */
result2->score=cumulative_score;
if(IsFakeNode(node1) || (!IsFakeNode(node1) && !IsSuperNode(LookupNode(nodes,node1,1)))) /* Overshoot by one segment */
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New end node is better */
{
result2->next=result1; /* working backwards */
result2->score=cumulative_score;
if(IsFakeNode(node1) || (!IsFakeNode(node1) && !IsSuperNode(LookupNode(nodes,node1,1)))) /* Overshoot by one segment */
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
endloop:
if(IsFakeNode(node1))
segment=NextFakeSegment(segment,node1);
else
segment=NextSegment(segments,segment,node1);
}
}
FreeQueueList(queue);
/* Check it worked */
if(results->number==1)
{
FreeResultsList(results);
return(NULL);
}
/* Create a results structure with the node at the end of the segment opposite the start */
results2=NewResultsList(64);
results2->finish_node=results->finish_node;
result3=FirstResult(results);
while(result3)
{
if(result3->next)
{
result2=InsertResult(results2,result3->next->node,result3->segment);
result2->score=result3->next->score;
}
result3=NextResult(results,result3);
}
/* Fix up the result->next pointers */
result3=FirstResult(results);
while(result3)
{
if(result3->next && result3->next->next)
{
result1=FindResult(results2,result3->next->node,result3->segment);
result2=FindResult(results2,result3->next->next->node,result3->next->segment);
result1->next=result2;
}
result3=NextResult(results,result3);
}
FreeResultsList(results);
return(results2);
}
Results *FindNormalRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *relations,Profile *profile,index_t start_node,index_t prev_segment,index_t finish_node)
{
Results *results;
Queue *queue;
score_t finish_score;
double finish_lat,finish_lon;
Result *finish_result;
Result *result1,*result2;
/* Set up the finish conditions */
finish_score=INF_SCORE;
finish_result=NULL;
if(IsFakeNode(finish_node))
GetFakeLatLong(finish_node,&finish_lat,&finish_lon);
else
GetLatLong(nodes,finish_node,&finish_lat,&finish_lon);
/* Create the list of results and insert the first node into the queue */
results=NewResultsList(64);
results->start_node=start_node;
results->prev_segment=prev_segment;
result1=InsertResult(results,results->start_node,results->prev_segment);
queue=NewQueueList();
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
Segment *segment;
index_t node1,seg1,seg1r;
index_t turnrelation=NO_RELATION;
/* score must be better than current best score */
if(result1->score>finish_score)
continue;
node1=result1->node;
seg1=result1->segment;
if(IsFakeSegment(seg1))
seg1r=IndexRealSegment(seg1);
else
seg1r=seg1;
/* lookup if a turn restriction applies */
if(profile->turns && !IsFakeNode(node1) && IsTurnRestrictedNode(LookupNode(nodes,node1,1)))
turnrelation=FindFirstTurnRelation2(relations,node1,seg1r);
/* Loop across all segments */
if(IsFakeNode(node1))
segment=FirstFakeSegment(node1);
else
segment=FirstSegment(segments,nodes,node1,1);
while(segment)
{
Way *way;
index_t node2,seg2,seg2r;
score_t segment_pref,segment_score,cumulative_score;
int i;
node2=OtherNode(segment,node1); /* need this here because we use node2 at the end of the loop */
/* must be a normal segment */
if(!IsNormalSegment(segment))
goto endloop;
/* must obey one-way restrictions (unless profile allows) */
if(profile->oneway && IsOnewayTo(segment,node1))
goto endloop;
if(IsFakeNode(node1) || IsFakeNode(node2))
{
seg2 =IndexFakeSegment(segment);
seg2r=IndexRealSegment(seg2);
}
else
{
seg2 =IndexSegment(segments,segment);
seg2r=seg2;
}
/* must not perform U-turn (unless profile allows) */
if(profile->turns && (seg1==seg2 || seg1==seg2r || seg1r==seg2 || (seg1r==seg2r && IsFakeUTurn(seg1,seg2))))
goto endloop;
/* must obey turn relations */
if(turnrelation!=NO_RELATION && !IsTurnAllowed(relations,turnrelation,node1,seg1r,seg2r,profile->allow))
goto endloop;
/* must not pass over super-node */
if(node2!=finish_node && !IsFakeNode(node2) && IsSuperNode(LookupNode(nodes,node2,2)))
goto endloop;
way=LookupWay(ways,segment->way,1);
/* mode of transport must be allowed on the highway */
if(!(way->allow&profile->allow))
goto endloop;
/* must obey weight restriction (if exists) */
if(way->weight && way->weight<profile->weight)
goto endloop;
/* must obey height/width/length restriction (if exists) */
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->file.props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
/* profile preferences must allow this highway */
if(segment_pref==0)
goto endloop;
/* mode of transport must be allowed through node2 */
if(!IsFakeNode(node2))
{
Node *node=LookupNode(nodes,node2,2);
if(!(node->allow&profile->allow))
goto endloop;
}
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
/* score must be better than current best score */
if(cumulative_score>finish_score)
goto endloop;
result2=FindResult(results,node2,seg2);
if(!result2) /* New end node/segment combination */
{
result2=InsertResult(results,node2,seg2);
result2->prev=result1;
result2->score=cumulative_score;
if(node2==finish_node)
{
if(cumulative_score<finish_score)
{
finish_score=cumulative_score;
finish_result=result2;
}
}
else
{
result2->sortby=result2->score;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New score for end node/segment combination is better */
{
result2->prev=result1;
result2->score=cumulative_score;
result2->segment=seg2;
if(node2==finish_node)
{
if(cumulative_score<finish_score)
{
finish_score=cumulative_score;
finish_result=result2;
}
}
else
{
result2->sortby=result2->score;
if(result2->score<finish_score)
InsertInQueue(queue,result2);
}
}
endloop:
if(IsFakeNode(node1))
segment=NextFakeSegment(segment,node1);
else if(IsFakeNode(node2))
segment=NULL; /* cannot call NextSegment() with a fake segment */
else
{
segment=NextSegment(segments,segment,node1);
if(!segment && IsFakeNode(finish_node))
segment=ExtraFakeSegment(node1,finish_node);
}
}
}
FreeQueueList(queue);
/* Check it worked */
if(!finish_result)
{
FreeResultsList(results);
return(NULL);
}
FixForwardRoute(results,finish_result);
return(results);
}
Results *FindMiddleRoute(Nodes *nodes,Segments *segments,Ways *ways,Relations *relations,Profile *profile,Results *begin,Results *end)
{
Results *results;
Queue *queue;
Result *finish_result;
score_t finish_score;
double finish_lat,finish_lon;
Result *result1,*result2,*result3,*result4;
if(!option_quiet)
printf_first("Routing: Super-Nodes checked = 0");
/* Set up the finish conditions */
finish_score=INF_DISTANCE;
finish_result=NULL;
if(IsFakeNode(end->finish_node))
GetFakeLatLong(end->finish_node,&finish_lat,&finish_lon);
else
GetLatLong(nodes,end->finish_node,&finish_lat,&finish_lon);
/* Create the list of results and insert the first node into the queue */
results=NewResultsList(65536);
results->start_node=begin->start_node;
results->prev_segment=begin->prev_segment;
if(begin->number==1)
{
if(begin->prev_segment==NO_SEGMENT)
results->prev_segment=NO_SEGMENT;
else
{
index_t superseg=FindSuperSegment(nodes,segments,ways,relations,profile,begin->start_node,begin->prev_segment);
results->prev_segment=superseg;
}
}
result1=InsertResult(results,results->start_node,results->prev_segment);
queue=NewQueueList();
/* Insert the finish points of the beginning part of the path into the queue,
translating the segments into super-segments. */
result3=FirstResult(begin);
while(result3)
{
if((results->start_node!=result3->node || results->prev_segment!=result3->segment) &&
!IsFakeNode(result3->node) && IsSuperNode(LookupNode(nodes,result3->node,1)))
{
Result *result5=result1;
index_t superseg=FindSuperSegment(nodes,segments,ways,relations,profile,result3->node,result3->segment);
if(superseg!=result3->segment)
{
result5=InsertResult(results,result3->node,result3->segment);
result5->prev=result1;
}
if(!FindResult(results,result3->node,superseg))
{
result2=InsertResult(results,result3->node,superseg);
result2->prev=result5;
result2->score=result3->score;
result2->sortby=result3->score;
InsertInQueue(queue,result2);
if((result4=FindResult(end,result2->node,result2->segment)))
{
if((result2->score+result4->score)<finish_score)
{
finish_score=result2->score+result4->score;
finish_result=result2;
}
}
}
}
result3=NextResult(begin,result3);
}
if(begin->number==1)
InsertInQueue(queue,result1);
/* Loop across all nodes in the queue */
while((result1=PopFromQueue(queue)))
{
index_t node1,seg1;
Segment *segment;
index_t turnrelation=NO_RELATION;
/* score must be better than current best score */
if(result1->score>finish_score)
continue;
node1=result1->node;
seg1=result1->segment;
/* lookup if a turn restriction applies */
if(profile->turns && IsTurnRestrictedNode(LookupNode(nodes,node1,1))) /* node1 cannot be a fake node (must be a super-node) */
turnrelation=FindFirstTurnRelation2(relations,node1,seg1);
/* Loop across all segments */
segment=FirstSegment(segments,nodes,node1,1); /* node1 cannot be a fake node (must be a super-node) */
while(segment)
{
Way *way;
Node *node;
index_t node2,seg2;
score_t segment_pref,segment_score,cumulative_score;
int i;
/* must be a super segment */
if(!IsSuperSegment(segment))
goto endloop;
/* must obey one-way restrictions (unless profile allows) */
if(profile->oneway && IsOnewayTo(segment,node1))
goto endloop;
node2=OtherNode(segment,node1);
seg2=IndexSegment(segments,segment); /* node2 cannot be a fake node (must be a super-node) */
/* must not perform U-turn */
if(seg1==seg2) /* No fake segments, applies to all profiles */
goto endloop;
/* must obey turn relations */
if(turnrelation!=NO_RELATION && !IsTurnAllowed(relations,turnrelation,node1,seg1,seg2,profile->allow))
goto endloop;
way=LookupWay(ways,segment->way,1);
/* transport must be allowed on the highway */
if(!(way->allow&profile->allow))
goto endloop;
/* must obey weight restriction (if exists) */
if(way->weight && way->weight<profile->weight)
goto endloop;
/* must obey height/width/length restriction (if exists) */
if((way->height && way->height<profile->height) ||
(way->width && way->width <profile->width ) ||
(way->length && way->length<profile->length))
goto endloop;
segment_pref=profile->highway[HIGHWAY(way->type)];
for(i=1;i<Property_Count;i++)
if(ways->file.props & PROPERTIES(i))
{
if(way->props & PROPERTIES(i))
segment_pref*=profile->props_yes[i];
else
segment_pref*=profile->props_no[i];
}
/* profile preferences must allow this highway */
if(segment_pref==0)
goto endloop;
node=LookupNode(nodes,node2,2); /* node2 cannot be a fake node (must be a super-node) */
/* mode of transport must be allowed through node2 */
if(!(node->allow&profile->allow))
goto endloop;
if(option_quickest==0)
segment_score=(score_t)DISTANCE(segment->distance)/segment_pref;
else
segment_score=(score_t)Duration(segment,way,profile)/segment_pref;
cumulative_score=result1->score+segment_score;
/* score must be better than current best score */
if(cumulative_score>finish_score)
goto endloop;
result2=FindResult(results,node2,seg2);
if(!result2) /* New end node/segment pair */
{
result2=InsertResult(results,node2,seg2);
result2->prev=result1;
result2->score=cumulative_score;
if((result3=FindResult(end,node2,seg2)))
{
if((result2->score+result3->score)<finish_score)
{
finish_score=result2->score+result3->score;
finish_result=result2;
}
}
else
{
double lat,lon;
distance_t direct;
GetLatLong(nodes,node2,&lat,&lon); /* node2 cannot be a fake node (must be a super-node) */
direct=Distance(lat,lon,finish_lat,finish_lon);
if(option_quickest==0)
result2->sortby=result2->score+(score_t)direct/profile->max_pref;
else
result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
InsertInQueue(queue,result2);
}
}
else if(cumulative_score<result2->score) /* New end node/segment pair is better */
{
result2->prev=result1;
result2->score=cumulative_score;
if((result3=FindResult(end,node2,seg2)))
{
if((result2->score+result3->score)<finish_score)
{
finish_score=result2->score+result3->score;
finish_result=result2;
}
}
else if(result2->score<finish_score)
{
double lat,lon;
distance_t direct;
GetLatLong(nodes,node2,&lat,&lon); /* node2 cannot be a fake node (must be a super-node) */
direct=Distance(lat,lon,finish_lat,finish_lon);
if(option_quickest==0)
result2->sortby=result2->score+(score_t)direct/profile->max_pref;
else
result2->sortby=result2->score+(score_t)distance_speed_to_duration(direct,profile->max_speed)/profile->max_pref;
InsertInQueue(queue,result2);
}
}
if(!option_quiet && !(results->number%1000))
printf_middle("Routing: Super-Nodes checked = %d",results->number);
endloop:
segment=NextSegment(segments,segment,node1); /* node1 cannot be a fake node (must be a super-node) */
}
}
if(!option_quiet)
printf_last("Routing: Super-Nodes checked = %d",results->number);
FreeQueueList(queue);
/* Check it worked */
if(!finish_result)
{
FreeResultsList(results);
return(NULL);
}
/* Finish off the end part of the route */
if(finish_result->node!=end->finish_node)
{
result3=InsertResult(results,end->finish_node,NO_SEGMENT);
result3->prev=finish_result;
result3->score=finish_score;
finish_result=result3;
}
FixForwardRoute(results,finish_result);
return(results);
}
