From 34a4b9825e46565407f466336000d79da2f5b8d3 Mon Sep 17 00:00:00 2001 From: Victor Shcherb Date: Wed, 29 Oct 2014 01:56:25 +0100 Subject: [PATCH] Calculate route from center of city --- .../net/osmand/router/BinaryRoutePlanner.java | 63 ++++++++++++++++--- .../osmand/router/RoutePlannerFrontEnd.java | 40 +++++++----- 2 files changed, 79 insertions(+), 24 deletions(-) diff --git a/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java b/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java index 2a1fa3e03c..8d18346a0d 100644 --- a/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java +++ b/OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java @@ -6,6 +6,7 @@ import java.io.IOException; import java.text.MessageFormat; import java.util.Comparator; import java.util.Iterator; +import java.util.List; import java.util.PriorityQueue; import net.osmand.PlatformUtil; @@ -64,7 +65,7 @@ public class BinaryRoutePlanner { * return list of segments */ @SuppressWarnings("unused") - FinalRouteSegment searchRouteInternal(final RoutingContext ctx, RouteSegment start, RouteSegment end) throws InterruptedException, IOException { + FinalRouteSegment searchRouteInternal(final RoutingContext ctx, RouteSegmentPoint start, RouteSegmentPoint end) throws InterruptedException, IOException { // measure time ctx.timeToLoad = 0; ctx.visitedSegments = 0; @@ -128,12 +129,11 @@ public class BinaryRoutePlanner { visitedDirectSegments, doNotAddIntersections); } updateCalculationProgress(ctx, graphDirectSegments, graphReverseSegments); - if(ctx.getPlanRoadDirection() <= 0 && graphReverseSegments.isEmpty()){ - throw new IllegalArgumentException("Route is not found to selected target point."); - } - if(ctx.getPlanRoadDirection() >= 0 && graphDirectSegments.isEmpty()){ - throw new IllegalArgumentException("Route is not found from selected start point."); - } + + 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()) { @@ -167,6 +167,47 @@ public class BinaryRoutePlanner { } + protected void checkIfGraphIsEmpty(final RoutingContext ctx, boolean allowDirection, + PriorityQueue graphSegments, RouteSegmentPoint pnt, TLongObjectHashMap visited, + String msg) { + if (allowDirection && graphSegments.isEmpty()) { + if (pnt.others != null) { + Iterator 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); + } + } + } + } + + private void initQueuesWithStartEnd(final RoutingContext ctx, RouteSegment start, RouteSegment end, PriorityQueue graphDirectSegments, PriorityQueue graphReverseSegments) { RouteSegment startPos = start.initRouteSegment(true); @@ -700,12 +741,16 @@ public class BinaryRoutePlanner { public void visitSegment(RouteSegment segment, int segmentEnd, boolean poll); } - public static class RouteSegmentPoint extends RouteSegment{ - public RouteSegmentPoint(RouteDataObject road, int segmentStart) { + public static class RouteSegmentPoint extends RouteSegment { + public RouteSegmentPoint(RouteDataObject road, int segmentStart, double dist) { super(road, segmentStart); + this.dist = dist; } + + public double dist; public int preciseX; public int preciseY; + public List others; } public static class RouteSegment { diff --git a/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java b/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java index d8e1117edc..9c82c36fa2 100644 --- a/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java +++ b/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java @@ -4,6 +4,8 @@ package net.osmand.router; import java.io.IOException; import java.util.ArrayList; import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; import java.util.List; import net.osmand.NativeLibrary; @@ -14,7 +16,6 @@ import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteRegion; import net.osmand.binary.RouteDataObject; import net.osmand.data.LatLon; import net.osmand.data.QuadPoint; -import net.osmand.router.BinaryRoutePlanner.RouteSegment; import net.osmand.router.BinaryRoutePlanner.RouteSegmentPoint; import net.osmand.util.MapUtils; @@ -59,30 +60,39 @@ public class RoutePlannerFrontEnd { if (dataObjects.isEmpty()) { ctx.loadTileData(px, py, 15, dataObjects); } - RouteSegmentPoint road = null; - - double sdist = 0; + List list = new ArrayList(); for (RouteDataObject r : dataObjects) { if (r.getPointsLength() > 1) { + RouteSegmentPoint road = null; for (int j = 1; j < r.getPointsLength(); j++) { QuadPoint pr = MapUtils.getProjectionPoint31(px, py, r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), r.getPoint31XTile(j ), r.getPoint31YTile(j )); double currentsDist = squareDist((int) pr.x, (int)pr.y, px, py); - if (road == null || currentsDist < sdist) { + if (road == null || currentsDist < road.dist) { RouteDataObject ro = new RouteDataObject(r); - road = new RouteSegmentPoint(ro, j); + road = new RouteSegmentPoint(ro, j, currentsDist); road.preciseX = (int) pr.x; road.preciseY = (int) pr.y; - sdist = currentsDist; } } + if(road != null) { + list.add(road); + } } } -// if (road != null) { - // re-register the best road because one more point was inserted -// ctx.registerRouteDataObject(road.getRoad()); -// } - return road; + Collections.sort(list, new Comparator() { + + @Override + public int compare(RouteSegmentPoint o1, RouteSegmentPoint o2) { + return Double.compare(o1.dist, o2.dist); + } + }); + if(list.size() > 0) { + RouteSegmentPoint ps = list.remove(0); + ps.others = list; + return ps; + } + return null; } @@ -271,7 +281,7 @@ public class RoutePlannerFrontEnd { } - private List searchRouteInternalPrepare(final RoutingContext ctx, RouteSegment start, RouteSegment end, + private List searchRouteInternalPrepare(final RoutingContext ctx, RouteSegmentPoint start, RouteSegmentPoint end, PrecalculatedRouteDirection routeDirection) throws IOException, InterruptedException { ctx.initStartAndTargetPoints(start, end); if(routeDirection != null) { @@ -391,8 +401,8 @@ public class RoutePlannerFrontEnd { } @SuppressWarnings("static-access") - private List searchRoute(final RoutingContext ctx, RouteSegment start, RouteSegment end, PrecalculatedRouteDirection routeDirection) - throws IOException, InterruptedException { + private List searchRoute(final RoutingContext ctx, RouteSegmentPoint start, RouteSegmentPoint end, + PrecalculatedRouteDirection routeDirection) throws IOException, InterruptedException { if(ctx.SHOW_GC_SIZE){ long h1 = ctx.runGCUsedMemory(); float mb = (1 << 20);