Formatting
This commit is contained in:
Roman Inflianskas
parent
28956f8ad5
commit
e5fe0d9eb7
1 changed files with 167 additions and 165 deletions
|
|
@ -18,49 +18,53 @@ import net.osmand.util.MapUtils;
|
|||
import org.apache.commons.logging.Log;
|
||||
|
||||
public class BinaryRoutePlanner {
|
||||
|
||||
|
||||
private static final int TEST_ID = 31370645;
|
||||
private static final boolean TEST_SPECIFIC = false;
|
||||
|
||||
|
||||
private static final int REVERSE_WAY_RESTRICTION_ONLY = 1024;
|
||||
/*private*/ static final int STANDARD_ROAD_IN_QUEUE_OVERHEAD = 220;
|
||||
/*private*/ static final int STANDARD_ROAD_VISITED_OVERHEAD = 150;
|
||||
|
||||
|
||||
protected static final Log log = PlatformUtil.getLog(BinaryRoutePlanner.class);
|
||||
|
||||
|
||||
private static final int ROUTE_POINTS = 11;
|
||||
private static final boolean TRACE_ROUTING = false;
|
||||
|
||||
|
||||
|
||||
|
||||
public static double squareRootDist(int x1, int y1, int x2, int y2) {
|
||||
// translate into meters
|
||||
|
||||
|
||||
double dy = MapUtils.convert31YToMeters(y1, y2);
|
||||
double dx = MapUtils.convert31XToMeters(x1, x2);
|
||||
return Math.sqrt(dx * dx + dy * dy);
|
||||
// return measuredDist(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private static class SegmentsComparator implements Comparator<RouteSegment> {
|
||||
final RoutingContext ctx;
|
||||
|
||||
public SegmentsComparator(RoutingContext ctx) {
|
||||
this.ctx = ctx;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int compare(RouteSegment o1, RouteSegment o2) {
|
||||
return ctx.roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static class NonHeuristicSegmentsComparator implements Comparator<RouteSegment> {
|
||||
public NonHeuristicSegmentsComparator() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public int compare(RouteSegment o1, RouteSegment o2) {
|
||||
return roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd, 0.5);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm)
|
||||
* return list of segments
|
||||
|
|
@ -71,48 +75,48 @@ public class BinaryRoutePlanner {
|
|||
// measure time
|
||||
ctx.timeToLoad = 0;
|
||||
ctx.visitedSegments = 0;
|
||||
ctx.memoryOverhead = 1000;
|
||||
ctx.memoryOverhead = 1000;
|
||||
ctx.timeToCalculate = System.nanoTime();
|
||||
|
||||
|
||||
|
||||
// Initializing priority queue to visit way segments
|
||||
Comparator<RouteSegment> nonHeuristicSegmentsComparator = new NonHeuristicSegmentsComparator();
|
||||
PriorityQueue<RouteSegment> graphDirectSegments = new PriorityQueue<RouteSegment>(50, new SegmentsComparator(ctx));
|
||||
PriorityQueue<RouteSegment> graphReverseSegments = new PriorityQueue<RouteSegment>(50, new SegmentsComparator(ctx));
|
||||
|
||||
|
||||
// Set to not visit one segment twice (stores road.id << X + segmentStart)
|
||||
TLongObjectHashMap<RouteSegment> visitedDirectSegments = new TLongObjectHashMap<RouteSegment>();
|
||||
TLongObjectHashMap<RouteSegment> visitedOppositeSegments = new TLongObjectHashMap<RouteSegment>();
|
||||
|
||||
|
||||
initQueuesWithStartEnd(ctx, start, end, recalculationEnd, graphDirectSegments, graphReverseSegments);
|
||||
|
||||
|
||||
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
|
||||
boolean forwardSearch = true;
|
||||
|
||||
PriorityQueue<RouteSegment> graphSegments = graphDirectSegments;
|
||||
|
||||
|
||||
PriorityQueue<RouteSegment> graphSegments = graphDirectSegments;
|
||||
|
||||
FinalRouteSegment finalSegment = null;
|
||||
boolean onlyBackward = ctx.getPlanRoadDirection() < 0;
|
||||
boolean onlyForward = ctx.getPlanRoadDirection() > 0 ;
|
||||
boolean onlyForward = ctx.getPlanRoadDirection() > 0;
|
||||
while (!graphSegments.isEmpty()) {
|
||||
RouteSegment segment = graphSegments.poll();
|
||||
// use accumulative approach
|
||||
ctx.memoryOverhead = (visitedDirectSegments.size() + visitedOppositeSegments.size()) * STANDARD_ROAD_VISITED_OVERHEAD +
|
||||
ctx.memoryOverhead = (visitedDirectSegments.size() + visitedOppositeSegments.size()) * STANDARD_ROAD_VISITED_OVERHEAD +
|
||||
(graphDirectSegments.size() +
|
||||
graphReverseSegments.size()) * STANDARD_ROAD_IN_QUEUE_OVERHEAD;
|
||||
|
||||
if(TRACE_ROUTING){
|
||||
if (TRACE_ROUTING) {
|
||||
printRoad(">", segment, !forwardSearch);
|
||||
}
|
||||
// if(segment.getParentRoute() != null)
|
||||
// System.out.println(segment.getRoad().getId() + " - " + segment.getParentRoute().getRoad().getId());
|
||||
if(segment instanceof FinalRouteSegment) {
|
||||
if(RoutingContext.SHOW_GC_SIZE){
|
||||
log.warn("Estimated overhead " + (ctx.memoryOverhead / (1<<20))+ " mb");
|
||||
if (segment instanceof FinalRouteSegment) {
|
||||
if (RoutingContext.SHOW_GC_SIZE) {
|
||||
log.warn("Estimated overhead " + (ctx.memoryOverhead / (1 << 20)) + " mb");
|
||||
printMemoryConsumption("Memory occupied after calculation : ");
|
||||
}
|
||||
finalSegment = (FinalRouteSegment) segment;
|
||||
if(TRACE_ROUTING){
|
||||
if (TRACE_ROUTING) {
|
||||
println("Final segment found");
|
||||
}
|
||||
break;
|
||||
|
|
@ -120,13 +124,13 @@ public class BinaryRoutePlanner {
|
|||
if (ctx.memoryOverhead > ctx.config.memoryLimitation * 0.95 && RoutingContext.SHOW_GC_SIZE) {
|
||||
printMemoryConsumption("Memory occupied before exception : ");
|
||||
}
|
||||
if(ctx.memoryOverhead > ctx.config.memoryLimitation * 0.95) {
|
||||
throw new IllegalStateException("There is no enough memory " + ctx.config.memoryLimitation/(1<<20) + " Mb");
|
||||
if (ctx.memoryOverhead > ctx.config.memoryLimitation * 0.95) {
|
||||
throw new IllegalStateException("There is no enough memory " + ctx.config.memoryLimitation / (1 << 20) + " Mb");
|
||||
}
|
||||
ctx.visitedSegments++;
|
||||
if (forwardSearch) {
|
||||
boolean doNotAddIntersections = onlyBackward;
|
||||
processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments,
|
||||
processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments,
|
||||
segment, visitedOppositeSegments, doNotAddIntersections);
|
||||
} else {
|
||||
boolean doNotAddIntersections = onlyForward;
|
||||
|
|
@ -134,7 +138,7 @@ public class BinaryRoutePlanner {
|
|||
visitedDirectSegments, doNotAddIntersections);
|
||||
}
|
||||
updateCalculationProgress(ctx, graphDirectSegments, graphReverseSegments);
|
||||
|
||||
|
||||
checkIfGraphIsEmpty(ctx, ctx.getPlanRoadDirection() <= 0, graphReverseSegments, end, visitedOppositeSegments,
|
||||
"Route is not found to selected target point.");
|
||||
checkIfGraphIsEmpty(ctx, ctx.getPlanRoadDirection() >= 0, graphDirectSegments, start, visitedDirectSegments,
|
||||
|
|
@ -149,21 +153,21 @@ public class BinaryRoutePlanner {
|
|||
} else {
|
||||
// different strategy : use onedirectional graph
|
||||
forwardSearch = onlyForward;
|
||||
if(onlyBackward && !graphDirectSegments.isEmpty()) {
|
||||
if (onlyBackward && !graphDirectSegments.isEmpty()) {
|
||||
forwardSearch = true;
|
||||
}
|
||||
if(onlyForward && !graphReverseSegments.isEmpty()) {
|
||||
if (onlyForward && !graphReverseSegments.isEmpty()) {
|
||||
forwardSearch = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (forwardSearch) {
|
||||
graphSegments = graphDirectSegments;
|
||||
} else {
|
||||
graphSegments = graphReverseSegments;
|
||||
}
|
||||
// check if interrupted
|
||||
if(ctx.calculationProgress != null && ctx.calculationProgress.isCancelled) {
|
||||
if (ctx.calculationProgress != null && ctx.calculationProgress.isCancelled) {
|
||||
throw new InterruptedException("Route calculation interrupted");
|
||||
}
|
||||
}
|
||||
|
|
@ -214,14 +218,14 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public RouteSegment initRouteSegment(final RoutingContext ctx, RouteSegment segment, boolean positiveDirection) {
|
||||
if(segment.getSegmentStart() == 0 && !positiveDirection && segment.getRoad().getPointsLength() > 0) {
|
||||
if (segment.getSegmentStart() == 0 && !positiveDirection && segment.getRoad().getPointsLength() > 0) {
|
||||
segment = loadSameSegment(ctx, segment, 1);
|
||||
} else if(segment.getSegmentStart() == segment.getRoad().getPointsLength() -1 && positiveDirection && segment.getSegmentStart() > 0) {
|
||||
segment = loadSameSegment(ctx, segment, segment.getSegmentStart() -1);
|
||||
} else if (segment.getSegmentStart() == segment.getRoad().getPointsLength() - 1 && positiveDirection && segment.getSegmentStart() > 0) {
|
||||
segment = loadSameSegment(ctx, segment, segment.getSegmentStart() - 1);
|
||||
}
|
||||
if(segment == null) {
|
||||
if (segment == null) {
|
||||
return null;
|
||||
}
|
||||
return segment.initRouteSegment(positiveDirection);
|
||||
|
|
@ -232,8 +236,8 @@ public class BinaryRoutePlanner {
|
|||
int x31 = segment.getRoad().getPoint31XTile(ind);
|
||||
int y31 = segment.getRoad().getPoint31YTile(ind);
|
||||
RouteSegment s = ctx.loadRouteSegment(x31, y31, 0);
|
||||
while(s != null) {
|
||||
if(s.getRoad().getId() == segment.getRoad().getId()) {
|
||||
while (s != null) {
|
||||
if (s.getRoad().getId() == segment.getRoad().getId()) {
|
||||
segment = s;
|
||||
break;
|
||||
}
|
||||
|
|
@ -250,34 +254,34 @@ public class BinaryRoutePlanner {
|
|||
RouteSegment endPos = initRouteSegment(ctx, end, true);
|
||||
RouteSegment endNeg = initRouteSegment(ctx, end, false);
|
||||
// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
|
||||
if(ctx.config.initialDirection != null) {
|
||||
if (ctx.config.initialDirection != null) {
|
||||
// mark here as positive for further check
|
||||
double plusDir = start.getRoad().directionRoute(start.getSegmentStart(), true);
|
||||
double diff = plusDir - ctx.config.initialDirection;
|
||||
if(Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
|
||||
if(startNeg != null) {
|
||||
double diff = plusDir - ctx.config.initialDirection;
|
||||
if (Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
|
||||
if (startNeg != null) {
|
||||
startNeg.distanceFromStart += 500;
|
||||
}
|
||||
} else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
|
||||
if(startPos != null) {
|
||||
} else if (Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
|
||||
if (startPos != null) {
|
||||
startPos.distanceFromStart += 500;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(recalculationEnd != null) {
|
||||
if (recalculationEnd != null) {
|
||||
ctx.targetX = recalculationEnd.getRoad().getPoint31XTile(recalculationEnd.getSegmentStart());
|
||||
ctx.targetY = recalculationEnd.getRoad().getPoint31YTile(recalculationEnd.getSegmentStart());
|
||||
}
|
||||
float estimatedDistance = (float) estimatedDistance(ctx, ctx.targetX, ctx.targetY, ctx.startX, ctx.startY);
|
||||
if(startPos != null) {
|
||||
if (startPos != null) {
|
||||
startPos.distanceToEnd = estimatedDistance;
|
||||
graphDirectSegments.add(startPos);
|
||||
}
|
||||
if(startNeg != null) {
|
||||
if (startNeg != null) {
|
||||
startNeg.distanceToEnd = estimatedDistance;
|
||||
graphDirectSegments.add(startNeg);
|
||||
}
|
||||
if(recalculationEnd != null) {
|
||||
if (recalculationEnd != null) {
|
||||
graphReverseSegments.add(recalculationEnd);
|
||||
} else {
|
||||
if (endPos != null) {
|
||||
|
|
@ -292,25 +296,25 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
|
||||
|
||||
private void printMemoryConsumption( String string) {
|
||||
private void printMemoryConsumption(String string) {
|
||||
long h1 = RoutingContext.runGCUsedMemory();
|
||||
float mb = (1 << 20);
|
||||
log.warn(string + h1 / mb);
|
||||
log.warn(string + h1 / mb);
|
||||
}
|
||||
|
||||
|
||||
private void updateCalculationProgress(final RoutingContext ctx, PriorityQueue<RouteSegment> graphDirectSegments,
|
||||
PriorityQueue<RouteSegment> graphReverseSegments) {
|
||||
if(ctx.calculationProgress != null) {
|
||||
if (ctx.calculationProgress != null) {
|
||||
ctx.calculationProgress.reverseSegmentQueueSize = graphReverseSegments.size();
|
||||
ctx.calculationProgress.directSegmentQueueSize = graphDirectSegments.size();
|
||||
if(graphDirectSegments.size() > 0 && ctx.getPlanRoadDirection() >= 0) {
|
||||
if (graphDirectSegments.size() > 0 && ctx.getPlanRoadDirection() >= 0) {
|
||||
RouteSegment peek = graphDirectSegments.peek();
|
||||
ctx.calculationProgress.distanceFromBegin = Math.max(peek.distanceFromStart,
|
||||
ctx.calculationProgress.distanceFromBegin = Math.max(peek.distanceFromStart,
|
||||
ctx.calculationProgress.distanceFromBegin);
|
||||
ctx.calculationProgress.directDistance = peek.distanceFromStart + peek.distanceToEnd;
|
||||
}
|
||||
if(graphReverseSegments.size() > 0 && ctx.getPlanRoadDirection() <= 0) {
|
||||
if (graphReverseSegments.size() > 0 && ctx.getPlanRoadDirection() <= 0) {
|
||||
RouteSegment peek = graphReverseSegments.peek();
|
||||
ctx.calculationProgress.distanceFromEnd = Math.max(peek.distanceFromStart + peek.distanceToEnd,
|
||||
ctx.calculationProgress.distanceFromEnd);
|
||||
|
|
@ -322,8 +326,8 @@ public class BinaryRoutePlanner {
|
|||
|
||||
private void printRoad(String prefix, RouteSegment segment, Boolean reverseWaySearch) {
|
||||
String pr;
|
||||
if(segment.parentRoute != null){
|
||||
pr = " pend="+segment.parentSegmentEnd +" parent=" + segment.parentRoute.road;
|
||||
if (segment.parentRoute != null) {
|
||||
pr = " pend=" + segment.parentSegmentEnd + " parent=" + segment.parentRoute.road;
|
||||
} else {
|
||||
pr = "";
|
||||
}
|
||||
|
|
@ -331,8 +335,8 @@ public class BinaryRoutePlanner {
|
|||
if (reverseWaySearch != null) {
|
||||
p = (reverseWaySearch ? "B" : "F");
|
||||
}
|
||||
println(p+prefix +"" + segment.road + " dir="+segment.getDirectionAssigned()+" ind=" + segment.getSegmentStart() +
|
||||
" ds=" + ((float)segment.distanceFromStart) + " es="+((float)segment.distanceToEnd) + pr);
|
||||
println(p + prefix + "" + segment.road + " dir=" + segment.getDirectionAssigned() + " ind=" + segment.getSegmentStart() +
|
||||
" ds=" + ((float) segment.distanceFromStart) + " es=" + ((float) segment.distanceToEnd) + pr);
|
||||
}
|
||||
|
||||
private float estimatedDistance(final RoutingContext ctx, int targetEndX, int targetEndY,
|
||||
|
|
@ -340,26 +344,25 @@ public class BinaryRoutePlanner {
|
|||
double distance = squareRootDist(startX, startY, targetEndX, targetEndY);
|
||||
return (float) (distance / ctx.getRouter().getMaxDefaultSpeed());
|
||||
}
|
||||
|
||||
|
||||
protected static float h(RoutingContext ctx, int begX, int begY, int endX, int endY) {
|
||||
double distToFinalPoint = squareRootDist(begX, begY, endX, endY);
|
||||
double distToFinalPoint = squareRootDist(begX, begY, endX, endY);
|
||||
double result = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
|
||||
if(ctx.precalculatedRouteDirection != null){
|
||||
float te = ctx.precalculatedRouteDirection.timeEstimate(begX, begY, endX, endY);
|
||||
if(te > 0) {
|
||||
if (ctx.precalculatedRouteDirection != null) {
|
||||
float te = ctx.precalculatedRouteDirection.timeEstimate(begX, begY, endX, endY);
|
||||
if (te > 0) {
|
||||
return te;
|
||||
}
|
||||
}
|
||||
return (float) result;
|
||||
return (float) result;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
private static void println(String logMsg) {
|
||||
// log.info(logMsg);
|
||||
System.out.println(logMsg);
|
||||
}
|
||||
|
||||
|
||||
private static void printInfo(String logMsg) {
|
||||
log.warn(logMsg);
|
||||
}
|
||||
|
|
@ -371,9 +374,9 @@ public class BinaryRoutePlanner {
|
|||
", time to calc dev : " + ctx.timeNanoToCalcDeviation/ 1e6);
|
||||
int maxLoadedTiles = Math.max(ctx.maxLoadedTiles, ctx.getCurrentlyLoadedTiles());
|
||||
printInfo("Current loaded tiles : " + ctx.getCurrentlyLoadedTiles() + ", maximum loaded tiles " + maxLoadedTiles);
|
||||
printInfo("Loaded tiles " + ctx.loadedTiles + " (distinct "+ctx.distinctLoadedTiles+ "), unloaded tiles " + ctx.unloadedTiles +
|
||||
printInfo("Loaded tiles " + ctx.loadedTiles + " (distinct " + ctx.distinctLoadedTiles + "), unloaded tiles " + ctx.unloadedTiles +
|
||||
", loaded more than once same tiles "
|
||||
+ ctx.loadedPrevUnloadedTiles );
|
||||
+ ctx.loadedPrevUnloadedTiles);
|
||||
printInfo("Visited roads " + ctx.visitedSegments + ", relaxed roads " + ctx.relaxedSegments);
|
||||
if (graphDirectSegments != null && graphReverseSegments != null) {
|
||||
printInfo("Priority queues sizes : " + graphDirectSegments.size() + "/" + graphReverseSegments.size());
|
||||
|
|
@ -381,22 +384,22 @@ public class BinaryRoutePlanner {
|
|||
if (visitedDirectSegments != null && visitedOppositeSegments != null) {
|
||||
printInfo("Visited interval sizes: " + visitedDirectSegments.size() + "/" + visitedOppositeSegments.size());
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@SuppressWarnings("unused")
|
||||
private void processRouteSegment(final RoutingContext ctx, boolean reverseWaySearch,
|
||||
PriorityQueue<RouteSegment> graphSegments, TLongObjectHashMap<RouteSegment> visitedSegments,
|
||||
RouteSegment segment, TLongObjectHashMap<RouteSegment> oppositeSegments, boolean doNotAddIntersections) throws IOException {
|
||||
final RouteDataObject road = segment.road;
|
||||
boolean initDirectionAllowed = checkIfInitialMovementAllowedOnSegment(ctx, reverseWaySearch, visitedSegments, segment, road);
|
||||
if(TEST_SPECIFIC && road.getId() == TEST_ID ) {
|
||||
if (TEST_SPECIFIC && road.getId() == TEST_ID) {
|
||||
printRoad(" ! " + +segment.distanceFromStart + " ", segment, reverseWaySearch);
|
||||
}
|
||||
boolean directionAllowed = initDirectionAllowed;
|
||||
if(!directionAllowed) {
|
||||
if(TRACE_ROUTING) {
|
||||
if (!directionAllowed) {
|
||||
if (TRACE_ROUTING) {
|
||||
println(" >> Already visited");
|
||||
}
|
||||
return;
|
||||
|
|
@ -413,29 +416,29 @@ public class BinaryRoutePlanner {
|
|||
while (directionAllowed) {
|
||||
// mark previous interval as visited and move to next intersection
|
||||
short prevInd = segmentPoint;
|
||||
if(dir) {
|
||||
segmentPoint ++;
|
||||
if (dir) {
|
||||
segmentPoint++;
|
||||
} else {
|
||||
segmentPoint --;
|
||||
segmentPoint--;
|
||||
}
|
||||
if (segmentPoint < 0 || segmentPoint >= road.getPointsLength()) {
|
||||
directionAllowed = false;
|
||||
continue;
|
||||
}
|
||||
// store <segment> in order to not have unique <segment, direction> in visitedSegments
|
||||
visitedSegments.put(calculateRoutePointId(segment.getRoad(), segment.isPositive() ? segmentPoint - 1 : segmentPoint,
|
||||
visitedSegments.put(calculateRoutePointId(segment.getRoad(), segment.isPositive() ? segmentPoint - 1 : segmentPoint,
|
||||
segment.isPositive()), previous != null ? previous : segment);
|
||||
final int x = road.getPoint31XTile(segmentPoint);
|
||||
final int y = road.getPoint31YTile(segmentPoint);
|
||||
final int prevx = road.getPoint31XTile(prevInd);
|
||||
final int prevy = road.getPoint31YTile(prevInd);
|
||||
if(x == prevx && y == prevy) {
|
||||
if (x == prevx && y == prevy) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
// 2. calculate point and try to load neighbor ways if they are not loaded
|
||||
segmentDist += squareRootDist(x, y, prevx, prevy);
|
||||
|
||||
segmentDist += squareRootDist(x, y, prevx, prevy);
|
||||
|
||||
// 2.1 calculate possible obstacle plus time
|
||||
double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentPoint);
|
||||
if (obstacle < 0) {
|
||||
|
|
@ -443,15 +446,15 @@ public class BinaryRoutePlanner {
|
|||
continue;
|
||||
}
|
||||
obstaclesTime += obstacle;
|
||||
|
||||
boolean alreadyVisited = checkIfOppositieSegmentWasVisited(ctx, reverseWaySearch, graphSegments, segment, oppositeSegments,
|
||||
|
||||
boolean alreadyVisited = checkIfOppositieSegmentWasVisited(ctx, reverseWaySearch, graphSegments, segment, oppositeSegments,
|
||||
segmentPoint, segmentDist, obstaclesTime);
|
||||
if (alreadyVisited) {
|
||||
directionAllowed = false;
|
||||
continue;
|
||||
}
|
||||
// correct way of handling precalculatedRouteDirection
|
||||
if(ctx.precalculatedRouteDirection != null) {
|
||||
if (ctx.precalculatedRouteDirection != null) {
|
||||
// long nt = System.nanoTime();
|
||||
// float devDistance = ctx.precalculatedRouteDirection.getDeviationDistance(x, y);
|
||||
// // 1. linear method
|
||||
|
|
@ -465,14 +468,14 @@ public class BinaryRoutePlanner {
|
|||
// could be expensive calculation
|
||||
// 3. get intersected ways
|
||||
final RouteSegment roadNext = ctx.loadRouteSegment(x, y, ctx.config.memoryLimitation - ctx.memoryOverhead);
|
||||
float distStartObstacles = segment.distanceFromStart + calculateTimeWithObstacles(ctx, road, segmentDist , obstaclesTime);
|
||||
if(ctx.precalculatedRouteDirection != null && ctx.precalculatedRouteDirection.isFollowNext()) {
|
||||
float distStartObstacles = segment.distanceFromStart + calculateTimeWithObstacles(ctx, road, segmentDist, obstaclesTime);
|
||||
if (ctx.precalculatedRouteDirection != null && ctx.precalculatedRouteDirection.isFollowNext()) {
|
||||
// reset to f
|
||||
// distStartObstacles = 0;
|
||||
// more precise but slower
|
||||
distStartObstacles = ctx.precalculatedRouteDirection.getDeviationDistance(x, y) / ctx.getRouter().getMaxDefaultSpeed();
|
||||
}
|
||||
|
||||
|
||||
// We don't check if there are outgoing connections
|
||||
previous = processIntersections(ctx, graphSegments, visitedSegments, distStartObstacles,
|
||||
segment, segmentPoint, roadNext, reverseWaySearch, doNotAddIntersections, processFurther);
|
||||
|
|
@ -481,7 +484,7 @@ public class BinaryRoutePlanner {
|
|||
continue;
|
||||
}
|
||||
}
|
||||
if(initDirectionAllowed && ctx.visitor != null){
|
||||
if (initDirectionAllowed && ctx.visitor != null) {
|
||||
ctx.visitor.visitSegment(segment, segmentPoint, true);
|
||||
}
|
||||
}
|
||||
|
|
@ -492,35 +495,35 @@ public class BinaryRoutePlanner {
|
|||
int oneway = ctx.getRouter().isOneWay(road);
|
||||
// use positive direction as agreed
|
||||
if (!reverseWaySearch) {
|
||||
if(segment.isPositive()){
|
||||
if (segment.isPositive()) {
|
||||
directionAllowed = oneway >= 0;
|
||||
} else {
|
||||
directionAllowed = oneway <= 0;
|
||||
}
|
||||
} else {
|
||||
if(segment.isPositive()){
|
||||
if (segment.isPositive()) {
|
||||
directionAllowed = oneway <= 0;
|
||||
} else {
|
||||
directionAllowed = oneway >= 0;
|
||||
}
|
||||
}
|
||||
|
||||
if(directionAllowed && visitedSegments.containsKey(calculateRoutePointId(segment, segment.isPositive()))) {
|
||||
|
||||
if (directionAllowed && visitedSegments.containsKey(calculateRoutePointId(segment, segment.isPositive()))) {
|
||||
directionAllowed = false;
|
||||
}
|
||||
return directionAllowed;
|
||||
}
|
||||
|
||||
|
||||
private boolean checkViaRestrictions(RouteSegment from, RouteSegment to) {
|
||||
if(from != null && to != null) {
|
||||
if (from != null && to != null) {
|
||||
long fid = to.getRoad().getId();
|
||||
for(int i = 0; i < from.getRoad().getRestrictionLength(); i++) {
|
||||
for (int i = 0; i < from.getRoad().getRestrictionLength(); i++) {
|
||||
long id = from.getRoad().getRestrictionId(i);
|
||||
if(fid == id) {
|
||||
if (fid == id) {
|
||||
int tp = from.getRoad().getRestrictionType(i);
|
||||
if(tp == MapRenderingTypes.RESTRICTION_NO_LEFT_TURN ||
|
||||
tp == MapRenderingTypes.RESTRICTION_NO_RIGHT_TURN ||
|
||||
tp == MapRenderingTypes.RESTRICTION_NO_STRAIGHT_ON ||
|
||||
if (tp == MapRenderingTypes.RESTRICTION_NO_LEFT_TURN ||
|
||||
tp == MapRenderingTypes.RESTRICTION_NO_RIGHT_TURN ||
|
||||
tp == MapRenderingTypes.RESTRICTION_NO_STRAIGHT_ON ||
|
||||
tp == MapRenderingTypes.RESTRICTION_NO_U_TURN) {
|
||||
return false;
|
||||
}
|
||||
|
|
@ -530,9 +533,9 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
private RouteSegment getParentDiffId(RouteSegment s) {
|
||||
while(s.getParentRoute() != null && s.getParentRoute().getRoad().getId() == s.getRoad().getId()) {
|
||||
while (s.getParentRoute() != null && s.getParentRoute().getRoad().getId() == s.getRoad().getId()) {
|
||||
s = s.getParentRoute();
|
||||
}
|
||||
return s.getParentRoute();
|
||||
|
|
@ -566,7 +569,7 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
private float calculateTimeWithObstacles(RoutingContext ctx, RouteDataObject road, float distOnRoadToPass, float obstaclesTime) {
|
||||
float priority = ctx.getRouter().defineSpeedPriority(road);
|
||||
|
|
@ -575,26 +578,26 @@ public class BinaryRoutePlanner {
|
|||
speed = (ctx.getRouter().getMinDefaultSpeed() * priority);
|
||||
}
|
||||
// speed can not exceed max default speed according to A*
|
||||
if(speed > ctx.getRouter().getMaxDefaultSpeed()) {
|
||||
if (speed > ctx.getRouter().getMaxDefaultSpeed()) {
|
||||
speed = ctx.getRouter().getMaxDefaultSpeed();
|
||||
}
|
||||
return obstaclesTime + distOnRoadToPass / speed;
|
||||
}
|
||||
|
||||
private long calculateRoutePointId(final RouteDataObject road, int intervalId, boolean positive) {
|
||||
if(intervalId < 0) {
|
||||
if (intervalId < 0) {
|
||||
// should be assert
|
||||
throw new IllegalStateException("Assert failed");
|
||||
}
|
||||
return (road.getId() << ROUTE_POINTS) + (intervalId << 1) + (positive ? 1 : 0);
|
||||
}
|
||||
|
||||
|
||||
private long calculateRoutePointId(RouteSegment segm, boolean direction) {
|
||||
if(segm.getSegmentStart() == 0 && !direction) {
|
||||
throw new IllegalStateException("Assert failed");
|
||||
if (segm.getSegmentStart() == 0 && !direction) {
|
||||
throw new IllegalStateException("Assert failed");
|
||||
}
|
||||
if(segm.getSegmentStart() == segm.getRoad().getPointsLength() - 1 && direction) {
|
||||
throw new IllegalStateException("Assert failed");
|
||||
if (segm.getSegmentStart() == segm.getRoad().getPointsLength() - 1 && direction) {
|
||||
throw new IllegalStateException("Assert failed");
|
||||
}
|
||||
return calculateRoutePointId(segm.getRoad(),
|
||||
direction ? segm.getSegmentStart() : segm.getSegmentStart() - 1, direction);
|
||||
|
|
@ -602,19 +605,19 @@ public class BinaryRoutePlanner {
|
|||
|
||||
|
||||
private boolean proccessRestrictions(RoutingContext ctx, RouteSegment segment, RouteSegment inputNext, boolean reverseWay) {
|
||||
if(!ctx.getRouter().restrictionsAware()) {
|
||||
if (!ctx.getRouter().restrictionsAware()) {
|
||||
return false;
|
||||
}
|
||||
RouteDataObject road = segment.getRoad();
|
||||
RouteSegment parent = getParentDiffId(segment);
|
||||
if (!reverseWay && road.getRestrictionLength() == 0 &&
|
||||
if (!reverseWay && road.getRestrictionLength() == 0 &&
|
||||
(parent == null || parent.getRoad().getRestrictionLength() == 0)) {
|
||||
return false;
|
||||
}
|
||||
ctx.segmentsToVisitPrescripted.clear();
|
||||
ctx.segmentsToVisitNotForbidden.clear();
|
||||
processRestriction(ctx, inputNext, reverseWay, false, road);
|
||||
if(parent != null) {
|
||||
if (parent != null) {
|
||||
processRestriction(ctx, inputNext, reverseWay, true, parent.getRoad());
|
||||
}
|
||||
return true;
|
||||
|
|
@ -667,7 +670,7 @@ public class BinaryRoutePlanner {
|
|||
} else if (type == MapRenderingTypes.RESTRICTION_NO_LEFT_TURN || type == MapRenderingTypes.RESTRICTION_NO_RIGHT_TURN
|
||||
|| type == MapRenderingTypes.RESTRICTION_NO_STRAIGHT_ON || type == MapRenderingTypes.RESTRICTION_NO_U_TURN) {
|
||||
// next = next.next; continue;
|
||||
if(via) {
|
||||
if (via) {
|
||||
ctx.segmentsToVisitPrescripted.remove(next);
|
||||
}
|
||||
} else if (type == -1) {
|
||||
|
|
@ -690,23 +693,21 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
next = next.next;
|
||||
}
|
||||
if(!via) {
|
||||
if (!via) {
|
||||
ctx.segmentsToVisitPrescripted.addAll(ctx.segmentsToVisitNotForbidden);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private RouteSegment processIntersections(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
|
||||
TLongObjectHashMap<RouteSegment> visitedSegments, float distFromStart, RouteSegment segment,
|
||||
short segmentPoint, RouteSegment inputNext, boolean reverseWaySearch, boolean doNotAddIntersections,
|
||||
boolean[] processFurther) {
|
||||
boolean thereAreRestrictions ;
|
||||
boolean thereAreRestrictions;
|
||||
RouteSegment itself = null;
|
||||
processFurther[0] = true;
|
||||
Iterator<RouteSegment> nextIterator = null;
|
||||
if(inputNext != null && inputNext.getRoad().getId() == segment.getRoad().getId() && inputNext.next == null) {
|
||||
if (inputNext != null && inputNext.getRoad().getId() == segment.getRoad().getId() && inputNext.next == null) {
|
||||
thereAreRestrictions = false;
|
||||
} else {
|
||||
thereAreRestrictions = proccessRestrictions(ctx, segment, inputNext, reverseWaySearch);
|
||||
|
|
@ -732,7 +733,7 @@ public class BinaryRoutePlanner {
|
|||
// find segment itself
|
||||
// (and process it as other with small exception that we don't add to graph segments and process immediately)
|
||||
itself = next.initRouteSegment(segment.isPositive());
|
||||
if(itself == null) {
|
||||
if (itself == null) {
|
||||
// do nothing
|
||||
} else if (itself.getParentRoute() == null
|
||||
|| ctx.roadPriorityComparator(itself.distanceFromStart, itself.distanceToEnd, distFromStart,
|
||||
|
|
@ -746,7 +747,7 @@ public class BinaryRoutePlanner {
|
|||
// and we had better results (so we shouldn't process)
|
||||
processFurther[0] = false;
|
||||
}
|
||||
} else if(!doNotAddIntersections) {
|
||||
} else if (!doNotAddIntersections) {
|
||||
RouteSegment nextPos = next.initRouteSegment(true);
|
||||
RouteSegment nextNeg = next.initRouteSegment(false);
|
||||
processOneRoadIntersection(ctx, graphSegments, visitedSegments, distFromStart, distanceToEnd, segment, segmentPoint,
|
||||
|
|
@ -776,11 +777,11 @@ public class BinaryRoutePlanner {
|
|||
next.getRoad().getPointsLength() - 1 : 0,
|
||||
segment, segmentPoint);
|
||||
distFromStart += obstaclesTime;
|
||||
if(TEST_SPECIFIC && next.road.getId() == TEST_ID) {
|
||||
printRoad(" !? distFromStart=" + +distFromStart + " from " + segment.getRoad().getId() +
|
||||
" dir=" + segment.getDirectionAssigned() +
|
||||
if (TEST_SPECIFIC && next.road.getId() == TEST_ID) {
|
||||
printRoad(" !? distFromStart=" + +distFromStart + " from " + segment.getRoad().getId() +
|
||||
" dir=" + segment.getDirectionAssigned() +
|
||||
" distToEnd=" + distanceToEnd +
|
||||
" segmentPoint="+ segmentPoint + " -- ", next, true);
|
||||
" segmentPoint=" + segmentPoint + " -- ", next, true);
|
||||
}
|
||||
if (!visitedSegments.containsKey(calculateRoutePointId(next, next.isPositive()))) {
|
||||
if (next.getParentRoute() == null
|
||||
|
|
@ -805,7 +806,7 @@ public class BinaryRoutePlanner {
|
|||
if (distFromStart < next.distanceFromStart) {
|
||||
if (ctx.config.heuristicCoefficient <= 1) {
|
||||
System.err.println("! Alert distance from start " + distFromStart + " < "
|
||||
+ next.distanceFromStart + " id="+next.road.id);
|
||||
+ next.distanceFromStart + " id=" + next.road.id);
|
||||
}
|
||||
// A: we can't change parent route just here, because we need to update visitedSegments
|
||||
// presumably we can do visitedSegments.put(calculateRoutePointId(next), next);
|
||||
|
|
@ -845,43 +846,43 @@ public class BinaryRoutePlanner {
|
|||
public int preciseY;
|
||||
public List<RouteSegmentPoint> others;
|
||||
}
|
||||
|
||||
|
||||
public static class RouteSegment {
|
||||
final short segStart;
|
||||
final RouteDataObject road;
|
||||
// needed to store intersection of routes
|
||||
RouteSegment next = null;
|
||||
RouteSegment oppositeDirection = null;
|
||||
|
||||
|
||||
// search context (needed for searching route)
|
||||
// Initially it should be null (!) because it checks was it segment visited before
|
||||
RouteSegment parentRoute = null;
|
||||
short parentSegmentEnd = 0;
|
||||
// 1 - positive , -1 - negative, 0 not assigned
|
||||
byte directionAssgn = 0;
|
||||
|
||||
|
||||
// distance measured in time (seconds)
|
||||
float distanceFromStart = 0;
|
||||
float distanceToEnd = 0;
|
||||
|
||||
|
||||
public RouteSegment(RouteDataObject road, int segmentStart) {
|
||||
this.road = road;
|
||||
this.segStart = (short) segmentStart;
|
||||
}
|
||||
|
||||
|
||||
public RouteSegment initRouteSegment(boolean positiveDirection) {
|
||||
if(segStart == 0 && !positiveDirection) {
|
||||
if (segStart == 0 && !positiveDirection) {
|
||||
return null;
|
||||
}
|
||||
if(segStart == road.getPointsLength() - 1 && positiveDirection) {
|
||||
if (segStart == road.getPointsLength() - 1 && positiveDirection) {
|
||||
return null;
|
||||
}
|
||||
RouteSegment rs = this;
|
||||
if(directionAssgn == 0) {
|
||||
if (directionAssgn == 0) {
|
||||
rs.directionAssgn = (byte) (positiveDirection ? 1 : -1);
|
||||
} else {
|
||||
if(positiveDirection != (directionAssgn == 1)) {
|
||||
if(oppositeDirection == null) {
|
||||
if (positiveDirection != (directionAssgn == 1)) {
|
||||
if (oppositeDirection == null) {
|
||||
oppositeDirection = new RouteSegment(road, segStart);
|
||||
oppositeDirection.directionAssgn = (byte) (positiveDirection ? 1 : -1);
|
||||
}
|
||||
|
|
@ -893,77 +894,78 @@ public class BinaryRoutePlanner {
|
|||
}
|
||||
return rs;
|
||||
}
|
||||
|
||||
public byte getDirectionAssigned(){
|
||||
|
||||
public byte getDirectionAssigned() {
|
||||
return directionAssgn;
|
||||
}
|
||||
|
||||
|
||||
public RouteSegment getParentRoute() {
|
||||
return parentRoute;
|
||||
}
|
||||
|
||||
|
||||
public boolean isPositive() {
|
||||
return directionAssgn == 1;
|
||||
}
|
||||
|
||||
|
||||
public void setParentRoute(RouteSegment parentRoute) {
|
||||
this.parentRoute = parentRoute;
|
||||
}
|
||||
|
||||
|
||||
public void assignDirection(byte b) {
|
||||
directionAssgn = b;
|
||||
}
|
||||
|
||||
|
||||
public void setParentSegmentEnd(int parentSegmentEnd) {
|
||||
this.parentSegmentEnd = (short) parentSegmentEnd;
|
||||
}
|
||||
|
||||
|
||||
public int getParentSegmentEnd() {
|
||||
return parentSegmentEnd;
|
||||
}
|
||||
|
||||
|
||||
public RouteSegment getNext() {
|
||||
return next;
|
||||
}
|
||||
|
||||
|
||||
public short getSegmentStart() {
|
||||
return segStart;
|
||||
}
|
||||
|
||||
|
||||
public float getDistanceFromStart() {
|
||||
return distanceFromStart;
|
||||
}
|
||||
|
||||
|
||||
public void setDistanceFromStart(float distanceFromStart) {
|
||||
this.distanceFromStart = distanceFromStart;
|
||||
}
|
||||
|
||||
|
||||
public RouteDataObject getRoad() {
|
||||
return road;
|
||||
}
|
||||
|
||||
public String getTestName(){
|
||||
return MessageFormat.format("s{0,number,#.##} e{1,number,#.##}", ((float)distanceFromStart), ((float)distanceToEnd));
|
||||
|
||||
public String getTestName() {
|
||||
return MessageFormat.format("s{0,number,#.##} e{1,number,#.##}", ((float) distanceFromStart), ((float) distanceToEnd));
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
public Iterator<RouteSegment> getIterator() {
|
||||
return new Iterator<BinaryRoutePlanner.RouteSegment>() {
|
||||
RouteSegment next = RouteSegment.this;
|
||||
|
||||
@Override
|
||||
public void remove() {
|
||||
throw new UnsupportedOperationException();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public RouteSegment next() {
|
||||
RouteSegment c = next;
|
||||
if(next != null) {
|
||||
if (next != null) {
|
||||
next = next.next;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public boolean hasNext() {
|
||||
return next != null;
|
||||
|
|
@ -971,16 +973,16 @@ public class BinaryRoutePlanner {
|
|||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static class FinalRouteSegment extends RouteSegment {
|
||||
|
||||
|
||||
boolean reverseWaySearch;
|
||||
RouteSegment opposite;
|
||||
|
||||
public FinalRouteSegment(RouteDataObject road, int segmentStart) {
|
||||
super(road, segmentStart);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue