package net.osmand.router;
import gnu.trove.map.hash.TLongObjectHashMap;
import java.io.IOException;
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.router.BinaryRoutePlanner.FinalRouteSegment;
import net.osmand.router.BinaryRoutePlanner.RouteSegment;
import net.osmand.util.MapUtils;
import org.apache.commons.logging.Log;
public class BinaryRoutePlannerOld {
public static boolean PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST = true;
private static final int REVERSE_WAY_RESTRICTION_ONLY = 1024;
private static final int STANDARD_ROAD_IN_QUEUE_OVERHEAD = 900;
protected static final Log log = PlatformUtil.getLog(BinaryRoutePlannerOld.class);
private static final int ROUTE_POINTS = 11;
private static double squareRootDist(int x1, int y1, int x2, int y2) {
return MapUtils.squareRootDist31(x1, y1, x2, y2);
// return measuredDist(x1, y1, x2, y2);
}
/**
* Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm)
* return list of segments
*/
void searchRouteInternal(final RoutingContext ctx, RouteSegment start, RouteSegment end) throws IOException, InterruptedException {
// measure time
ctx.timeToLoad = 0;
ctx.visitedSegments = 0;
ctx.timeToCalculate = System.nanoTime();
if(ctx.config.initialDirection != null) {
ctx.firstRoadId = (start.getRoad().id << ROUTE_POINTS) + start.getSegmentStart();
double plusDir = start.getRoad().directionRoute(start.getSegmentStart(), true);
double diff = plusDir - ctx.config.initialDirection;
if(Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
ctx.firstRoadDirection = 1;
} else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
ctx.firstRoadDirection = -1;
}
}
// Initializing priority queue to visit way segments
Comparator<RouteSegment> segmentsComparator = new Comparator<RouteSegment>(){
@Override
public int compare(RouteSegment o1, RouteSegment o2) {
return ctx.roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
}
};
Comparator<RouteSegment> nonHeuristicSegmentsComparator = new Comparator<RouteSegment>(){
@Override
public int compare(RouteSegment o1, RouteSegment o2) {
return roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd, 0.5);
}
};
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)
TLongObjectHashMap<RouteSegment> visitedDirectSegments = new TLongObjectHashMap<RouteSegment>();
TLongObjectHashMap<RouteSegment> visitedOppositeSegments = new TLongObjectHashMap<RouteSegment>();
boolean runRecalculation = ctx.previouslyCalculatedRoute != null && ctx.previouslyCalculatedRoute.size() > 0
&& ctx.config.recalculateDistance != 0;
if (runRecalculation) {
RouteSegment previous = null;
List<RouteSegmentResult> rlist = new ArrayList<RouteSegmentResult>();
float distanceThreshold = ctx.config.recalculateDistance;
float threshold = 0;
for(RouteSegmentResult rr : ctx.previouslyCalculatedRoute) {
threshold += rr.getDistance();
if(threshold > distanceThreshold) {
rlist.add(rr);
}
}
runRecalculation = rlist.size() > 0;
if (rlist.size() > 0) {
for (RouteSegmentResult rr : rlist) {
RouteSegment segment = new RouteSegment(rr.getObject(), rr.getEndPointIndex());
if (previous != null) {
previous.setParentRoute(segment);
previous.setParentSegmentEnd(rr.getStartPointIndex());
long t = (rr.getObject().getId() << ROUTE_POINTS) + segment.getSegmentStart();
visitedOppositeSegments.put(t, segment);
}
previous = segment;
}
end = previous;
}
}
// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
int targetEndX = end.road.getPoint31XTile(end.getSegmentStart());
int targetEndY = end.road.getPoint31YTile(end.getSegmentStart());
int startX = start.road.getPoint31XTile(start.getSegmentStart());
int startY = start.road.getPoint31YTile(start.getSegmentStart());
float estimatedDistance = (float) h(ctx, targetEndX, targetEndY, startX, startY);
end.distanceToEnd = start.distanceToEnd = estimatedDistance;
graphDirectSegments.add(start);
graphReverseSegments.add(end);
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
boolean inverse = false;
boolean init = false;
PriorityQueue<RouteSegment> graphSegments;
if(inverse) {
graphSegments = graphReverseSegments;
} else {
graphSegments = graphDirectSegments;
}
while (!graphSegments.isEmpty()) {
RouteSegment segment = graphSegments.poll();
ctx.visitedSegments++;
// for debug purposes
if (ctx.visitor != null) {
// ctx.visitor.visitSegment(segment, true);
}
updateCalculationProgress(ctx, graphDirectSegments, graphReverseSegments);
boolean routeFound = false;
if (!inverse) {
routeFound = processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments, targetEndX, targetEndY,
segment, visitedOppositeSegments);
} else {
routeFound = processRouteSegment(ctx, true, graphReverseSegments, visitedOppositeSegments, startX, startY, segment,
visitedDirectSegments);
}
if (graphReverseSegments.isEmpty() || graphDirectSegments.isEmpty() || routeFound) {
break;
}
if(runRecalculation) {
// nothing to do
inverse = false;
} else if (!init) {
inverse = !inverse;
init = true;
} else if (ctx.planRouteIn2Directions()) {
inverse = nonHeuristicSegmentsComparator.compare(graphDirectSegments.peek(), graphReverseSegments.peek()) > 0;
if (graphDirectSegments.size() * 1.3 > graphReverseSegments.size()) {
inverse = true;
} else if (graphDirectSegments.size() < 1.3 * graphReverseSegments.size()) {
inverse = false;
}
} else {
// different strategy : use onedirectional graph
inverse = ctx.getPlanRoadDirection() < 0;
}
if (inverse) {
graphSegments = graphReverseSegments;
} else {
graphSegments = graphDirectSegments;
}
// check if interrupted
if(ctx.calculationProgress != null && ctx.calculationProgress.isCancelled) {
throw new InterruptedException("Route calculation interrupted");
}
}
println("Result is found");
printDebugMemoryInformation(ctx, graphDirectSegments, graphReverseSegments, visitedDirectSegments, visitedOppositeSegments);
}
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();
RouteSegment dirPeek = graphDirectSegments.peek();
if(dirPeek != null) {
ctx.calculationProgress.distanceFromBegin =
Math.max(dirPeek.distanceFromStart, ctx.calculationProgress.distanceFromBegin);
}
RouteSegment revPeek = graphReverseSegments.peek();
if(revPeek != null) {
ctx.calculationProgress.distanceFromEnd =
Math.max(revPeek.distanceFromStart, ctx.calculationProgress.distanceFromEnd);
}
}
}
private double h(final RoutingContext ctx, int targetEndX, int targetEndY,
int startX, int startY) {
double distance = squareRootDist(startX, startY, targetEndX, targetEndY);
return distance / ctx.getRouter().getMaxDefaultSpeed();
}
protected static double h(RoutingContext ctx, double distToFinalPoint, RouteSegment next) {
return distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
}
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);
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 segments sizes: " + visitedDirectSegments.size() + "/" + visitedOppositeSegments.size());
}
}
private boolean processRouteSegment(final RoutingContext ctx, boolean reverseWaySearch,
PriorityQueue<RouteSegment> graphSegments, TLongObjectHashMap<RouteSegment> visitedSegments, int targetEndX, int targetEndY,
RouteSegment segment, TLongObjectHashMap<RouteSegment> 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 RouteDataObject road = segment.road;
final int middle = segment.getSegmentStart();
float obstaclePlusTime = 0;
float obstacleMinusTime = 0;
// This is correct way of checking but it has problem with relaxing strategy
// long ntf = (segment.road.getId() << ROUTE_POINTS) + segment.segmentStart;
// visitedSegments.put(ntf, segment);
// if (oppositeSegments.contains(ntf) && oppositeSegments.get(ntf) != null) {
// RouteSegment opposite = oppositeSegments.get(ntf);
// if (opposite.segmentStart == segment.segmentStart) {
// if (reverseWaySearch) {
// reverse : segment.parentSegmentEnd - segment.parentRoute
// } else {
// reverse : opposite.parentSegmentEnd - oppositie.parentRoute
// }
// return true;
// }
// }
// 0. mark route segment as visited
long nt = (road.getId() << ROUTE_POINTS) + middle;
// avoid empty segments to connect but mark the point as visited
visitedSegments.put(nt, null);
int oneway = ctx.getRouter().isOneWay(road);
boolean minusAllowed;
boolean plusAllowed;
if(ctx.firstRoadId == nt) {
if(ctx.firstRoadDirection < 0) {
obstaclePlusTime += 500;
} else if(ctx.firstRoadDirection > 0) {
obstacleMinusTime += 500;
}
}
if (!reverseWaySearch) {
minusAllowed = oneway <= 0;
plusAllowed = oneway >= 0;
} else {
minusAllowed = oneway >= 0;
plusAllowed = oneway <= 0;
}
// +/- diff from middle point
int d = plusAllowed ? 1 : -1;
if(segment.parentRoute != null) {
if(plusAllowed && middle < segment.getRoad().getPointsLength() - 1) {
obstaclePlusTime = (float) ctx.getRouter().calculateTurnTime(segment, segment.getRoad().getPointsLength() - 1,
segment.parentRoute, segment.parentSegmentEnd);
}
if(minusAllowed && middle > 0) {
obstacleMinusTime = (float) ctx.getRouter().calculateTurnTime(segment, 0,
segment.parentRoute, segment.parentSegmentEnd);
}
}
// 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)
float posSegmentDist = 0;
float negSegmentDist = 0;
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;
boolean positive = d > 0;
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() << ROUTE_POINTS) + segmentEnd;
visitedSegments.put(nts, segment);
// 2. calculate point and try to load neighbor ways if they are not loaded
int x = road.getPoint31XTile(segmentEnd);
int y = road.getPoint31YTile(segmentEnd);
if(positive) {
posSegmentDist += squareRootDist(x, y,
road.getPoint31XTile(segmentEnd - 1), road.getPoint31YTile(segmentEnd - 1));
} else {
negSegmentDist += squareRootDist(x, y,
road.getPoint31XTile(segmentEnd + 1), road.getPoint31YTile(segmentEnd + 1));
}
// 2.1 calculate possible obstacle plus time
if(positive){
double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentEnd);
if (obstacle < 0) {
plusAllowed = false;
continue;
}
obstaclePlusTime += obstacle;
} else {
double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentEnd);
if (obstacle < 0) {
minusAllowed = false;
continue;
}
obstacleMinusTime += obstacle;
}
// int overhead = 0;
// could be expensive calculation
int overhead = (ctx.visitedSegments - ctx.relaxedSegments ) *
STANDARD_ROAD_IN_QUEUE_OVERHEAD;
if(overhead > ctx.config.memoryLimitation * 0.95){
throw new OutOfMemoryError("There is no enough memory " + ctx.config.memoryLimitation/(1<<20) + " Mb");
}
RouteSegment next = ctx.loadRouteSegment(x, y, ctx.config.memoryLimitation - overhead);
// 3. get intersected ways
if (next != null) {
// TO-DO U-Turn
if((next == segment || next.road.id == road.id) && next.next == null) {
// simplification if there is no real intersection
continue;
}
// Using A* routing algorithm
// g(x) - calculate distance to that point and calculate time
float priority = ctx.getRouter().defineSpeedPriority(road);
float speed = ctx.getRouter().defineRoutingSpeed(road) * priority;
if (speed == 0) {
speed = ctx.getRouter().getMinDefaultSpeed() * priority;
}
float distOnRoadToPass = positive? posSegmentDist : negSegmentDist;
float distStartObstacles = segment.distanceFromStart + ( positive ? obstaclePlusTime : obstacleMinusTime) +
distOnRoadToPass / speed;
float distToFinalPoint = (float) squareRootDist(x, y, targetEndX, targetEndY);
boolean routeFound = processIntersections(ctx, graphSegments, visitedSegments, oppositeSegments,
distStartObstacles, distToFinalPoint, segment, segmentEnd, next, reverseWaySearch);
if(routeFound){
return routeFound;
}
}
}
return false;
}
private boolean proccessRestrictions(RoutingContext ctx, RouteDataObject road, RouteSegment inputNext, boolean reverseWay) {
ctx.segmentsToVisitPrescripted.clear();
ctx.segmentsToVisitNotForbidden.clear();
boolean exclusiveRestriction = false;
RouteSegment next = inputNext;
if (!reverseWay && road.getRestrictionLength() == 0) {
return false;
}
if(!ctx.getRouter().restrictionsAware()) {
return 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;
} else if (type == -1) {
// case no restriction
ctx.segmentsToVisitNotForbidden.add(next);
} else {
// 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;
}
ctx.segmentsToVisitPrescripted.addAll(ctx.segmentsToVisitNotForbidden);
return true;
}
private boolean processIntersections(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
TLongObjectHashMap<RouteSegment> visitedSegments, TLongObjectHashMap<RouteSegment> oppositeSegments,
float distFromStart, float distToFinalPoint,
RouteSegment segment, int segmentEnd, RouteSegment inputNext,
boolean reverseWay) {
boolean thereAreRestrictions = proccessRestrictions(ctx, segment.road, inputNext, reverseWay);
Iterator<RouteSegment> nextIterator = null;
if (thereAreRestrictions) {
nextIterator = ctx.segmentsToVisitPrescripted.iterator();
}
// 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();
}
long nts = (next.road.getId() << ROUTE_POINTS) + next.getSegmentStart();
// 1. Check if opposite segment found so we can stop calculations
if (oppositeSegments.contains(nts) && oppositeSegments.get(nts) != null) {
// restrictions checked
RouteSegment opposite = oppositeSegments.get(nts);
// additional check if opposite way not the same as current one
if (next.getSegmentStart() != segmentEnd ||
opposite.getRoad().getId() != segment.getRoad().getId()) {
FinalRouteSegment frs = new FinalRouteSegment(segment.getRoad(), segment.getSegmentStart());
float distStartObstacles = segment.distanceFromStart;
frs.setParentRoute(segment.getParentRoute());
frs.setParentSegmentEnd(segment.getParentSegmentEnd());
frs.reverseWaySearch = reverseWay;
frs.distanceFromStart = opposite.distanceFromStart + distStartObstacles;
RouteSegment op = new RouteSegment(segment.getRoad(), segmentEnd);
op.setParentRoute(opposite);
op.setParentSegmentEnd(next.getSegmentStart());
frs.distanceToEnd = 0;
frs.opposite = op;
ctx.finalRouteSegment = frs;
return true;
}
}
// road.id could be equal on roundabout, but we should accept them
boolean alreadyVisited = visitedSegments.contains(nts);
if (!alreadyVisited) {
float distanceToEnd = (float) h(ctx, distToFinalPoint, next);
if (next.parentRoute == null
|| ctx.roadPriorityComparator(next.distanceFromStart, next.distanceToEnd, distFromStart, distanceToEnd) > 0) {
if (next.parentRoute != null) {
// already in queue remove it
if (!graphSegments.remove(next)) {
// exist in different queue!
next = new RouteSegment(next.getRoad(), next.getSegmentStart());
}
}
next.distanceFromStart = distFromStart;
next.distanceToEnd = distanceToEnd;
// put additional information to recover whole route after
next.setParentRoute(segment);
next.setParentSegmentEnd(segmentEnd);
graphSegments.add(next);
}
if (ctx.visitor != null) {
// ctx.visitor.visitSegment(next, false);
}
} else {
// the segment was already visited! We need to follow better route if it exists
// that is very strange situation and almost exception (it can happen when we underestimate distnceToEnd)
if (distFromStart < next.distanceFromStart && next.road.id != segment.road.id) {
// That code is incorrect (when segment is processed itself,
// then it tries to make wrong u-turn) -
// this situation should be very carefully checked in future (seems to be fixed)
// System.out.println(segment.getRoad().getName() + " " + next.getRoad().getName());
// System.out.println(next.distanceFromStart + " ! " + distFromStart);
next.distanceFromStart = distFromStart;
next.setParentRoute(segment);
next.setParentSegmentEnd(segmentEnd);
if (ctx.visitor != null) {
// ctx.visitor.visitSegment(next, next.getSegmentStart(), false);
}
}
}
// iterate to next road
if (nextIterator == null) {
next = next.next;
hasNext = next != null;
} else {
hasNext = nextIterator.hasNext();
}
}
return 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);
}
}