986 lines
36 KiB
Java
986 lines
36 KiB
Java
package net.osmand.router;
|
|
|
|
import gnu.trove.map.hash.TLongObjectHashMap;
|
|
|
|
import java.io.IOException;
|
|
import java.text.MessageFormat;
|
|
import java.util.ArrayList;
|
|
import java.util.Comparator;
|
|
import java.util.Iterator;
|
|
import java.util.List;
|
|
import java.util.PriorityQueue;
|
|
|
|
import net.osmand.PlatformUtil;
|
|
import net.osmand.binary.RouteDataObject;
|
|
import net.osmand.osm.MapRenderingTypes;
|
|
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
|
|
*/
|
|
@SuppressWarnings("unused")
|
|
FinalRouteSegment searchRouteInternal(final RoutingContext ctx, RouteSegmentPoint start, RouteSegmentPoint end,
|
|
RouteSegment recalculationEnd ) throws InterruptedException, IOException {
|
|
// measure time
|
|
ctx.timeToLoad = 0;
|
|
ctx.visitedSegments = 0;
|
|
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;
|
|
|
|
FinalRouteSegment finalSegment = null;
|
|
boolean onlyBackward = 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 +
|
|
(graphDirectSegments.size() +
|
|
graphReverseSegments.size()) * STANDARD_ROAD_IN_QUEUE_OVERHEAD;
|
|
|
|
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");
|
|
printMemoryConsumption("Memory occupied after calculation : ");
|
|
}
|
|
finalSegment = (FinalRouteSegment) segment;
|
|
if(TRACE_ROUTING){
|
|
println("Final segment found");
|
|
}
|
|
break;
|
|
}
|
|
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");
|
|
}
|
|
ctx.visitedSegments++;
|
|
if (forwardSearch) {
|
|
boolean doNotAddIntersections = onlyBackward;
|
|
processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments,
|
|
segment, visitedOppositeSegments, doNotAddIntersections);
|
|
} else {
|
|
boolean doNotAddIntersections = onlyForward;
|
|
processRouteSegment(ctx, true, graphReverseSegments, visitedOppositeSegments, segment,
|
|
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,
|
|
"Route is not found from selected start point.");
|
|
if (ctx.planRouteIn2Directions()) {
|
|
forwardSearch = (nonHeuristicSegmentsComparator.compare(graphDirectSegments.peek(), graphReverseSegments.peek()) < 0);
|
|
// if (graphDirectSegments.size() * 2 > graphReverseSegments.size()) {
|
|
// forwardSearch = false;
|
|
// } else if (graphDirectSegments.size() < 2 * graphReverseSegments.size()) {
|
|
// forwardSearch = true;
|
|
// }
|
|
} else {
|
|
// different strategy : use onedirectional graph
|
|
forwardSearch = onlyForward;
|
|
if(onlyBackward && !graphDirectSegments.isEmpty()) {
|
|
forwardSearch = true;
|
|
}
|
|
if(onlyForward && !graphReverseSegments.isEmpty()) {
|
|
forwardSearch = false;
|
|
}
|
|
}
|
|
|
|
if (forwardSearch) {
|
|
graphSegments = graphDirectSegments;
|
|
} else {
|
|
graphSegments = graphReverseSegments;
|
|
}
|
|
// check if interrupted
|
|
if(ctx.calculationProgress != null && ctx.calculationProgress.isCancelled) {
|
|
throw new InterruptedException("Route calculation interrupted");
|
|
}
|
|
}
|
|
printDebugMemoryInformation(ctx, graphDirectSegments, graphReverseSegments, visitedDirectSegments, visitedOppositeSegments);
|
|
return finalSegment;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
protected void checkIfGraphIsEmpty(final RoutingContext ctx, boolean allowDirection,
|
|
PriorityQueue<RouteSegment> graphSegments, RouteSegmentPoint pnt, TLongObjectHashMap<RouteSegment> visited,
|
|
String msg) {
|
|
if (allowDirection && graphSegments.isEmpty()) {
|
|
if (pnt.others != null) {
|
|
Iterator<RouteSegmentPoint> pntIterator = pnt.others.iterator();
|
|
while (pntIterator.hasNext()) {
|
|
RouteSegmentPoint next = pntIterator.next();
|
|
boolean visitedAlready = false;
|
|
if (next.getSegmentStart() > 0 && visited.containsKey(calculateRoutePointId(next, false))) {
|
|
visitedAlready = true;
|
|
} else if (next.getSegmentStart() < next.getRoad().getPointsLength() - 1
|
|
&& visited.containsKey(calculateRoutePointId(next, true))) {
|
|
visitedAlready = true;
|
|
}
|
|
pntIterator.remove();
|
|
if (!visitedAlready) {
|
|
float estimatedDistance = (float) estimatedDistance(ctx, ctx.targetX, ctx.targetY, ctx.startX,
|
|
ctx.startY);
|
|
RouteSegment pos = next.initRouteSegment(true);
|
|
RouteSegment neg = next.initRouteSegment(false);
|
|
if (pos != null) {
|
|
pos.distanceToEnd = estimatedDistance;
|
|
graphSegments.add(pos);
|
|
}
|
|
if (neg != null) {
|
|
neg.distanceToEnd = estimatedDistance;
|
|
graphSegments.add(neg);
|
|
}
|
|
println("Reiterate point with new start/destination " + next.getRoad());
|
|
break;
|
|
}
|
|
}
|
|
if (graphSegments.isEmpty()) {
|
|
throw new IllegalArgumentException(msg);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public RouteSegment initRouteSegment(final RoutingContext ctx, RouteSegment segment, boolean positiveDirection) {
|
|
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);
|
|
}
|
|
if(segment == null) {
|
|
return null;
|
|
}
|
|
return segment.initRouteSegment(positiveDirection);
|
|
}
|
|
|
|
|
|
protected RouteSegment loadSameSegment(final RoutingContext ctx, RouteSegment segment, int ind) {
|
|
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()) {
|
|
segment = s;
|
|
break;
|
|
}
|
|
s = s.getNext();
|
|
}
|
|
return segment;
|
|
}
|
|
|
|
|
|
private void initQueuesWithStartEnd(final RoutingContext ctx, RouteSegment start, RouteSegment end,
|
|
RouteSegment recalculationEnd, PriorityQueue<RouteSegment> graphDirectSegments, PriorityQueue<RouteSegment> graphReverseSegments) {
|
|
RouteSegment startPos = initRouteSegment(ctx, start, true);
|
|
RouteSegment startNeg = initRouteSegment(ctx, start, false);
|
|
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) {
|
|
// 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) {
|
|
startNeg.distanceFromStart += 500;
|
|
}
|
|
} else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
|
|
if(startPos != null) {
|
|
startPos.distanceFromStart += 500;
|
|
}
|
|
}
|
|
}
|
|
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) {
|
|
startPos.distanceToEnd = estimatedDistance;
|
|
graphDirectSegments.add(startPos);
|
|
}
|
|
if(startNeg != null) {
|
|
startNeg.distanceToEnd = estimatedDistance;
|
|
graphDirectSegments.add(startNeg);
|
|
}
|
|
if(recalculationEnd != null) {
|
|
graphReverseSegments.add(recalculationEnd);
|
|
} else {
|
|
if (endPos != null) {
|
|
endPos.distanceToEnd = estimatedDistance;
|
|
graphReverseSegments.add(endPos);
|
|
}
|
|
if (endNeg != null) {
|
|
endNeg.distanceToEnd = estimatedDistance;
|
|
graphReverseSegments.add(endNeg);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
private void printMemoryConsumption( String string) {
|
|
long h1 = RoutingContext.runGCUsedMemory();
|
|
float mb = (1 << 20);
|
|
log.warn(string + h1 / mb);
|
|
}
|
|
|
|
|
|
private void updateCalculationProgress(final RoutingContext ctx, PriorityQueue<RouteSegment> graphDirectSegments,
|
|
PriorityQueue<RouteSegment> graphReverseSegments) {
|
|
if(ctx.calculationProgress != null) {
|
|
ctx.calculationProgress.reverseSegmentQueueSize = graphReverseSegments.size();
|
|
ctx.calculationProgress.directSegmentQueueSize = graphDirectSegments.size();
|
|
if(graphDirectSegments.size() > 0 && ctx.getPlanRoadDirection() >= 0) {
|
|
RouteSegment peek = graphDirectSegments.peek();
|
|
ctx.calculationProgress.distanceFromBegin = Math.max(peek.distanceFromStart,
|
|
ctx.calculationProgress.distanceFromBegin);
|
|
ctx.calculationProgress.directDistance = peek.distanceFromStart + peek.distanceToEnd;
|
|
}
|
|
if(graphReverseSegments.size() > 0 && ctx.getPlanRoadDirection() <= 0) {
|
|
RouteSegment peek = graphReverseSegments.peek();
|
|
ctx.calculationProgress.distanceFromEnd = Math.max(peek.distanceFromStart + peek.distanceToEnd,
|
|
ctx.calculationProgress.distanceFromEnd);
|
|
ctx.calculationProgress.reverseDistance = peek.distanceFromStart + peek.distanceToEnd;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
private void printRoad(String prefix, RouteSegment segment, Boolean reverseWaySearch) {
|
|
String pr;
|
|
if(segment.parentRoute != null){
|
|
pr = " pend="+segment.parentSegmentEnd +" parent=" + segment.parentRoute.road;
|
|
} else {
|
|
pr = "";
|
|
}
|
|
String p = "";
|
|
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);
|
|
}
|
|
|
|
private float estimatedDistance(final RoutingContext ctx, int targetEndX, int targetEndY,
|
|
int startX, int startY) {
|
|
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 result = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
|
|
if(ctx.precalculatedRouteDirection != null){
|
|
float te = ctx.precalculatedRouteDirection.timeEstimate(begX, begY, endX, endY);
|
|
if(te > 0) {
|
|
return te;
|
|
}
|
|
}
|
|
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);
|
|
}
|
|
|
|
public void printDebugMemoryInformation(RoutingContext ctx, PriorityQueue<RouteSegment> graphDirectSegments, PriorityQueue<RouteSegment> graphReverseSegments,
|
|
TLongObjectHashMap<RouteSegment> visitedDirectSegments,TLongObjectHashMap<RouteSegment> visitedOppositeSegments) {
|
|
printInfo("Time to calculate : " + (System.nanoTime() - ctx.timeToCalculate) / 1e6 +
|
|
", time to load : " + ctx.timeToLoad / 1e6 + ", time to load headers : " + ctx.timeToLoadHeaders / 1e6 +
|
|
", 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 +
|
|
", loaded more than once same tiles "
|
|
+ ctx.loadedPrevUnloadedTiles );
|
|
printInfo("Visited roads " + ctx.visitedSegments + ", relaxed roads " + ctx.relaxedSegments);
|
|
if (graphDirectSegments != null && graphReverseSegments != null) {
|
|
printInfo("Priority queues sizes : " + graphDirectSegments.size() + "/" + graphReverseSegments.size());
|
|
}
|
|
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 ) {
|
|
printRoad(" ! " + +segment.distanceFromStart + " ", segment, reverseWaySearch);
|
|
}
|
|
boolean directionAllowed = initDirectionAllowed;
|
|
if(!directionAllowed) {
|
|
if(TRACE_ROUTING) {
|
|
println(" >> Already visited");
|
|
}
|
|
return;
|
|
}
|
|
// 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 the way (it doesn't take into account)
|
|
float obstaclesTime = 0;
|
|
float segmentDist = 0;
|
|
// +/- diff from middle point
|
|
short segmentPoint = segment.getSegmentStart();
|
|
boolean[] processFurther = new boolean[1];
|
|
RouteSegment previous = segment;
|
|
boolean dir = segment.isPositive();
|
|
while (directionAllowed) {
|
|
// mark previous interval as visited and move to next intersection
|
|
short prevInd = segmentPoint;
|
|
if(dir) {
|
|
segmentPoint ++;
|
|
} else {
|
|
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,
|
|
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) {
|
|
continue;
|
|
}
|
|
|
|
// 2. calculate point and try to load neighbor ways if they are not loaded
|
|
segmentDist += squareRootDist(x, y, prevx, prevy);
|
|
|
|
// 2.1 calculate possible obstacle plus time
|
|
double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentPoint);
|
|
if (obstacle < 0) {
|
|
directionAllowed = false;
|
|
continue;
|
|
}
|
|
obstaclesTime += obstacle;
|
|
|
|
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) {
|
|
// long nt = System.nanoTime();
|
|
// float devDistance = ctx.precalculatedRouteDirection.getDeviationDistance(x, y);
|
|
// // 1. linear method
|
|
// segmentDist = segmentDist * (1 + devDistance / ctx.config.DEVIATION_RADIUS);
|
|
// // 2. exponential method
|
|
// segmentDist = segmentDist * (float) Math.pow(1.5, devDistance / 500);
|
|
// 3. next by method
|
|
// segmentDist = devDistance ;
|
|
// ctx.timeNanoToCalcDeviation += (System.nanoTime() - nt);
|
|
}
|
|
// 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()) {
|
|
// 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);
|
|
if (!processFurther[0]) {
|
|
directionAllowed = false;
|
|
continue;
|
|
}
|
|
}
|
|
if(initDirectionAllowed && ctx.visitor != null){
|
|
ctx.visitor.visitSegment(segment, segmentPoint, true);
|
|
}
|
|
}
|
|
|
|
private boolean checkIfInitialMovementAllowedOnSegment(final RoutingContext ctx, boolean reverseWaySearch,
|
|
TLongObjectHashMap<RouteSegment> visitedSegments, RouteSegment segment, final RouteDataObject road) {
|
|
boolean directionAllowed;
|
|
int oneway = ctx.getRouter().isOneWay(road);
|
|
// use positive direction as agreed
|
|
if (!reverseWaySearch) {
|
|
if(segment.isPositive()){
|
|
directionAllowed = oneway >= 0;
|
|
} else {
|
|
directionAllowed = oneway <= 0;
|
|
}
|
|
} else {
|
|
if(segment.isPositive()){
|
|
directionAllowed = oneway <= 0;
|
|
} else {
|
|
directionAllowed = oneway >= 0;
|
|
}
|
|
}
|
|
|
|
if(directionAllowed && visitedSegments.containsKey(calculateRoutePointId(segment, segment.isPositive()))) {
|
|
directionAllowed = false;
|
|
}
|
|
return directionAllowed;
|
|
}
|
|
|
|
private boolean checkViaRestrictions(RouteSegment from, RouteSegment to) {
|
|
if(from != null && to != null) {
|
|
long fid = to.getRoad().getId();
|
|
for(int i = 0; i < from.getRoad().getRestrictionLength(); i++) {
|
|
long id = from.getRoad().getRestrictionId(i);
|
|
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 ||
|
|
tp == MapRenderingTypes.RESTRICTION_NO_U_TURN) {
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private RouteSegment getParentDiffId(RouteSegment s) {
|
|
while(s.getParentRoute() != null && s.getParentRoute().getRoad().getId() == s.getRoad().getId()) {
|
|
s = s.getParentRoute();
|
|
}
|
|
return s.getParentRoute();
|
|
}
|
|
|
|
private boolean checkIfOppositieSegmentWasVisited(final RoutingContext ctx, boolean reverseWaySearch,
|
|
PriorityQueue<RouteSegment> graphSegments, RouteSegment segment, TLongObjectHashMap<RouteSegment> oppositeSegments,
|
|
int segmentPoint, float segmentDist, float obstaclesTime) {
|
|
RouteDataObject road = segment.getRoad();
|
|
long opp = calculateRoutePointId(road, segment.isPositive() ? segmentPoint - 1 : segmentPoint, !segment.isPositive());
|
|
if (oppositeSegments.containsKey(opp)) {
|
|
RouteSegment opposite = oppositeSegments.get(opp);
|
|
RouteSegment to = reverseWaySearch ? getParentDiffId(segment) : getParentDiffId(opposite);
|
|
RouteSegment from = !reverseWaySearch ? getParentDiffId(segment) : getParentDiffId(opposite);
|
|
if (checkViaRestrictions(from, to)) {
|
|
FinalRouteSegment frs = new FinalRouteSegment(road, segmentPoint);
|
|
float distStartObstacles = segment.distanceFromStart
|
|
+ calculateTimeWithObstacles(ctx, road, segmentDist, obstaclesTime);
|
|
frs.setParentRoute(segment);
|
|
frs.setParentSegmentEnd(segmentPoint);
|
|
frs.reverseWaySearch = reverseWaySearch;
|
|
frs.distanceFromStart = opposite.distanceFromStart + distStartObstacles;
|
|
frs.distanceToEnd = 0;
|
|
frs.opposite = opposite;
|
|
graphSegments.add(frs);
|
|
if (TRACE_ROUTING) {
|
|
printRoad(" >> Final segment : ", frs, reverseWaySearch);
|
|
}
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
private float calculateTimeWithObstacles(RoutingContext ctx, RouteDataObject road, float distOnRoadToPass, float obstaclesTime) {
|
|
float priority = ctx.getRouter().defineSpeedPriority(road);
|
|
float speed = (ctx.getRouter().defineRoutingSpeed(road) * priority);
|
|
if (speed == 0) {
|
|
speed = (ctx.getRouter().getMinDefaultSpeed() * priority);
|
|
}
|
|
// speed can not exceed max default speed according to A*
|
|
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) {
|
|
// 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() == segm.getRoad().getPointsLength() - 1 && direction) {
|
|
throw new IllegalStateException("Assert failed");
|
|
}
|
|
return calculateRoutePointId(segm.getRoad(),
|
|
direction ? segm.getSegmentStart() : segm.getSegmentStart() - 1, direction);
|
|
}
|
|
|
|
|
|
private boolean proccessRestrictions(RoutingContext ctx, RouteSegment segment, RouteSegment inputNext, boolean reverseWay) {
|
|
if(!ctx.getRouter().restrictionsAware()) {
|
|
return false;
|
|
}
|
|
RouteDataObject road = segment.getRoad();
|
|
RouteSegment parent = getParentDiffId(segment);
|
|
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) {
|
|
processRestriction(ctx, inputNext, reverseWay, true, parent.getRoad());
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
protected void processRestriction(RoutingContext ctx, RouteSegment inputNext, boolean reverseWay, boolean via,
|
|
RouteDataObject road) {
|
|
RouteSegment next = inputNext;
|
|
boolean exclusiveRestriction = false;
|
|
while (next != null) {
|
|
int type = -1;
|
|
if (!reverseWay) {
|
|
for (int i = 0; i < road.getRestrictionLength(); i++) {
|
|
if (road.getRestrictionId(i) == next.road.id) {
|
|
type = road.getRestrictionType(i);
|
|
break;
|
|
}
|
|
}
|
|
} else {
|
|
for (int i = 0; i < next.road.getRestrictionLength(); i++) {
|
|
int rt = next.road.getRestrictionType(i);
|
|
long restrictedTo = next.road.getRestrictionId(i);
|
|
if (restrictedTo == road.id) {
|
|
type = rt;
|
|
break;
|
|
}
|
|
|
|
// Check if there is restriction only to the other than current road
|
|
if (rt == MapRenderingTypes.RESTRICTION_ONLY_RIGHT_TURN || rt == MapRenderingTypes.RESTRICTION_ONLY_LEFT_TURN
|
|
|| rt == MapRenderingTypes.RESTRICTION_ONLY_STRAIGHT_ON) {
|
|
// check if that restriction applies to considered junk
|
|
RouteSegment foundNext = inputNext;
|
|
while (foundNext != null) {
|
|
if (foundNext.getRoad().id == restrictedTo) {
|
|
break;
|
|
}
|
|
foundNext = foundNext.next;
|
|
}
|
|
if (foundNext != null) {
|
|
type = REVERSE_WAY_RESTRICTION_ONLY; // special constant
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (type == REVERSE_WAY_RESTRICTION_ONLY) {
|
|
// next = next.next; continue;
|
|
} else if (type == -1 && exclusiveRestriction) {
|
|
// next = next.next; continue;
|
|
} 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) {
|
|
ctx.segmentsToVisitPrescripted.remove(next);
|
|
}
|
|
} else if (type == -1) {
|
|
// case no restriction
|
|
ctx.segmentsToVisitNotForbidden.add(next);
|
|
} else {
|
|
if (!via) {
|
|
// case exclusive restriction (only_right, only_straight, ...)
|
|
// 1. in case we are going backward we should not consider only_restriction
|
|
// as exclusive because we have many "in" roads and one "out"
|
|
// 2. in case we are going forward we have one "in" and many "out"
|
|
if (!reverseWay) {
|
|
exclusiveRestriction = true;
|
|
ctx.segmentsToVisitNotForbidden.clear();
|
|
ctx.segmentsToVisitPrescripted.add(next);
|
|
} else {
|
|
ctx.segmentsToVisitNotForbidden.add(next);
|
|
}
|
|
}
|
|
}
|
|
next = next.next;
|
|
}
|
|
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 ;
|
|
RouteSegment itself = null;
|
|
processFurther[0] = true;
|
|
Iterator<RouteSegment> nextIterator = null;
|
|
if(inputNext != null && inputNext.getRoad().getId() == segment.getRoad().getId() && inputNext.next == null) {
|
|
thereAreRestrictions = false;
|
|
} else {
|
|
thereAreRestrictions = proccessRestrictions(ctx, segment, inputNext, reverseWaySearch);
|
|
if (thereAreRestrictions) {
|
|
nextIterator = ctx.segmentsToVisitPrescripted.iterator();
|
|
if (TRACE_ROUTING) {
|
|
println(" >> There are restrictions");
|
|
}
|
|
}
|
|
}
|
|
int targetEndX = reverseWaySearch ? ctx.startX : ctx.targetX;
|
|
int targetEndY = reverseWaySearch ? ctx.startY : ctx.targetY;
|
|
float distanceToEnd = h(ctx, segment.getRoad().getPoint31XTile(segmentPoint), segment.getRoad()
|
|
.getPoint31YTile(segmentPoint), targetEndX, targetEndY);
|
|
// Calculate possible ways to put into priority queue
|
|
RouteSegment next = inputNext;
|
|
boolean hasNext = nextIterator == null || nextIterator.hasNext();
|
|
while (hasNext) {
|
|
if (nextIterator != null) {
|
|
next = nextIterator.next();
|
|
}
|
|
if (next.getSegmentStart() == segmentPoint && next.getRoad().getId() == segment.getRoad().id) {
|
|
// 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) {
|
|
// do nothing
|
|
} else if (itself.getParentRoute() == null
|
|
|| ctx.roadPriorityComparator(itself.distanceFromStart, itself.distanceToEnd, distFromStart,
|
|
distanceToEnd) > 0) {
|
|
itself.distanceFromStart = distFromStart;
|
|
itself.distanceToEnd = distanceToEnd;
|
|
itself.setParentRoute(segment);
|
|
itself.setParentSegmentEnd(segmentPoint);
|
|
} else {
|
|
// we already processed that segment earlier or it is in graph segments
|
|
// and we had better results (so we shouldn't process)
|
|
processFurther[0] = false;
|
|
}
|
|
} else if(!doNotAddIntersections) {
|
|
RouteSegment nextPos = next.initRouteSegment(true);
|
|
RouteSegment nextNeg = next.initRouteSegment(false);
|
|
processOneRoadIntersection(ctx, graphSegments, visitedSegments, distFromStart, distanceToEnd, segment, segmentPoint,
|
|
nextPos);
|
|
processOneRoadIntersection(ctx, graphSegments, visitedSegments, distFromStart, distanceToEnd, segment, segmentPoint,
|
|
nextNeg);
|
|
|
|
}
|
|
// iterate to next road
|
|
if (nextIterator == null) {
|
|
next = next.next;
|
|
hasNext = next != null;
|
|
} else {
|
|
hasNext = nextIterator.hasNext();
|
|
}
|
|
}
|
|
return itself;
|
|
}
|
|
|
|
|
|
@SuppressWarnings("unused")
|
|
private void processOneRoadIntersection(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
|
|
TLongObjectHashMap<RouteSegment> visitedSegments, float distFromStart, float distanceToEnd, RouteSegment segment,
|
|
int segmentPoint, RouteSegment next) {
|
|
if (next != null) {
|
|
float obstaclesTime = (float) ctx.getRouter().calculateTurnTime(next, next.isPositive()?
|
|
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() +
|
|
" distToEnd=" + distanceToEnd +
|
|
" segmentPoint="+ segmentPoint + " -- ", next, true);
|
|
}
|
|
if (!visitedSegments.containsKey(calculateRoutePointId(next, next.isPositive()))) {
|
|
if (next.getParentRoute() == null
|
|
|| ctx.roadPriorityComparator(next.distanceFromStart, next.distanceToEnd,
|
|
distFromStart, distanceToEnd) > 0) {
|
|
next.distanceFromStart = distFromStart;
|
|
next.distanceToEnd = distanceToEnd;
|
|
if (TRACE_ROUTING) {
|
|
printRoad(" >>", next, null);
|
|
}
|
|
// put additional information to recover whole route after
|
|
next.setParentRoute(segment);
|
|
next.setParentSegmentEnd(segmentPoint);
|
|
graphSegments.add(next);
|
|
}
|
|
} else {
|
|
// the segment was already visited! We need to follow better route if it exists
|
|
// that is very exceptional situation and almost exception, it can happen
|
|
// 1. when we underestimate distnceToEnd - wrong h()
|
|
// 2. because we process not small segments but the whole road, it could be that
|
|
// deviation from the road is faster than following the whole road itself!
|
|
if (distFromStart < next.distanceFromStart) {
|
|
if (ctx.config.heuristicCoefficient <= 1) {
|
|
System.err.println("! Alert distance from start " + distFromStart + " < "
|
|
+ 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);
|
|
// next.distanceFromStart = distFromStart;
|
|
// next.setParentRoute(segment);
|
|
// next.setParentSegmentEnd(segmentPoint);
|
|
if (ctx.visitor != null) {
|
|
// ctx.visitor.visitSegment(next, false);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/*public */static int roadPriorityComparator(double o1DistanceFromStart, double o1DistanceToEnd,
|
|
double o2DistanceFromStart, double o2DistanceToEnd, double heuristicCoefficient ) {
|
|
// f(x) = g(x) + h(x) --- g(x) - distanceFromStart, h(x) - distanceToEnd (not exact)
|
|
return Double.compare(o1DistanceFromStart + heuristicCoefficient * o1DistanceToEnd,
|
|
o2DistanceFromStart + heuristicCoefficient * o2DistanceToEnd);
|
|
}
|
|
|
|
|
|
public interface RouteSegmentVisitor {
|
|
|
|
public void visitSegment(RouteSegment segment, int segmentEnd, boolean poll);
|
|
}
|
|
|
|
public static class RouteSegmentPoint extends RouteSegment {
|
|
public RouteSegmentPoint(RouteDataObject road, int segmentStart, double distSquare) {
|
|
super(road, segmentStart);
|
|
this.distSquare = distSquare;
|
|
}
|
|
|
|
public double distSquare;
|
|
public int preciseX;
|
|
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) {
|
|
return null;
|
|
}
|
|
if(segStart == road.getPointsLength() - 1 && positiveDirection) {
|
|
return null;
|
|
}
|
|
RouteSegment rs = this;
|
|
if(directionAssgn == 0) {
|
|
rs.directionAssgn = (byte) (positiveDirection ? 1 : -1);
|
|
} else {
|
|
if(positiveDirection != (directionAssgn == 1)) {
|
|
if(oppositeDirection == null) {
|
|
oppositeDirection = new RouteSegment(road, segStart);
|
|
oppositeDirection.directionAssgn = (byte) (positiveDirection ? 1 : -1);
|
|
}
|
|
if ((oppositeDirection.directionAssgn == 1) != positiveDirection) {
|
|
throw new IllegalStateException();
|
|
}
|
|
rs = oppositeDirection;
|
|
}
|
|
}
|
|
return rs;
|
|
}
|
|
|
|
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 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) {
|
|
next = next.next;
|
|
}
|
|
return c;
|
|
}
|
|
|
|
@Override
|
|
public boolean hasNext() {
|
|
return next != null;
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
static class FinalRouteSegment extends RouteSegment {
|
|
|
|
boolean reverseWaySearch;
|
|
RouteSegment opposite;
|
|
|
|
public FinalRouteSegment(RouteDataObject road, int segmentStart) {
|
|
super(road, segmentStart);
|
|
}
|
|
|
|
}
|
|
|
|
}
|