diff --git a/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java b/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java index 406b8b896f..6e408dc41d 100644 --- a/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java +++ b/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java @@ -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 { 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 { 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 nonHeuristicSegmentsComparator = new NonHeuristicSegmentsComparator(); PriorityQueue graphDirectSegments = new PriorityQueue(50, new SegmentsComparator(ctx)); PriorityQueue graphReverseSegments = new PriorityQueue(50, new SegmentsComparator(ctx)); - + // Set to not visit one segment twice (stores road.id << X + segmentStart) TLongObjectHashMap visitedDirectSegments = new TLongObjectHashMap(); TLongObjectHashMap visitedOppositeSegments = new TLongObjectHashMap(); - + 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 graphSegments = graphDirectSegments; - + + PriorityQueue 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 graphDirectSegments, PriorityQueue 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 graphSegments, TLongObjectHashMap visitedSegments, RouteSegment segment, TLongObjectHashMap 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 in order to not have unique 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 graphSegments, TLongObjectHashMap 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 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 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 getIterator() { return new Iterator() { 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); } - + } - + }