Fix critical issue with route recalculation

DataExtractionOSM/src/net/osmand/router/BinaryRoutePlanner.java

@@ -166,16 +166,46 @@ public class BinaryRoutePlanner {
 	}
 	
 	public List<RouteSegmentResult> searchRoute(final RoutingContext ctx, RouteSegment start, RouteSegment end, List<RouteSegment> intermediate, boolean leftSideNavigation) throws IOException {
-		// TODO
+		if(intermediate != null && intermediate.size() > 0) {
+			// TODO previously calculated route
+			ArrayList<RouteSegment> ps = new ArrayList<RouteSegment>(intermediate);
+			ps.add(end);
+			ps.add(0, start);
+			List<RouteSegmentResult> results = new ArrayList<RouteSegmentResult>();
+			for (int i = 0; i < ps.size() - 1; i++) {
+				RoutingContext local = new RoutingContext(ctx.config);
+				local.visitor = ctx.visitor;
+				List<RouteSegmentResult> res = searchRouteInternal(local, ps.get(i), ps.get(i + 1), leftSideNavigation);
+				results.addAll(res);
+				ctx.distinctLoadedTiles += local.distinctLoadedTiles;
+				ctx.distinctUnloadedTiles.addAll(local.distinctUnloadedTiles);
+				ctx.loadedTiles += local.loadedTiles;
+				ctx.loadedPrevUnloadedTiles += local.loadedPrevUnloadedTiles;
+				ctx.timeToCalculate += local.timeToCalculate;
+				ctx.timeToLoad += local.timeToLoad;
+				ctx.relaxedSegments += local.relaxedSegments;
+			}
+			printResults(ctx, start, end, results);
+			return results;
+		}
 		return searchRoute(ctx, start, end, leftSideNavigation);
 	}
 	
+	public List<RouteSegmentResult> searchRoute(final RoutingContext ctx, RouteSegment start, RouteSegment end, boolean leftSideNavigation) throws IOException {
+		List<RouteSegmentResult> result = searchRouteInternal(ctx, start, end, leftSideNavigation);
+		if(result != null) {
+			printResults(ctx, start, end, result);
+		}
+		
+		return result;
+	}
+	
 	
 	/**
 	 * 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, boolean leftSideNavigation) throws IOException {
+	public List<RouteSegmentResult> searchRouteInternal(final RoutingContext ctx, RouteSegment start, RouteSegment end, boolean leftSideNavigation) throws IOException {
 		// measure time
 		ctx.timeToLoad = 0;
 		ctx.visitedSegments = 0;
@@ -318,10 +348,10 @@ public class BinaryRoutePlanner {
 			}
 		}
 		println("Result is found");
+		printDebugMemoryInformation(ctx, graphDirectSegments, graphReverseSegments, visitedDirectSegments, visitedOppositeSegments);
 		
 		// 4. Route is found : collect all segments and prepare result
 		List<RouteSegmentResult> resultPrepared = prepareResult(ctx, start, end, leftSideNavigation);
-		printDebugMemoryInformation(ctx, graphDirectSegments, graphReverseSegments, visitedDirectSegments, visitedOppositeSegments);
 		Object[] vls = ctx.tiles.values();
 		for(Object tl : vls) {
 			ctx.unloadTile((RoutingTile) tl, false);
@@ -727,9 +757,9 @@ public class BinaryRoutePlanner {
 						ctx.finalReverseEndSegment = next.segmentStart;
 						ctx.finalReverseRoute = opposite;
 					}
-				}
 					return true;
 				}
+			}
 			// road.id could be equal on roundabout, but we should accept them
 			boolean alreadyVisited = visitedSegments.contains(nts);
 			if (!alreadyVisited) {
@@ -888,6 +918,11 @@ public class BinaryRoutePlanner {
 			
 		}
 		addTurnInfo(leftside, result);
+		return result;
+	}
+
+
+	private void printResults(RoutingContext ctx, RouteSegment start, RouteSegment end, List<RouteSegmentResult> result) {
 		float completeTime = 0;
 		float completeDistance = 0;
 		for(RouteSegmentResult r : result) {
@@ -936,7 +971,6 @@ public class BinaryRoutePlanner {
 			}
 		}
 		println("</test>");
-		return result;
 	}
 
 

DataExtractionOSM/src/net/osmand/swing/MapRouterLayer.java

@@ -66,6 +66,7 @@ public class MapRouterLayer implements MapPanelLayer {
 	private MapPanel map;
 	private LatLon startRoute ;
 	private LatLon endRoute ;
+	private List<LatLon> intermediates = new ArrayList<LatLon>();
 	private boolean nextAvailable = true;
 	private boolean pause = true;
 	private boolean stop = false;
@@ -174,7 +175,7 @@ public class MapRouterLayer implements MapPanelLayer {
 				new Thread() {
 					@Override
 					public void run() {
-						List<Way> ways = selfRoute(startRoute, endRoute, null);
+						List<Way> ways = selfRoute(startRoute, endRoute, intermediates, null);
 						if (ways != null) {
 							DataTileManager<Way> points = new DataTileManager<Way>();
 							points.setZoom(11);
@@ -203,7 +204,7 @@ public class MapRouterLayer implements MapPanelLayer {
 				new Thread() {
 					@Override
 					public void run() {
-						List<Way> ways = selfRoute(startRoute, endRoute, previousRoute);
+						List<Way> ways = selfRoute(startRoute, endRoute, intermediates,  previousRoute);
 						if (ways != null) {
 							DataTileManager<Way> points = new DataTileManager<Way>();
 							points.setZoom(11);
@@ -274,6 +275,32 @@ public class MapRouterLayer implements MapPanelLayer {
 			}
 		};
 		menu.add(swapLocations);
+		Action addIntermediate = new AbstractAction("Add transit point") {
+
+			@Override
+			public void actionPerformed(ActionEvent e) {
+				Point popupMenuPoint = map.getPopupMenuPoint();
+				double fy = (popupMenuPoint.y - map.getCenterPointY()) / map.getTileSize();
+				double fx = (popupMenuPoint.x - map.getCenterPointX()) / map.getTileSize();
+				double latitude = MapUtils.getLatitudeFromTile(map.getZoom(), map.getYTile() + fy);
+				double longitude = MapUtils.getLongitudeFromTile(map.getZoom(), map.getXTile() + fx);
+				intermediates.add(new LatLon(latitude, longitude));
+				map.repaint();
+			}
+		};
+		menu.add(addIntermediate);
+		
+		Action remove = new AbstractAction("Remove transit point") {
+
+			@Override
+			public void actionPerformed(ActionEvent e) {
+				if(intermediates.size() > 0){
+					intermediates.remove(0);
+				}
+				map.repaint();
+			}
+		};
+		menu.add(remove);
 
 	}
 	
@@ -551,7 +578,7 @@ public class MapRouterLayer implements MapPanelLayer {
 		return res;
 	}
 	
-	public List<Way> selfRoute(LatLon start, LatLon end, List<RouteSegmentResult> previousRoute) {
+	public List<Way> selfRoute(LatLon start, LatLon end, List<LatLon> intermediates, List<RouteSegmentResult> previousRoute) {
 		List<Way> res = new ArrayList<Way>();
 		long time = System.currentTimeMillis();
 		List<File> files = new ArrayList<File>();
@@ -619,6 +646,19 @@ public class MapRouterLayer implements MapPanelLayer {
 				}
 				System.out.println("ROAD TO END " + e.getRoad().getHighway() + " " + e.getRoad().id);
 				
+				List<RouteSegment> inters  = new ArrayList<BinaryRoutePlanner.RouteSegment>();
+				if (intermediates != null) {
+					int ind = 1;
+					for (LatLon il : intermediates) {
+						RouteSegment is = router.findRouteSegment(il.getLatitude(), il.getLongitude(), ctx);
+						if (is == null) {
+							throw new RuntimeException("Intremediate point "+ind+" was not found.");
+						}
+						inters.add(is);
+						ind++;
+					}
+				}
+				
 				final DataTileManager<Entity> points = new DataTileManager<Entity>();
 				points.setZoom(11);
 				map.setPoints(points);
@@ -675,7 +715,7 @@ public class MapRouterLayer implements MapPanelLayer {
 
 				});
 				
-				List<RouteSegmentResult> searchRoute = router.searchRoute(ctx, st, e, false);
+				List<RouteSegmentResult> searchRoute = router.searchRoute(ctx, st, e, inters, false);
 				this.previousRoute = searchRoute;
 				if (animateRoutingCalculation) {
 					playPauseButton.setVisible(false);
@@ -792,6 +832,13 @@ public class MapRouterLayer implements MapPanelLayer {
 			g.drawOval(x, y, 12, 12);
 			g.fillOval(x, y, 12, 12);
 		}
+		g.setColor(Color.yellow);
+		for(LatLon i : intermediates) {
+			int x = map.getMapXForPoint(i.getLongitude());
+			int y = map.getMapYForPoint(i.getLatitude());
+			g.drawOval(x, y, 12, 12);
+			g.fillOval(x, y, 12, 12);
+		}
 	}
 
 }

OsmAnd/src/net/osmand/plus/routing/RouteProvider.java

@@ -369,6 +369,7 @@ public class RouteProvider {
 				if (is == null) {
 					return new RouteCalculationResult(app.getString(R.string.intermediate_point_too_far, "'" + ind + "'"));
 				}
+				inters.add(is);
 				ind++;
 			}
 		}