406 lines
15 KiB
Java
406 lines
15 KiB
Java
|
package net.osmand.router;
|
||
|
|
|
||
|
|
import gnu.trove.iterator.TIntIterator;
|
||
|
|
import gnu.trove.list.array.TIntArrayList;
|
||
|
|
import gnu.trove.map.hash.TIntObjectHashMap;
|
||
|
|
import gnu.trove.map.hash.TLongObjectHashMap;
|
||
|
|
import gnu.trove.set.hash.TLongHashSet;
|
||
|
|
|
||
|
|
import java.io.File;
|
||
|
|
import java.io.FileNotFoundException;
|
||
|
|
import java.io.IOException;
|
||
|
|
import java.io.RandomAccessFile;
|
||
|
|
import java.util.ArrayList;
|
||
|
|
import java.util.Comparator;
|
||
|
|
import java.util.LinkedHashMap;
|
||
|
|
import java.util.List;
|
||
|
|
import java.util.Map;
|
||
|
|
import java.util.PriorityQueue;
|
||
|
|
|
||
|
|
import net.osmand.binary.BinaryMapIndexReader;
|
||
|
|
import net.osmand.binary.RouteDataObject;
|
||
|
|
import net.osmand.binary.BinaryMapIndexReader.SearchRequest;
|
||
|
|
import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteSubregion;
|
||
|
|
import net.osmand.data.LatLon;
|
||
|
|
import net.osmand.data.QuadRect;
|
||
|
|
import net.osmand.data.QuadTree;
|
||
|
|
import net.osmand.data.TransportRoute;
|
||
|
|
import net.osmand.data.TransportStop;
|
||
|
|
import net.osmand.router.BinaryRoutePlanner.FinalRouteSegment;
|
||
|
|
import net.osmand.router.BinaryRoutePlanner.RouteSegment;
|
||
|
|
import net.osmand.router.BinaryRoutePlanner.RouteSegmentPoint;
|
||
|
|
import net.osmand.util.MapUtils;
|
||
|
|
|
||
|
|
public class TransportRoutePlanner {
|
||
|
|
|
||
|
|
public void searchRouteInternal(final TransportRoutingContext ctx, TransportRouteSegment start, TransportRouteSegment end,
|
||
|
|
RouteSegment recalculationEnd) throws InterruptedException, IOException {
|
||
|
|
Comparator<TransportRouteSegment> nonHeuristicSegmentsComparator = new NonHeuristicSegmentsComparator();
|
||
|
|
PriorityQueue<TransportRouteSegment> graphDirectSegments = new PriorityQueue<TransportRouteSegment>(50, new SegmentsComparator(
|
||
|
|
ctx));
|
||
|
|
PriorityQueue<TransportRouteSegment> graphReverseSegments = new PriorityQueue<TransportRouteSegment>(50, new SegmentsComparator(
|
||
|
|
ctx));
|
||
|
|
// Set to not visit one segment twice (stores road.id << X + segmentStart)
|
||
|
|
TLongObjectHashMap<TransportRouteSegment> visitedDirectSegments = new TLongObjectHashMap<TransportRouteSegment>();
|
||
|
|
TLongObjectHashMap<TransportRouteSegment> visitedOppositeSegments = new TLongObjectHashMap<TransportRouteSegment>();
|
||
|
|
|
||
|
|
|
||
|
|
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
|
||
|
|
boolean forwardSearch = true;
|
||
|
|
|
||
|
|
PriorityQueue<TransportRouteSegment> graphSegments = graphDirectSegments;
|
||
|
|
|
||
|
|
FinalRouteSegment finalSegment = null;
|
||
|
|
boolean onlyBackward = ctx.getPlanRoadDirection() < 0;
|
||
|
|
boolean onlyForward = ctx.getPlanRoadDirection() > 0;
|
||
|
|
while (!graphSegments.isEmpty()) {
|
||
|
|
TransportRouteSegment segment = graphSegments.poll();
|
||
|
|
// TODO break when it is final segment
|
||
|
|
// if (segment instanceof FinalRouteSegment) {
|
||
|
|
// finalSegment = (FinalRouteSegment) segment;
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
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);
|
||
|
|
} 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");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
private void checkIfGraphIsEmpty(TransportRoutingContext ctx, boolean b,
|
||
|
|
PriorityQueue<TransportRouteSegment> graphReverseSegments, TransportRouteSegment end,
|
||
|
|
TLongObjectHashMap<TransportRouteSegment> visitedOppositeSegments, String string) {
|
||
|
|
// TODO Auto-generated method stub
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
private void updateCalculationProgress(TransportRoutingContext ctx,
|
||
|
|
PriorityQueue<TransportRouteSegment> graphDirectSegments, PriorityQueue<TransportRouteSegment> graphReverseSegments) {
|
||
|
|
// TODO Auto-generated method stub
|
||
|
|
}
|
||
|
|
|
||
|
|
private static int sleft = MapUtils.get31TileNumberX(4.7495);
|
||
|
|
private static int sright = MapUtils.get31TileNumberX(4.8608);
|
||
|
|
private static int stop = MapUtils.get31TileNumberY(52.3395);
|
||
|
|
private static int sbottom = MapUtils.get31TileNumberY(52.2589);
|
||
|
|
private static int szoom = 15;
|
||
|
|
|
||
|
|
@SuppressWarnings("unused")
|
||
|
|
private void processRouteSegment(final TransportRoutingContext ctx, boolean reverseWaySearch,
|
||
|
|
PriorityQueue<TransportRouteSegment> graphSegments, TLongObjectHashMap<TransportRouteSegment> visitedSegments,
|
||
|
|
TransportRouteSegment segment, TLongObjectHashMap<TransportRouteSegment> oppositeSegments, boolean doNotAddIntersections) throws IOException {
|
||
|
|
final TransportRoute road = segment.road;
|
||
|
|
BinaryMapIndexReader reader = ctx.map.keySet().iterator().next();
|
||
|
|
List<TransportStop> transportStops = reader.searchTransportIndex(BinaryMapIndexReader.buildSearchTransportRequest(sleft, sright, stop, sbottom,
|
||
|
|
-1, null));
|
||
|
|
for(TransportStop s : transportStops) {
|
||
|
|
int[] referencesToRoutes = s.getReferencesToRoutes();
|
||
|
|
TIntObjectHashMap<TransportRoute> routes = reader.getTransportRoutes(referencesToRoutes);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
public static void main(String[] args) throws IOException {
|
||
|
|
File fl = new File(System.getProperty("maps.dir"), "Netherlands_amstelveen.obf");
|
||
|
|
RandomAccessFile raf = new RandomAccessFile(fl, "r");
|
||
|
|
BinaryMapIndexReader reader = new BinaryMapIndexReader(raf, fl);
|
||
|
|
|
||
|
|
LatLon start = new LatLon(52.28094, 4.853248);
|
||
|
|
LatLon end = new LatLon(52.320988, 4.87256);
|
||
|
|
TransportRoutingContext ctx = new TransportRoutingContext(reader);
|
||
|
|
TransportRoutePlanner planner = new TransportRoutePlanner();
|
||
|
|
planner.buildRoute(ctx, start, end);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
private void buildRoute(TransportRoutingContext ctx, LatLon start, LatLon end) throws IOException {
|
||
|
|
double limit = 1000;
|
||
|
|
BinaryMapIndexReader reader = ctx.map.keySet().iterator().next();
|
||
|
|
|
||
|
|
List<TransportRouteSegment> startStops = ctx.getTransportStops(start);
|
||
|
|
List<TransportRouteSegment> endStops = ctx.getTransportStops(end);
|
||
|
|
|
||
|
|
TLongObjectHashMap<TransportRouteSegment> endSegments = new TLongObjectHashMap<TransportRouteSegment>();
|
||
|
|
for(TransportRouteSegment s : endStops) {
|
||
|
|
endSegments.put(s.getId(), s);
|
||
|
|
}
|
||
|
|
PriorityQueue<TransportRouteSegment> queue = new PriorityQueue<TransportRoutePlanner.TransportRouteSegment>();
|
||
|
|
queue.addAll(startStops);
|
||
|
|
while(!queue.isEmpty()) {
|
||
|
|
TransportRouteSegment segment = queue.poll();
|
||
|
|
long l = segment.getId();
|
||
|
|
for (int i = 0; i < segment.getLength() - segment.segStart; i++) {
|
||
|
|
TransportRouteSegment finish = endSegments.get(l+i);
|
||
|
|
if(finish != null){
|
||
|
|
System.out.println(segment + " " + finish);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
private static class SegmentsComparator implements Comparator<TransportRouteSegment> {
|
||
|
|
final TransportRoutingContext ctx;
|
||
|
|
|
||
|
|
public SegmentsComparator(TransportRoutingContext ctx) {
|
||
|
|
this.ctx = ctx;
|
||
|
|
}
|
||
|
|
|
||
|
|
@Override
|
||
|
|
public int compare(TransportRouteSegment o1, TransportRouteSegment o2) {
|
||
|
|
return BinaryRoutePlanner.roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd,
|
||
|
|
o2.distanceFromStart, o2.distanceToEnd, ctx.config.heuristicCoefficient);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private static class NonHeuristicSegmentsComparator implements Comparator<TransportRouteSegment> {
|
||
|
|
public NonHeuristicSegmentsComparator() {
|
||
|
|
}
|
||
|
|
|
||
|
|
@Override
|
||
|
|
public int compare(TransportRouteSegment o1, TransportRouteSegment o2) {
|
||
|
|
return BinaryRoutePlanner.
|
||
|
|
roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public RouteSegment initRouteSegment(final TransportRoutingContext ctx, TransportRouteSegment 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) {
|
||
|
|
// // assymetric cause we calculate initial point differently (segmentStart means that point is between ]segmentStart-1, segmentStart]
|
||
|
|
// } else if (segment.getSegmentStart() > 0 && positiveDirection) {
|
||
|
|
// segment = loadSameSegment(ctx, segment, segment.getSegmentStart() - 1);
|
||
|
|
// }
|
||
|
|
// if (segment == null) {
|
||
|
|
// return null;
|
||
|
|
// }
|
||
|
|
return null;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static class TransportRouteSegment {
|
||
|
|
public double distanceToEnd;
|
||
|
|
final short segStart;
|
||
|
|
final TransportRoute road;
|
||
|
|
private static final int SHIFT = 10; // assume less than 1024 stops
|
||
|
|
|
||
|
|
TransportRouteSegment parentRoute = null;
|
||
|
|
float distanceFromStart = 0;
|
||
|
|
|
||
|
|
public TransportRouteSegment(TransportRoute road, int stopIndex) {
|
||
|
|
this.road = road;
|
||
|
|
this.segStart = (short) stopIndex;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
public int getLength() {
|
||
|
|
return road.getForwardStops().size();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
public long getId() {
|
||
|
|
long l = road.getId() << SHIFT;
|
||
|
|
if(l < 0 ) {
|
||
|
|
throw new IllegalStateException("too long id " + road.getId());
|
||
|
|
}
|
||
|
|
if(segStart >= (1 << SHIFT)) {
|
||
|
|
throw new IllegalStateException("too many stops " + road.getId() + " " + segStart);
|
||
|
|
}
|
||
|
|
return l + segStart;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
public int getDepth() {
|
||
|
|
if(parentRoute != null) {
|
||
|
|
return parentRoute.getDepth() + 1;
|
||
|
|
}
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
@Override
|
||
|
|
public String toString() {
|
||
|
|
return String.format("Route stop: %s %s", road, road.getForwardStops().get(segStart));
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
public static class TransportRoutingContext {
|
||
|
|
|
||
|
|
public final int walkRadius = 5000; // meters from start/end/intermediate locations
|
||
|
|
public int searchThreshold = 10; // don't search stops beyond searchThreshold*dist(start, end) radius
|
||
|
|
|
||
|
|
public int visitedSegments;
|
||
|
|
public RoutingConfiguration config;
|
||
|
|
public RouteCalculationProgress calculationProgress;
|
||
|
|
public TLongObjectHashMap<TransportStop> visitedTransportStops = new TLongObjectHashMap<TransportStop>();
|
||
|
|
private TIntArrayList visitedTiles = new TIntArrayList();
|
||
|
|
private static final int ZOOM_TO_LOAD_TILES = 14;
|
||
|
|
|
||
|
|
public QuadTree<TransportRouteSegment> quadTree =
|
||
|
|
new QuadTree<TransportRouteSegment>(new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE), 8, 0.55f);
|
||
|
|
public final Map<BinaryMapIndexReader, TIntObjectHashMap<TransportRoute>> map =
|
||
|
|
new LinkedHashMap<BinaryMapIndexReader, TIntObjectHashMap<TransportRoute>>();
|
||
|
|
|
||
|
|
private final int walkRaidusIn31;
|
||
|
|
|
||
|
|
|
||
|
|
public TransportRoutingContext(BinaryMapIndexReader... readers) {
|
||
|
|
walkRaidusIn31 = (int) (walkRadius / MapUtils.getTileDistanceWidth(31));
|
||
|
|
for (BinaryMapIndexReader r : readers) {
|
||
|
|
map.put(r, new TIntObjectHashMap<TransportRoute>());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public List<TransportRouteSegment> getTransportStops(LatLon start) throws IOException {
|
||
|
|
int y = MapUtils.get31TileNumberY(start.getLatitude());
|
||
|
|
int x = MapUtils.get31TileNumberX(start.getLongitude());
|
||
|
|
return getTransportStops(x, y);
|
||
|
|
}
|
||
|
|
|
||
|
|
public List<TransportRouteSegment> getTransportStops(int x, int y) throws IOException {
|
||
|
|
return loadNativeTransportStops(x, y);
|
||
|
|
}
|
||
|
|
|
||
|
|
private List<TransportRouteSegment> loadNativeTransportStops(int sx, int sy) throws IOException {
|
||
|
|
List<TransportRouteSegment> allstops = new ArrayList<TransportRouteSegment>();
|
||
|
|
int lx = (sx - walkRaidusIn31 ) >> (31 - ZOOM_TO_LOAD_TILES);
|
||
|
|
int rx = (sx + walkRaidusIn31 ) >> (31 - ZOOM_TO_LOAD_TILES);
|
||
|
|
int ty = (sy - walkRaidusIn31 ) >> (31 - ZOOM_TO_LOAD_TILES);
|
||
|
|
int by = (sy + walkRaidusIn31 ) >> (31 - ZOOM_TO_LOAD_TILES);
|
||
|
|
for(int x = lx; x <= rx; x++) {
|
||
|
|
for(int y = ty; y <= by; y++) {
|
||
|
|
loadTile(x, y);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
quadTree.queryInBox(new QuadRect(sx - walkRaidusIn31, sy - walkRaidusIn31, sx + walkRaidusIn31,
|
||
|
|
sy + walkRadius), allstops);
|
||
|
|
return allstops;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
private void loadTile(int x, int y) throws IOException {
|
||
|
|
int tileId = x << (ZOOM_TO_LOAD_TILES + 1) + y;
|
||
|
|
if(visitedTiles.contains(tileId)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
int pz = (31 - ZOOM_TO_LOAD_TILES);
|
||
|
|
SearchRequest<TransportStop> sr = BinaryMapIndexReader.buildSearchTransportRequest(x << pz, (x + 1) << pz,
|
||
|
|
y << pz, (y + 1) << pz, -1, null);
|
||
|
|
TIntArrayList allPoints = new TIntArrayList();
|
||
|
|
TIntArrayList allPointsUnique = new TIntArrayList();
|
||
|
|
for(BinaryMapIndexReader r : map.keySet()) {
|
||
|
|
sr.clearSearchResults();
|
||
|
|
List<TransportStop> stops = r.searchTransportIndex(sr);
|
||
|
|
for(TransportStop s : stops) {
|
||
|
|
if(!visitedTransportStops.contains(s.getId())) {
|
||
|
|
visitedTransportStops.put(s.getId(), s);
|
||
|
|
allPoints.addAll(s.getReferencesToRoutes());
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
makeUnique(allPoints, allPointsUnique);
|
||
|
|
if(allPointsUnique.size() > 0) {
|
||
|
|
loadTransportSegments(allPointsUnique, r, stops);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
visitedTiles.add(tileId);
|
||
|
|
}
|
||
|
|
|
||
|
|
private void loadTransportSegments(TIntArrayList allPointsUnique, BinaryMapIndexReader r,
|
||
|
|
List<TransportStop> stops) throws IOException {
|
||
|
|
TIntObjectHashMap<TransportRoute> routes = r.getTransportRoutes(allPointsUnique.toArray());
|
||
|
|
map.get(r).putAll(routes);
|
||
|
|
for(TransportStop s : stops) {
|
||
|
|
for(int ref : s.getReferencesToRoutes()) {
|
||
|
|
TransportRoute route = routes.get(ref);
|
||
|
|
if(route != null) {
|
||
|
|
int stopIndex = -1;
|
||
|
|
for(int k = 0; k < route.getForwardStops().size(); k++) {
|
||
|
|
TransportStop st = route.getForwardStops().get(k);
|
||
|
|
if(st.x31 == s.x31 && s.y31 == st.y31) {
|
||
|
|
stopIndex = k;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if(stopIndex != -1) {
|
||
|
|
TransportRouteSegment segment = new TransportRouteSegment(route, stopIndex);
|
||
|
|
quadTree.insert(segment, s.x31, s.y31);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void makeUnique(TIntArrayList allPoints, TIntArrayList allPointsUnique) {
|
||
|
|
allPoints.sort();
|
||
|
|
int p = 0;
|
||
|
|
TIntIterator it = allPoints.iterator();
|
||
|
|
while(it.hasNext()) {
|
||
|
|
int nxt = it.next();
|
||
|
|
if(p != nxt) {
|
||
|
|
allPointsUnique.add(nxt);
|
||
|
|
p = nxt;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public int getPlanRoadDirection() {
|
||
|
|
return config.planRoadDirection;
|
||
|
|
}
|
||
|
|
|
||
|
|
public SearchRequest<TransportStop> getBuildRequest(LatLon start) {
|
||
|
|
int sy = MapUtils.get31TileNumberY(start.getLatitude());
|
||
|
|
int sx = MapUtils.get31TileNumberX(start.getLongitude());
|
||
|
|
int w = (int) (walkRadius / MapUtils.getTileDistanceWidth(31));
|
||
|
|
int h = (int) (walkRadius / MapUtils.getTileDistanceWidth(31));
|
||
|
|
return BinaryMapIndexReader.buildSearchTransportRequest(sx - w, sx + w, sy - h, sy + h, -1, null);
|
||
|
|
}
|
||
|
|
|
||
|
|
public VehicleRouter getRouter() {
|
||
|
|
return config.router;
|
||
|
|
}
|
||
|
|
|
||
|
|
public boolean planRouteIn2Directions() {
|
||
|
|
return config.planRoadDirection == 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|