Add gpx route approximation

OsmAnd-java/src/main/java/net/osmand/router/RoutePlannerFrontEnd.java

@@ -23,6 +23,8 @@ import net.osmand.util.MapUtils;
 
 import org.apache.commons.logging.Log;
 
+import gnu.trove.list.array.TIntArrayList;
+
 public class RoutePlannerFrontEnd {
 
 	protected static final Log log = PlatformUtil.getLog(RoutePlannerFrontEnd.class);
@@ -31,7 +33,7 @@ public class RoutePlannerFrontEnd {
 	public boolean useSmartRouteRecalculation = true;
 	
 	public static double AVERAGE_SPLIT_DISTANCE_GPX = 500;
-	public static double MINIMUM_SPLIT_DISTANCE_GPX = 100;
+	public static double MINIMUM_SPLIT_DISTANCE_GPX = 30;
 	public static double DISTANCE_APPROXIMATE_THRESHOLD = 50;
 
 	
@@ -151,9 +153,18 @@ public class RoutePlannerFrontEnd {
 		useSmartRouteRecalculation = use;
 	}
 	
-	private static class GpxApproximateContext {
+	public static class GpxApproximationResult {
-		int routeCalculations = 0;
+		public int routeCalculations = 0;
-		double routeDistCalculations = 0;
+		public double routeDistCalculations = 0;
+		public List<RouteSegmentResult> res = new ArrayList<RouteSegmentResult>();
+		public double routeDistance;
+		public double routeDistanceDisconnected;
+		
+		@Override
+		public String toString() {
+			return String.format("GPX approximation (%d of %f m route calcs) for %f m: %f m disconnected",
+					routeCalculations, routeDistCalculations, routeDistance, routeDistanceDisconnected);
+		}
 	}
 	
 	private static class GpxPoint {
@@ -166,11 +177,10 @@ public class RoutePlannerFrontEnd {
 	}
 
 	
-	public List<RouteSegmentResult> searchGpxRoute(final RoutingContext ctx, List<LatLon> points) throws IOException, InterruptedException {
+	public GpxApproximationResult searchGpxRoute(final RoutingContext ctx, List<LatLon> points) throws IOException, InterruptedException {
-		GpxApproximateContext gctx = new GpxApproximateContext();
+		GpxApproximationResult gctx = new GpxApproximationResult();
 		List<GpxPoint> gpxPoints = new ArrayList<>(points.size());
 		GpxPoint prev = null;
-		int firstInd = gpxPoints.size();
 		for(int i = 0; i < points.size(); i++) {
 			GpxPoint p = new GpxPoint();
 			p.ind = i;
@@ -178,15 +188,11 @@ public class RoutePlannerFrontEnd {
 			if (prev != null) {
 				p.cumDist = MapUtils.getDistance(p.loc, prev.loc) + prev.cumDist;
 			}
-			if (firstInd == gpxPoints.size()) {
-				p.pnt = findRouteSegment(p.loc.getLatitude(), p.loc.getLongitude(), ctx, null, false);
-				if(p.pnt != null) {
-					firstInd = i;
-				}
-			}
 			gpxPoints.add(p);
+			gctx.routeDistance = p.cumDist;
 			prev = p;
 		}
+		int firstInd = 0;
 		while (firstInd < gpxPoints.size() - 1) {
 			int nextInd = findGpxRouteFromPoint(ctx, gctx, gpxPoints, firstInd);
 			if(nextInd == -1) {
@@ -195,65 +201,143 @@ public class RoutePlannerFrontEnd {
 				firstInd = nextInd;
 			}
 		}
-		List<RouteSegmentResult> res = new ArrayList<RouteSegmentResult>();
+		TIntArrayList lastStraightLine = null;
-		for (int i = 0; i < gpxPoints.size() - 1; i++) {
+		for (int i = 0; i < gpxPoints.size() - 1; ) {
 			GpxPoint pnt = gpxPoints.get(i);
 			if (pnt.routeToTarget != null && !pnt.routeToTarget.isEmpty()) {
-				res.addAll(pnt.routeToTarget);
+				if (lastStraightLine != null) {
+					addStraightLine(gctx.res, lastStraightLine);
+					lastStraightLine = null;
+				}
+				pnt.routeToTarget.get(0).setTurnType(null);
+				RouteSegmentResult last = pnt.routeToTarget.get(pnt.routeToTarget.size() - 1);
+				last.setTurnType(null);
+				gctx.res.addAll(pnt.routeToTarget);
 				i = pnt.targetInd;
 			} else {
-				// TODO add straight line from i -> i+1 
+				// add straight line from i -> i+1 
-//				res.add(new RouteSegmentResult(new RouteDataObject(null)));
+				if (lastStraightLine == null) {
+					lastStraightLine = new TIntArrayList();
+					lastStraightLine.add(MapUtils.get31TileNumberX(pnt.loc.getLongitude()));
+					lastStraightLine.add(MapUtils.get31TileNumberY(pnt.loc.getLatitude()));
+				}
+				GpxPoint nxt = gpxPoints.get(i + 1);
+				lastStraightLine.add(MapUtils.get31TileNumberX(nxt.loc.getLongitude()));
+				lastStraightLine.add(MapUtils.get31TileNumberY(nxt.loc.getLatitude()));
+				gctx.routeDistanceDisconnected += (nxt.cumDist - pnt.cumDist);
+				i++;
 			}
 		}
-		if (!res.isEmpty()) {
+		if (lastStraightLine != null) {
-			System.out.println(String.format("GPX route approximations was found by %d of %f m route calculations for %f distance",
+			addStraightLine(gctx.res, lastStraightLine);
-					gctx.routeCalculations, gctx.routeDistCalculations, gpxPoints.get(gpxPoints.size() - 1).cumDist));
+			lastStraightLine = null;
-			new RouteResultPreparation().printResults(ctx, points.get(0), points.get(points.size() - 1), res);
 		}
-		return res;
+		// clean turns to recaculate them
+		RouteResultPreparation preparation = new RouteResultPreparation();
+		for (RouteSegmentResult r : gctx.res) {
+			r.setTurnType(null);
+			r.setDescription("");
+		}
+		preparation.prepareTurnResults(ctx, gctx.res);
+
+		if (!gctx.res.isEmpty()) {
+			preparation.printResults(ctx, points.get(0), points.get(points.size() - 1), gctx.res);
+			System.out.println(gctx);
+		}
+		return gctx;
 	}
 
-	private int findGpxRouteFromPoint(final RoutingContext ctx, GpxApproximateContext gctx, List<GpxPoint> gpxPoints,
+	private void addStraightLine(List<RouteSegmentResult> res, TIntArrayList lastStraightLine) {
-			int pointFrom) throws IOException, InterruptedException {
+		// TODO add missing turns for straight lines
-		GpxPoint start = gpxPoints.get(pointFrom);
+		RouteDataObject rdo = new RouteDataObject(new RouteRegion());
-		for (int targetInd = gpxPoints.size() - 1; targetInd > pointFrom; targetInd--) {
+		int l = lastStraightLine.size() / 2;
-			GpxPoint target = gpxPoints.get(targetInd);
+		rdo.pointsX = new int[l];
-			if (target.cumDist - start.cumDist < AVERAGE_SPLIT_DISTANCE_GPX) {
+		rdo.pointsY = new int[l];
-				target.pnt = findRouteSegment(target.loc.getLatitude(), target.loc.getLongitude(), ctx, null, false);
+		rdo.types = new int[0];
-				List<RouteSegmentResult> res = null;
+		rdo.id = -1;
-				if (target.pnt != null) {
+		for (int i = 0; i < l; i++) {
-					gctx.routeDistCalculations += (target.cumDist - start.cumDist);
+			rdo.pointsX[i] = lastStraightLine.get(i * 2);
-					gctx.routeCalculations++;
+			rdo.pointsY[i] = lastStraightLine.get(i * 2 + 1);
-					res = searchRouteInternalPrepare(ctx, start.pnt, target.pnt, null);
+		}
-				} else if (target.cumDist - start.cumDist < MINIMUM_SPLIT_DISTANCE_GPX) {
+		res.add(new RouteSegmentResult(rdo, 0, rdo.getPointsLength() - 1));
-					// distance too short, so break here and leave non-built
+	}
-					return targetInd;
-				}
-				boolean routeIsCorrect = res != null && res.isEmpty();
-				for (int k = pointFrom + 1; k < targetInd; k++) {
-					GpxPoint ipoint = gpxPoints.get(k);
-					if (dist(ipoint, res) > DISTANCE_APPROXIMATE_THRESHOLD) {
-						routeIsCorrect = false;
-						targetInd = k;
-						break;
-					}
-				}
 
-				if (routeIsCorrect) {
+	private int findGpxRouteFromPoint(final RoutingContext ctx, GpxApproximationResult gctx, List<GpxPoint> gpxPoints,
-					start.routeToTarget = res;
+			int pointFrom) throws IOException, InterruptedException {
-					start.targetInd = target.ind;
+		
-					makeStartEndPointsPrecise(res, start.pnt.getPreciseLatLon(), target.pnt.getPreciseLatLon(), null);
+		GpxPoint start = gpxPoints.get(pointFrom);
-					return targetInd;
+		if (start.pnt == null) {
+			start.pnt = findRouteSegment(start.loc.getLatitude(), start.loc.getLongitude(), ctx, null, false);
+		}
+		if (start.pnt == null) {
+			return -1;
+		}
+		int targetInd = pointFrom + 1;
+		while (targetInd < gpxPoints.size() - 1) {
+			GpxPoint target = gpxPoints.get(targetInd);
+			if (target.cumDist - start.cumDist > AVERAGE_SPLIT_DISTANCE_GPX) {
+				break;
+			}
+			targetInd++;
+		}
+		while (targetInd > pointFrom) {
+			GpxPoint target = gpxPoints.get(targetInd);
+			int nextTargetInd = targetInd - 1;
+			if (target.cumDist - start.cumDist < MINIMUM_SPLIT_DISTANCE_GPX) {
+				// distance too short, so break here and leave non-built
+				return targetInd;
+			}
+			target.pnt = findRouteSegment(target.loc.getLatitude(), target.loc.getLongitude(), ctx, null, false);
+			List<RouteSegmentResult> res = null;
+			if (target.pnt != null) {
+				gctx.routeDistCalculations += (target.cumDist - start.cumDist);
+				gctx.routeCalculations++;
+				res = searchRouteInternalPrepare(ctx, start.pnt, target.pnt, null);
+			}
+			boolean routeIsCorrect = res != null && !res.isEmpty();
+			for (int k = pointFrom + 1; k < targetInd; k++) {
+				GpxPoint ipoint = gpxPoints.get(k);
+				if (!pointCloseEnough(ipoint, res)) {
+					routeIsCorrect = false;
+					nextTargetInd = k;
+					break;
 				}
 			}
+			if (routeIsCorrect) {
+				start.routeToTarget = res;
+				start.targetInd = target.ind;
+				makeStartEndPointsPrecise(res, start.pnt.getPreciseLatLon(), target.pnt.getPreciseLatLon(), null);
+				return targetInd;
+			} else {
+				targetInd = nextTargetInd;
+			}
+			
 		}
 		return -1;
 	}
 
-	private double dist(GpxPoint ipoint, List<RouteSegmentResult> res) {
+	private boolean pointCloseEnough(GpxPoint ipoint, List<RouteSegmentResult> res) {
-		// TODO Auto-generated method stub
+		int px = MapUtils.get31TileNumberX(ipoint.loc.getLongitude());
-		return 0;
+		int py = MapUtils.get31TileNumberY(ipoint.loc.getLatitude());
+		double SQR = DISTANCE_APPROXIMATE_THRESHOLD * DISTANCE_APPROXIMATE_THRESHOLD;
+		for(RouteSegmentResult sr : res) {
+			int start = sr.getStartPointIndex();
+			int end = sr.getEndPointIndex();
+			if(sr.getStartPointIndex() > sr.getEndPointIndex()) {
+				start = sr.getEndPointIndex();
+				end = sr.getStartPointIndex();
+			}
+			for (int i = start; i < end; i++) {
+				RouteDataObject r = sr.getObject();
+				QuadPoint pp = MapUtils.getProjectionPoint31(px, py, r.getPoint31XTile(i), r.getPoint31YTile(i),
+						r.getPoint31XTile(i + 1), r.getPoint31YTile(i + 1));
+				double currentsDist = squareDist((int) pp.x, (int) pp.y, px, py);
+				if (currentsDist <= SQR) {
+					return true;
+				}
+			}
+		}
+		return false;
 	}
 
 	private boolean needRequestPrivateAccessRouting(RoutingContext ctx, List<LatLon> points) throws IOException {

OsmAnd-java/src/main/java/net/osmand/router/RouteResultPreparation.java

@@ -175,6 +175,11 @@ public class RouteResultPreparation {
 		splitRoadsAndAttachRoadSegments(ctx, result, recalculation);
 		calculateTimeSpeed(ctx, result);
 		
+		prepareTurnResults(ctx, result);
+		return result;
+	}
+
+	public void prepareTurnResults(RoutingContext ctx, List<RouteSegmentResult> result) {
 		for (int i = 0; i < result.size(); i ++) {
 			TurnType turnType = getTurnInfo(result, i, ctx.leftSideNavigation);
 			result.get(i).setTurnType(turnType);
@@ -184,7 +189,6 @@ public class RouteResultPreparation {
 		ignorePrecedingStraightsOnSameIntersection(ctx.leftSideNavigation, result);
 		justifyUTurns(ctx.leftSideNavigation, result);
 		addTurnInfoDescriptions(result);
-		return result;
 	}
 
 	protected void ignorePrecedingStraightsOnSameIntersection(boolean leftside, List<RouteSegmentResult> result) {
@@ -283,11 +287,12 @@ public class RouteResultPreparation {
 						float height = heightDistanceArray[heightIndex];
 						if (prevHeight != -99999.0f) {
 							float heightDiff = height - prevHeight;
-							if (heightDiff > 0) {  //ascent only
+							if (heightDiff > 0) { // ascent only
-								distOnRoadToPass += heightDiff * 6.0f;  //Naismith's rule: add 1 hour per every 600m of ascent
+								distOnRoadToPass += heightDiff * 6.0f; // Naismith's rule: add 1 hour per every 600m of
+																		// ascent
 							}
 						}
-					prevHeight = height;
+						prevHeight = height;
 					}
 				}
 			}
@@ -359,7 +364,7 @@ public class RouteResultPreparation {
 
 	private void checkAndInitRouteRegion(RoutingContext ctx, RouteDataObject road) throws IOException {
 		BinaryMapIndexReader reader = ctx.reverseMap.get(road.region);
-		if(reader != null) {
+		if (reader != null) {
 			reader.initRouteRegion(road.region);
 		}
 	}