Experiment 2Head routing
This commit is contained in:
Victor Shcherb
parent
5ad8716d7a
commit
4d34b696c0
1 changed files with 140 additions and 96 deletions
|
|
@ -170,50 +170,41 @@ public class BinaryRoutePlanner {
|
|||
return ctx.roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
|
||||
}
|
||||
};
|
||||
PriorityQueue<RouteSegment> graphSegments = new PriorityQueue<RouteSegment>(50, segmentsComparator);
|
||||
PriorityQueue<RouteSegment> graphDirectSegments = new PriorityQueue<RouteSegment>(50, segmentsComparator);
|
||||
PriorityQueue<RouteSegment> graphReverseSegments = new PriorityQueue<RouteSegment>(50, segmentsComparator);
|
||||
|
||||
// Set to not visit one segment twice (stores road.id << X + segmentStart)
|
||||
TLongHashSet visitedSegments = new TLongHashSet();
|
||||
TLongHashSet visitedDirectSegments = new TLongHashSet();
|
||||
TLongHashSet visitedOppositeSegments = new TLongHashSet();
|
||||
|
||||
|
||||
if(reverseWaySearch){
|
||||
RouteSegment t = start;
|
||||
start = end;
|
||||
end = t;
|
||||
}
|
||||
int targetEndX = end.road.getPoint31XTile(end.segmentEnd);
|
||||
int targetEndY = end.road.getPoint31YTile(end.segmentEnd);
|
||||
// if(reverseWaySearch){
|
||||
// RouteSegment t = start;
|
||||
// start = end;
|
||||
// end = t;
|
||||
// }
|
||||
|
||||
int targetEndX = end.road.getPoint31XTile(end.segmentStart);
|
||||
int targetEndY = end.road.getPoint31YTile(end.segmentStart);
|
||||
int startX = start.road.getPoint31XTile(start.segmentStart);
|
||||
int startY = start.road.getPoint31YTile(start.segmentStart);
|
||||
// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
|
||||
start.distanceToEnd = squareRootDist(startX, startY, targetEndX, targetEndY) / ctx.router.getMaxDefaultSpeed();
|
||||
|
||||
// add start segment to priority queue
|
||||
graphSegments.add(start);
|
||||
end.distanceToEnd = start.distanceToEnd;
|
||||
|
||||
// because first point of the start is not visited do the same as in cycle but only for one point
|
||||
long ls = (((long) startX) << 31) + (long) startY;
|
||||
loadRoutes(ctx, (startX >> (31 - ZOOM_LOAD_TILES)), (startY >> (31 - ZOOM_LOAD_TILES)));
|
||||
RouteSegment startNbs = ctx.routes.get(ls);
|
||||
while(startNbs != null) { // startNbs.road.id >> 1, start.road.id >> 1
|
||||
if(startNbs.road.getId() != start.road.getId()){
|
||||
startNbs.parentRoute = start;
|
||||
startNbs.parentSegmentEnd = start.segmentStart;
|
||||
startNbs.distanceToEnd = start.distanceToEnd;
|
||||
long nt = (startNbs.road.getId() << 8l) + startNbs.segmentStart;
|
||||
visitedSegments.add(nt);
|
||||
graphSegments.add(startNbs);
|
||||
}
|
||||
startNbs = startNbs.next;
|
||||
}
|
||||
// it matters when start point is intersection of different roads
|
||||
// add start segment to priority queue
|
||||
visitAllStartSegments(ctx, start, graphDirectSegments, visitedDirectSegments, startX, startY);
|
||||
visitAllStartSegments(ctx, end, graphReverseSegments, visitedOppositeSegments, targetEndX, targetEndY);
|
||||
|
||||
// final segment before end
|
||||
RouteSegment finalRoute = null;
|
||||
|
||||
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
|
||||
while(!graphSegments.isEmpty() && finalRoute == null){
|
||||
RouteSegment segment = graphSegments.poll();
|
||||
BinaryMapDataObject road = segment.road;
|
||||
while(!graphDirectSegments.isEmpty() && finalRoute == null){
|
||||
RouteSegment segment = graphDirectSegments.poll();
|
||||
|
||||
|
||||
ctx.visitedSegments ++;
|
||||
// for debug purposes
|
||||
|
|
@ -221,71 +212,8 @@ public class BinaryRoutePlanner {
|
|||
ctx.visitor.visitSegment(segment);
|
||||
}
|
||||
|
||||
// Always start from segmentStart (!), not from segmentEnd
|
||||
// It makes difference only for the first start segment
|
||||
// Middle point will always be skipped from observation considering already visited
|
||||
int middle = segment.segmentStart;
|
||||
int middlex = road.getPoint31XTile(middle);
|
||||
int middley = road.getPoint31YTile(middle);
|
||||
|
||||
boolean oneway = ctx.router.isOneWay(road.getHighwayAttributes());
|
||||
boolean minusAllowed = !oneway || reverseWaySearch;
|
||||
boolean plusAllowed = !oneway || !reverseWaySearch;
|
||||
|
||||
if(end.road.getId() == road.getId() && end.segmentStart == middle){
|
||||
finalRoute = segment;
|
||||
}
|
||||
|
||||
// +/- diff from middle point
|
||||
int d = plusAllowed ? 1 : -1;
|
||||
// Go through all point of the way and find ways to continue
|
||||
while(finalRoute == null && (minusAllowed || plusAllowed)) {
|
||||
// 1. calculate point not equal to middle
|
||||
// (algorithm should visit all point on way if it is not oneway)
|
||||
int j = middle + d;
|
||||
if (!minusAllowed && d > 0) {
|
||||
d++;
|
||||
} else if (!plusAllowed && d < 0) {
|
||||
d--;
|
||||
} else {
|
||||
if (d <= 0){
|
||||
d = -d + 1;
|
||||
} else {
|
||||
d = -d;
|
||||
}
|
||||
}
|
||||
if(j < 0){
|
||||
minusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
if(j >= road.getPointsLength()){
|
||||
plusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
// if we found end point break cycle
|
||||
if(end.road.getId() == road.getId() && end.segmentStart == j){
|
||||
finalRoute = segment;
|
||||
break;
|
||||
}
|
||||
|
||||
// 2. calculate point and try to load neighbor ways if they are not loaded
|
||||
long l = (((long) road.getPoint31XTile(j)) << 31) + (long) road.getPoint31YTile(j);
|
||||
loadRoutes(ctx, (road.getPoint31XTile(j) >> (31 - ZOOM_LOAD_TILES)), (road.getPoint31YTile(j) >> (31 - ZOOM_LOAD_TILES)));
|
||||
long nt = (road.getId() << 8l) + segment.segmentStart;
|
||||
visitedSegments.add(nt);
|
||||
|
||||
// 3. get intersected ways
|
||||
RouteSegment next = ctx.routes.get(l);
|
||||
if (next != null) {
|
||||
int x = road.getPoint31XTile(j);
|
||||
int y = road.getPoint31YTile(j);
|
||||
double distOnRoadToPass = squareRootDist(x, y, middlex, middley);
|
||||
double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY);
|
||||
processIntersectionsWithWays(ctx, graphSegments, visitedSegments, distOnRoadToPass, distToFinalPoint,
|
||||
segment, road, d == 0, j, next);
|
||||
}
|
||||
}
|
||||
finalRoute = processRouteSegment(ctx, end, reverseWaySearch, graphDirectSegments, visitedDirectSegments, targetEndX, targetEndY,
|
||||
segment, visitedOppositeSegments);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -296,6 +224,113 @@ public class BinaryRoutePlanner {
|
|||
|
||||
|
||||
|
||||
private void visitAllStartSegments(final RoutingContext ctx, RouteSegment start, PriorityQueue<RouteSegment> graphDirectSegments,
|
||||
TLongHashSet visitedSegments, int startX, int startY) throws IOException {
|
||||
// mark as visited code seems to be duplicated
|
||||
long nt = (start.road.getId() << 8l) + start.segmentStart;
|
||||
visitedSegments.add(nt);
|
||||
graphDirectSegments.add(start);
|
||||
|
||||
loadRoutes(ctx, (startX >> (31 - ZOOM_LOAD_TILES)), (startY >> (31 - ZOOM_LOAD_TILES)));
|
||||
long ls = (((long) startX) << 31) + (long) startY;
|
||||
RouteSegment startNbs = ctx.routes.get(ls);
|
||||
while(startNbs != null) { // startNbs.road.id >> 1, start.road.id >> 1
|
||||
if(startNbs.road.getId() != start.road.getId()){
|
||||
startNbs.parentRoute = start;
|
||||
startNbs.parentSegmentEnd = start.segmentStart;
|
||||
startNbs.distanceToEnd = start.distanceToEnd;
|
||||
|
||||
// duplicated to be sure start is added
|
||||
nt = (startNbs.road.getId() << 8l) + startNbs.segmentStart;
|
||||
visitedSegments.add(nt);
|
||||
graphDirectSegments.add(startNbs);
|
||||
}
|
||||
startNbs = startNbs.next;
|
||||
}
|
||||
}
|
||||
|
||||
private RouteSegment processRouteSegment(final RoutingContext ctx, RouteSegment end, boolean reverseWaySearch,
|
||||
PriorityQueue<RouteSegment> graphSegments, TLongHashSet visitedSegments, int targetEndX, int targetEndY,
|
||||
RouteSegment segment, TLongHashSet oppositeSegments) throws IOException {
|
||||
// Always start from segmentStart (!), not from segmentEnd
|
||||
// It makes difference only for the first start segment
|
||||
// Middle point will always be skipped from observation considering already visited
|
||||
final BinaryMapDataObject road = segment.road;
|
||||
final int middle = segment.segmentStart;
|
||||
int middlex = road.getPoint31XTile(middle);
|
||||
int middley = road.getPoint31YTile(middle);
|
||||
|
||||
// 0. mark route segment as visited
|
||||
long nt = (road.getId() << 8l) + middle;
|
||||
visitedSegments.add(nt);
|
||||
if(oppositeSegments.contains(nt)){
|
||||
// if(end.road.getId() == road.getId() && end.segmentStart == middle){
|
||||
segment.segmentEnd = middle;
|
||||
return segment;
|
||||
}
|
||||
|
||||
|
||||
boolean oneway = ctx.router.isOneWay(road.getHighwayAttributes());
|
||||
boolean minusAllowed = !oneway || reverseWaySearch;
|
||||
boolean plusAllowed = !oneway || !reverseWaySearch;
|
||||
|
||||
// +/- diff from middle point
|
||||
int d = plusAllowed ? 1 : -1;
|
||||
// Go through all point of the way and find ways to continue
|
||||
// ! Actually there is small bug when there is restriction to move forward on way (it doesn't take into account)
|
||||
while(minusAllowed || plusAllowed) {
|
||||
// 1. calculate point not equal to middle
|
||||
// (algorithm should visit all point on way if it is not oneway)
|
||||
int segmentEnd = middle + d;
|
||||
if (!minusAllowed && d > 0) {
|
||||
d++;
|
||||
} else if (!plusAllowed && d < 0) {
|
||||
d--;
|
||||
} else {
|
||||
if (d <= 0){
|
||||
d = -d + 1;
|
||||
} else {
|
||||
d = -d;
|
||||
}
|
||||
}
|
||||
if(segmentEnd < 0){
|
||||
minusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
if(segmentEnd >= road.getPointsLength()){
|
||||
plusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
// if we found end point break cycle
|
||||
long nts = (road.getId() << 8l) + segmentEnd;
|
||||
if(oppositeSegments.contains(nts)){
|
||||
// if(end.road.getId() == road.getId() && end.segmentStart == segmentEnd){
|
||||
segment.segmentEnd = segmentEnd;
|
||||
return segment;
|
||||
}
|
||||
visitedSegments.add(nts);
|
||||
|
||||
// 2. calculate point and try to load neighbor ways if they are not loaded
|
||||
int x = road.getPoint31XTile(segmentEnd);
|
||||
int y = road.getPoint31YTile(segmentEnd);
|
||||
loadRoutes(ctx, (x >> (31 - ZOOM_LOAD_TILES)), (y >> (31 - ZOOM_LOAD_TILES)));
|
||||
long l = (((long) x) << 31) + (long) y;
|
||||
RouteSegment next = ctx.routes.get(l);
|
||||
|
||||
// 3. get intersected ways
|
||||
if (next != null) {
|
||||
double distOnRoadToPass = squareRootDist(x, y, middlex, middley);
|
||||
double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY);
|
||||
processIntersectionsWithWays(ctx, graphSegments, visitedSegments, distOnRoadToPass, distToFinalPoint,
|
||||
segment, road, d == 0, segmentEnd, next);
|
||||
}
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
|
||||
|
||||
private void processIntersectionsWithWays(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
|
||||
TLongHashSet visitedSegments, double distOnRoadToPass, double distToFinalPoint,
|
||||
RouteSegment segment, BinaryMapDataObject road, boolean firstOfSegment, int segmentEnd, RouteSegment next) {
|
||||
|
|
@ -393,9 +428,18 @@ public class BinaryRoutePlanner {
|
|||
RouteSegment finalRoute, boolean reverseResult) {
|
||||
List<RouteSegmentResult> result = new ArrayList<RouteSegmentResult>();
|
||||
// Try to define direction of last movement and reverse start and end point for end if needed
|
||||
int parentSegmentEnd = finalRoute != null && finalRoute.segmentStart <= end.segmentStart ?
|
||||
end.segmentEnd : end.segmentStart;
|
||||
int parentSegmentEnd = 0;
|
||||
RouteSegment segment = finalRoute;
|
||||
if(finalRoute != null){
|
||||
if(finalRoute.getRoad().getId() == end.getRoad().getId()){
|
||||
parentSegmentEnd = finalRoute.segmentStart <= end.segmentStart ? end.segmentEnd : end.segmentStart;
|
||||
} else {
|
||||
end.parentRoute = finalRoute;
|
||||
end.parentSegmentEnd = finalRoute.segmentEnd;
|
||||
parentSegmentEnd = end.segmentEnd;
|
||||
segment = end;
|
||||
}
|
||||
}
|
||||
|
||||
if (PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST) {
|
||||
System.out.println("ROUTE : ");
|
||||
|
|
|
|||
Loading…
Reference in a new issue