private void visitAllStartSegments( final RoutingContext ctx, RouteSegment start, PriorityQueue<RouteSegment> graphDirectSegments, TLongObjectHashMap<RouteSegment> visitedSegments, int startX, int startY) throws IOException { // mark as visited code seems to be duplicated long nt = (start.road.getId() << 8l) + start.segmentStart; visitedSegments.put(nt, start); graphDirectSegments.add(start); loadRoutes( ctx, (startX >> (31 - ctx.getZoomToLoadTileWithRoads())), (startY >> (31 - ctx.getZoomToLoadTileWithRoads()))); long ls = (((long) startX) << 31) + (long) startY; RouteSegment startNbs = ctx.routes.get(ls); while (startNbs != null) { // startNbs.road.id >> 1, start.road.id >> 1 if (startNbs.road.getId() != start.road.getId()) { startNbs.parentRoute = start; startNbs.parentSegmentEnd = start.segmentStart; startNbs.distanceToEnd = start.distanceToEnd; // duplicated to be sure start is added nt = (startNbs.road.getId() << 8l) + startNbs.segmentStart; visitedSegments.put(nt, startNbs); graphDirectSegments.add(startNbs); } startNbs = startNbs.next; } }
public void loadRoutes(final RoutingContext ctx, int tileX, int tileY) throws IOException { int tileC = (tileX << ctx.getZoomToLoadTileWithRoads()) + tileY; if (ctx.loadedTiles.contains(tileC)) { return; } long now = System.nanoTime(); int zoomToLoad = 31 - ctx.getZoomToLoadTileWithRoads(); SearchFilter searchFilter = new BinaryMapIndexReader.SearchFilter() { @Override public boolean accept(TIntArrayList types, MapIndex index) { for (int j = 0; j < types.size(); j++) { int wholeType = types.get(j); TagValuePair pair = index.decodeType(wholeType); if (pair != null) { int t = wholeType & 3; if (t == MapRenderingTypes.POINT_TYPE) { if (ctx.getRouter().acceptPoint(pair)) { return true; } } else if (t == MapRenderingTypes.POLYLINE_TYPE) { if (ctx.getRouter().acceptLine(pair)) { return true; } } } } return false; } }; SearchRequest<BinaryMapDataObject> request = BinaryMapIndexReader.buildSearchRequest( tileX << zoomToLoad, (tileX + 1) << zoomToLoad, tileY << zoomToLoad, (tileY + 1) << zoomToLoad, 15, searchFilter); for (BinaryMapIndexReader r : map) { r.searchMapIndex(request); for (BinaryMapDataObject o : request.getSearchResults()) { BinaryMapDataObject old = ctx.idObjects.get(o.getId()); // sometimes way are presented only partially in one index if (old != null && old.getPointsLength() >= o.getPointsLength()) { continue; } ctx.idObjects.put(o.getId(), o); for (int j = 0; j < o.getPointsLength(); j++) { long l = (((long) o.getPoint31XTile(j)) << 31) + (long) o.getPoint31YTile(j); RouteSegment segment = new RouteSegment(); segment.road = o; segment.segmentEnd = segment.segmentStart = j; if (ctx.routes.get(l) != null) { segment.next = ctx.routes.get(l); } ctx.routes.put(l, segment); } } ctx.loadedTiles.add(tileC); ctx.timeToLoad += (System.nanoTime() - now); } }
private RouteSegment processIntersectionsWithWays( RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments, TLongObjectHashMap<RouteSegment> visitedSegments, TLongObjectHashMap<RouteSegment> oppositeSegments, double distOnRoadToPass, double distToFinalPoint, RouteSegment segment, BinaryMapDataObject road, boolean firstOfSegment, int segmentEnd, RouteSegment inputNext, boolean reverseWay) { // This variables can be in routing context // initialize temporary lists to calculate not forbidden ways at way intersections ArrayList<RouteSegment> segmentsToVisitPrescripted = new ArrayList<RouteSegment>(5); ArrayList<RouteSegment> segmentsToVisitNotForbidden = new ArrayList<RouteSegment>(5); // collect time for obstacles double obstaclesTime = 0; boolean exclusiveRestriction = false; // 3.1 calculate time for obstacles (bumps, traffic_signals, level_crossing) if (firstOfSegment) { RouteSegment possibleObstacle = inputNext; while (possibleObstacle != null) { obstaclesTime += ctx.getRouter().defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart); possibleObstacle = possibleObstacle.next; } } // 3.2 calculate possible ways to put into priority queue // for debug next.road.getId() >> 1 == 33911427 && road.getId() >> 1 == 33911442 RouteSegment next = inputNext; while (next != null) { long nts = (next.road.getId() << 8l) + next.segmentStart; boolean oppositeConnectionFound = oppositeSegments.containsKey(nts) && oppositeSegments.get(nts) != null; boolean processRoad = true; if (ctx.isUseStrategyOfIncreasingRoadPriorities()) { double roadPriority = ctx.getRouter().getRoadPriorityHeuristicToIncrease(segment.road); double nextRoadPriority = ctx.getRouter().getRoadPriorityHeuristicToIncrease(segment.road); if (nextRoadPriority < roadPriority) { processRoad = false; } } /* next.road.getId() >> 1 (3) != road.getId() >> 1 (3) - used that line for debug with osm map */ // road.id could be equal on roundabout, but we should accept them if ((!visitedSegments.contains(nts) && processRoad) || oppositeConnectionFound) { int type = -1; if (!reverseWay) { for (int i = 0; i < road.getRestrictionCount(); i++) { if (road.getRestriction(i) == next.road.getId()) { type = road.getRestrictionType(i); break; } } } else { for (int i = 0; i < next.road.getRestrictionCount(); i++) { if (next.road.getRestriction(i) == road.getId()) { type = next.road.getRestrictionType(i); break; } // Check if there is restriction only to the current road if (next.road.getRestrictionType(i) == MapRenderingTypes.RESTRICTION_ONLY_RIGHT_TURN || next.road.getRestrictionType(i) == MapRenderingTypes.RESTRICTION_ONLY_LEFT_TURN || next.road.getRestrictionType(i) == MapRenderingTypes.RESTRICTION_ONLY_STRAIGHT_ON) { // check if that restriction applies to considered junk RouteSegment foundNext = inputNext; while (foundNext != null && foundNext.getRoad().getId() != next.road.getRestriction(i)) { 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 { // no restriction can go out if (oppositeConnectionFound) { RouteSegment oppSegment = oppositeSegments.get(nts); oppSegment.segmentEnd = next.segmentStart; return oppSegment; } double distanceToEnd = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed(); if (ctx.isUseDynamicRoadPrioritising()) { double priority = ctx.getRouter().getRoadPriorityToCalculateRoute(next.road); distanceToEnd /= priority; } // Using A* routing algorithm // g(x) - calculate distance to that point and calculate time double speed = ctx.getRouter().defineSpeed(road); if (speed == 0) { speed = ctx.getRouter().getMinDefaultSpeed(); } double distanceFromStart = segment.distanceFromStart + distOnRoadToPass / speed; // calculate turn time distanceFromStart += ctx.getRouter().calculateTurnTime(segment, next, segmentEnd); // add obstacles time distanceFromStart += obstaclesTime; // segment.getRoad().getId() >> 1 if (next.parentRoute == null || ctx.roadPriorityComparator( next.distanceFromStart, next.distanceToEnd, distanceFromStart, distanceToEnd) > 0) { next.distanceFromStart = distanceFromStart; next.distanceToEnd = distanceToEnd; if (next.parentRoute != null) { // already in queue remove it graphSegments.remove(next); } // put additional information to recover whole route after next.parentRoute = segment; next.parentSegmentEnd = segmentEnd; if (type == -1) { // case no restriction 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 main "in" roads and one "out" // 2. in case we are going forward we have one "in" and many "out" if (!reverseWay) { exclusiveRestriction = true; segmentsToVisitNotForbidden.clear(); segmentsToVisitPrescripted.add(next); } else { segmentsToVisitNotForbidden.add(next); } } } } } next = next.next; } // add all allowed route segments to priority queue for (RouteSegment s : segmentsToVisitNotForbidden) { graphSegments.add(s); } for (RouteSegment s : segmentsToVisitPrescripted) { graphSegments.add(s); } return null; }
private RoutePair processRouteSegment( final RoutingContext ctx, RouteSegment end, 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 BinaryMapDataObject road = segment.road; final int middle = segment.segmentStart; int middlex = road.getPoint31XTile(middle); int middley = road.getPoint31YTile(middle); // 0. mark route segment as visited long nt = (road.getId() << 8l) + middle; // avoid empty segments to connect but mark the point as visited visitedSegments.put(nt, null); if (oppositeSegments.contains(nt) && oppositeSegments.get(nt) != null) { segment.segmentEnd = middle; RouteSegment opposite = oppositeSegments.get(nt); opposite.segmentEnd = middle; return new RoutePair(segment, opposite); } boolean oneway = ctx.getRouter().isOneWay(road); boolean minusAllowed = !oneway || reverseWaySearch; boolean plusAllowed = !oneway || !reverseWaySearch; // +/- diff from middle point int d = plusAllowed ? 1 : -1; // Go through all point of the way and find ways to continue // ! Actually there is small bug when there is restriction to move forward on way (it doesn't // take into account) while (minusAllowed || plusAllowed) { // 1. calculate point not equal to middle // (algorithm should visit all point on way if it is not oneway) int segmentEnd = middle + d; if (!minusAllowed && d > 0) { d++; } else if (!plusAllowed && d < 0) { d--; } else { if (d <= 0) { d = -d + 1; } else { d = -d; } } if (segmentEnd < 0) { minusAllowed = false; continue; } if (segmentEnd >= road.getPointsLength()) { plusAllowed = false; continue; } // if we found end point break cycle long nts = (road.getId() << 8l) + segmentEnd; if (oppositeSegments.contains(nts) && oppositeSegments.get(nt) != null) { segment.segmentEnd = segmentEnd; RouteSegment opposite = oppositeSegments.get(nts); opposite.segmentEnd = segmentEnd; return new RoutePair(segment, opposite); } 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); loadRoutes( ctx, (x >> (31 - ctx.getZoomToLoadTileWithRoads())), (y >> (31 - ctx.getZoomToLoadTileWithRoads()))); long l = (((long) x) << 31) + (long) y; RouteSegment next = ctx.routes.get(l); // 3. get intersected ways if (next != null) { double distOnRoadToPass = squareRootDist(x, y, middlex, middley); double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY); RouteSegment foundIntersection = processIntersectionsWithWays( ctx, graphSegments, visitedSegments, oppositeSegments, distOnRoadToPass, distToFinalPoint, segment, road, d == 0, segmentEnd, next, reverseWaySearch); if (foundIntersection != null) { segment.segmentEnd = segmentEnd; return new RoutePair(segment, foundIntersection); } } } return null; }
/** * Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm) return list * of segments */ public List<RouteSegmentResult> searchRoute( final RoutingContext ctx, RouteSegment start, RouteSegment end) throws IOException { // measure time ctx.timeToLoad = 0; ctx.visitedSegments = 0; long startNanoTime = System.nanoTime(); // 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>(); int targetEndX = end.road.getPoint31XTile(end.segmentStart); int targetEndY = end.road.getPoint31YTile(end.segmentStart); int startX = start.road.getPoint31XTile(start.segmentStart); int startY = start.road.getPoint31YTile(start.segmentStart); // for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start) start.distanceToEnd = squareRootDist(startX, startY, targetEndX, targetEndY) / ctx.getRouter().getMaxDefaultSpeed(); end.distanceToEnd = start.distanceToEnd; // because first point of the start is not visited do the same as in cycle but only for one // point // it matters when start point is intersection of different roads // add start segment to priority queue visitAllStartSegments(ctx, start, graphDirectSegments, visitedDirectSegments, startX, startY); visitAllStartSegments( ctx, end, graphReverseSegments, visitedOppositeSegments, targetEndX, targetEndY); // final segment before end RouteSegment finalDirectRoute = null; RouteSegment finalReverseRoute = null; // Extract & analyze segment with min(f(x)) from queue while final segment is not found boolean inverse = false; PriorityQueue<RouteSegment> graphSegments = inverse ? graphReverseSegments : graphDirectSegments; while (!graphSegments.isEmpty()) { RouteSegment segment = graphSegments.poll(); ctx.visitedSegments++; // for debug purposes if (ctx.visitor != null) { ctx.visitor.visitSegment(segment); } if (!inverse) { RoutePair pair = processRouteSegment( ctx, end, false, graphDirectSegments, visitedDirectSegments, targetEndX, targetEndY, segment, visitedOppositeSegments); if (pair != null) { finalDirectRoute = pair.a; finalReverseRoute = pair.b; break; } } else { RoutePair pair = processRouteSegment( ctx, start, true, graphReverseSegments, visitedOppositeSegments, startX, startY, segment, visitedDirectSegments); if (pair != null) { finalReverseRoute = pair.a; finalDirectRoute = pair.b; break; } } if (graphReverseSegments.isEmpty() || graphDirectSegments.isEmpty()) { break; } if (ctx.planRouteIn2Directions()) { inverse = nonHeuristicSegmentsComparator.compare( graphDirectSegments.peek(), graphReverseSegments.peek()) > 0; // make it more simmetrical with dynamic prioritizing it makes big sense // inverse = !inverse; } else { // different strategy : use onedirectional graph inverse = !ctx.getPlanRoadDirection().booleanValue(); } graphSegments = inverse ? graphReverseSegments : graphDirectSegments; } // 4. Route is found : collect all segments and prepare result return prepareResult(ctx, start, end, startNanoTime, finalDirectRoute, finalReverseRoute); }
public RouteSegment findRouteSegment(double lat, double lon, RoutingContext ctx) throws IOException { double tileX = MapUtils.getTileNumberX(ctx.getZoomToLoadTileWithRoads(), lon); double tileY = MapUtils.getTileNumberY(ctx.getZoomToLoadTileWithRoads(), lat); loadRoutes(ctx, (int) tileX, (int) tileY); RouteSegment road = null; double sdist = 0; int px = MapUtils.get31TileNumberX(lon); int py = MapUtils.get31TileNumberY(lat); for (BinaryMapDataObject r : ctx.values()) { if (r.getPointsLength() > 1) { double priority = ctx.getRouter().getRoadPriorityToCalculateRoute(r); for (int j = 1; j < r.getPointsLength(); j++) { double mDist = squareRootDist( r.getPoint31XTile(j), r.getPoint31YTile(j), r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1)); double projection = calculateProjection( r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), r.getPoint31XTile(j), r.getPoint31YTile(j), px, py, mDist); double currentsDist; if (projection < 0) { // TODO: first 2 and last 2 points of a route should be only near and not based // on road priority (I.E. a motorway road node unreachable near my house) currentsDist = squareDist(r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), px, py) / (priority * priority); } else if (projection > mDist) { currentsDist = squareDist(r.getPoint31XTile(j), r.getPoint31YTile(j), px, py) / (priority * priority); } else { currentsDist = calculatesquareDistance( r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), r.getPoint31XTile(j), r.getPoint31YTile(j), px, py, mDist) / (priority * priority); } if (road == null || currentsDist < sdist) { road = new RouteSegment(); road.road = r; road.segmentStart = j - 1; // TODO: first 2 and last 2 segments should be based on projection. my // start/finish point S/F, fake point P between j-1 & j -> SP, PJ; should // end at finish point: JP,PF road.segmentEnd = j; sdist = currentsDist; } } } } return road; }