Fix route calculation for GPX

OsmAnd-java/src/main/java/net/osmand/IndexConstants.java

@@ -28,9 +28,6 @@ public class IndexConstants {
 	public static final String EXTRA_EXT = ".extra";
 	public static final String EXTRA_ZIP_EXT = ".extra.zip";
 	
-	public static final String TOUR_INDEX_EXT = ".tour"; //$NON-NLS-1$
-	public static final String TOUR_INDEX_EXT_ZIP = ".tour.zip"; //$NON-NLS-1$
-	
 	public static final String GEN_LOG_EXT = ".gen.log"; //$NON-NLS-1$
 	
 	public static final String VOICE_INDEX_EXT_ZIP = ".voice.zip"; //$NON-NLS-1$

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

@@ -50,7 +50,7 @@ public class RoutePlannerFrontEnd {
 		// don't search subsegments shorter than specified distance (also used to step back for car turns)
 		public double MINIMUM_STEP_APPROXIMATION = 100;
 		// Parameter to smoother the track itself (could be 0 if it's not recorded track)
-		public double SMOOTHEN_POINTS_NO_ROUTE = 2;
+		public double SMOOTHEN_POINTS_NO_ROUTE = 5;
 		
 		public final RoutingContext ctx;
 		public int routeCalculations = 0;
@@ -92,6 +92,7 @@ public class RoutePlannerFrontEnd {
 		public double cumDist;
 		public RouteSegmentPoint pnt;
 		public List<RouteSegmentResult> routeToTarget;
+		public List<RouteSegmentResult> stepBackRoute;
 		public int targetInd = -1;
 	}
 
@@ -203,11 +204,7 @@ public class RoutePlannerFrontEnd {
 	}
 
 
-	// TODO last segment not correct (cut) before end point and point of straight line
-	
-	// TODO add missing turns for straight lines (compare code)
 	// TODO native matches less roads
-	
 	// TODO fix progress - next iteration
 	// TODO fix timings and remove logging every iteration
 	public GpxRouteApproximation searchGpxRoute(GpxRouteApproximation gctx, List<LatLon> points) throws IOException, InterruptedException {
@@ -217,7 +214,7 @@ public class RoutePlannerFrontEnd {
 		}
 		List<GpxPoint> gpxPoints = generageGpxPoints(points, gctx);
 		GpxPoint start = gpxPoints.size() > 0 ? gpxPoints.get(0) : null;
-		boolean prevRouteFound = false;
+		GpxPoint prev = null;
 		while (start != null) {
 			double routeDist = gctx.MAXIMUM_STEP_APPROXIMATION;
 			GpxPoint next = findNextGpxPointWithin(gctx, gpxPoints, start, routeDist);
@@ -227,13 +224,13 @@ public class RoutePlannerFrontEnd {
 				while (routeDist >= gctx.MINIMUM_STEP_APPROXIMATION && !routeFound) {
 					routeFound = initRoutingPoint(next, gctx, gctx.MINIMUM_POINT_APPROXIMATION);
 					if (routeFound) {
-						routeFound = findGpxRouteSegment(gctx, gpxPoints, start, next, prevRouteFound);
+						routeFound = findGpxRouteSegment(gctx, gpxPoints, start, next, prev != null);
 						if (routeFound) {
 							// route is found - cut the end of the route and move to next iteration
 							boolean stepBack = stepBackAndFindPrevPointInRoute(gctx, gpxPoints, start, next);
 							if (!stepBack) {
 								// not supported case (workaround increase MAXIMUM_STEP_APPROXIMATION)
-								log.info("Consider to increase MAXIMUM_STEP_APPROXIMATION to: " + routeDist*2);
+								log.info("Consider to increase MAXIMUM_STEP_APPROXIMATION to: " + routeDist * 2);
 								start.routeToTarget = null;
 								routeFound = false;
 								break;
@@ -257,16 +254,17 @@ public class RoutePlannerFrontEnd {
 			if (!routeFound) {
 				// route is not found, move start point by 
 				next = findNextGpxPointWithin(gctx, gpxPoints, start, gctx.MINIMUM_STEP_APPROXIMATION);
-				if (prevRouteFound) {
+				if (prev != null) {
-					if (next == null) {
+					prev.routeToTarget.addAll(prev.stepBackRoute);
-						// TODO finish
+					makeSegmentPointPrecise(prev.routeToTarget.get(prev.routeToTarget.size() - 1), start.loc, false);
-						// makeSegmentPointPrecise(prev.routeToTarget.get(prev.routeToTarget.size() - 1), start.loc, false);				
+					if (next != null) {
-					} else {
 						log.warn("NOT found route from: " + start.pnt.getRoad() + " at " + start.pnt.getSegmentStart());
 					}
 				}
+				prev = null;
+			} else {
+				prev = start;
 			}
-			prevRouteFound = routeFound;
 			start = next;
 		}
 		
@@ -289,6 +287,7 @@ public class RoutePlannerFrontEnd {
 		double d = 0;
 		int segmendInd = start.routeToTarget.size() - 1;
 		boolean search = true;
+		start.stepBackRoute = new ArrayList<RouteSegmentResult>();
 		mainLoop: for (; segmendInd >= 0 && search; segmendInd--) {
 			RouteSegmentResult rr = start.routeToTarget.get(segmendInd);
 			boolean minus = rr.getStartPointIndex() < rr.getEndPointIndex();
@@ -300,6 +299,7 @@ public class RoutePlannerFrontEnd {
 					if (nextInd == rr.getStartPointIndex()) {
 						segmendInd--;
 					} else {
+						start.stepBackRoute.add(new RouteSegmentResult(rr.getObject(), nextInd, rr.getEndPointIndex()));
 						rr.setEndPointIndex(nextInd);
 					}
 					search = false;
@@ -311,8 +311,10 @@ public class RoutePlannerFrontEnd {
 			// here all route segments - 1 is longer than needed distance to step back 
 			return false;
 		}
+		
 		while (start.routeToTarget.size() > segmendInd + 1) {
-			start.routeToTarget.remove(segmendInd + 1);
+			RouteSegmentResult removed = start.routeToTarget.remove(segmendInd + 1);
+			start.stepBackRoute.add(removed);
 		}
 		RouteSegmentResult res = start.routeToTarget.get(segmendInd);
 		next.pnt = new RouteSegmentPoint(res.getObject(), res.getEndPointIndex(), 0);
@@ -321,7 +323,7 @@ public class RoutePlannerFrontEnd {
 
 	private void calculateGpxRoute(GpxRouteApproximation gctx, List<GpxPoint> gpxPoints) {
 		RouteRegion reg = new RouteRegion();
-		reg.initRouteEncodingRule(0, "highway", "unmatched");
+		reg.initRouteEncodingRule(0, "highway", RouteResultPreparation.UNMATCHED_HIGHWAY_TYPE);
 		List<LatLon> lastStraightLine = null;
 		for (int i = 0; i < gpxPoints.size(); ) {
 			GpxPoint pnt = gpxPoints.get(i);
@@ -424,8 +426,17 @@ public class RoutePlannerFrontEnd {
 		rdo.pointsY = y.toArray();
 		rdo.types = new int[] { 0 } ;
 		rdo.id = -1;
-		// comment to see road without straight connections
+		List<RouteSegmentResult> rts = new ArrayList<>();
-		res.add(new RouteSegmentResult(rdo, 0, rdo.getPointsLength() - 1));
+		rts.add(new RouteSegmentResult(rdo, 0, rdo.getPointsLength() - 1));
+		RouteResultPreparation preparation = new RouteResultPreparation();
+		try {
+			preparation.prepareResult(gctx.ctx, rts, false);
+		} catch (IOException e) {
+			throw new IllegalStateException(e);
+		}
+		
+		// VIEW: comment to see road without straight connections
+		res.addAll(rts);
 	}
 	
 	

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

@@ -44,8 +44,11 @@ public class RouteResultPreparation {
 	public static boolean PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST = false;
 	public static String PRINT_TO_GPX_FILE = null;
 	private static final float TURN_DEGREE_MIN = 45;
+	private static final float UNMATCHED_TURN_DEGREE_MINIMUM = 45;
+	private static final float SPLIT_TURN_DEGREE_NOT_STRAIGHT = 100;
 	public static final int SHIFT_ID = 6;
 	private Log log = PlatformUtil.getLog(RouteResultPreparation.class);
+	public static final String UNMATCHED_HIGHWAY_TYPE = "unmatched";
 	/**
 	 * Helper method to prepare final result 
 	 */
@@ -222,7 +225,7 @@ public class RouteResultPreparation {
 
 	private void justifyUTurns(boolean leftSide, List<RouteSegmentResult> result) {
 		int next;
-		for (int i = 0; i < result.size() - 1; i = next) {
+		for (int i = 1; i < result.size() - 1; i = next) {
 			next = i + 1;
 			TurnType t = result.get(i).getTurnType();
 			// justify turn
@@ -313,6 +316,7 @@ public class RouteResultPreparation {
 			RouteSegmentResult rr = result.get(i);
 			boolean plus = rr.getStartPointIndex() < rr.getEndPointIndex();
 			int next;
+			boolean unmatched = UNMATCHED_HIGHWAY_TYPE.equals(rr.getObject().getHighway());
 			for (int j = rr.getStartPointIndex(); j != rr.getEndPointIndex(); j = next) {
 				next = plus ? j + 1 : j - 1;
 				if (j == rr.getStartPointIndex()) {
@@ -323,27 +327,33 @@ public class RouteResultPreparation {
 				}
 				List<RouteSegmentResult> attachedRoutes = rr.getAttachedRoutes(next);
 				boolean tryToSplit = next != rr.getEndPointIndex() && !rr.getObject().roundabout() && attachedRoutes != null;
-				if(rr.getDistance(next, plus ) == 0) {
+				if (rr.getDistance(next, plus) == 0) {
 					// same point will be processed next step
 					tryToSplit = false;
 				}
 				if (tryToSplit) {
+					float distBearing = unmatched ? RouteSegmentResult.DIST_BEARING_DETECT_UNMATCHED : RouteSegmentResult.DIST_BEARING_DETECT;
 					// avoid small zigzags
-					float before = rr.getBearing(next, !plus);
+					float before = rr.getBearingEnd(next, distBearing);
-					float after = rr.getBearing(next, plus);
+					float after = rr.getBearingBegin(next, distBearing);
-					if(rr.getDistance(next, plus ) < 5) {
+					if (rr.getDistance(next, plus) < distBearing) {
-						after = before + 180;
+						after = before;
-					} else if(rr.getDistance(next, !plus ) < 5) {
+					} else if (rr.getDistance(next, !plus) < distBearing) {
-						before = after - 180;
+						before = after;
 					}
-					boolean straight = Math.abs(MapUtils.degreesDiff(before + 180, after)) < TURN_DEGREE_MIN;
+					double contAngle = Math.abs(MapUtils.degreesDiff(before, after));
+					boolean straight = contAngle < TURN_DEGREE_MIN;
 					boolean isSplit = false;
+					
+					if (unmatched && Math.abs(contAngle) >= UNMATCHED_TURN_DEGREE_MINIMUM) {
+						isSplit = true;
+					}
 					// split if needed
 					for (RouteSegmentResult rs : attachedRoutes) {
-						double diff = MapUtils.degreesDiff(before + 180, rs.getBearingBegin());
+						double diff = MapUtils.degreesDiff(before, rs.getBearingBegin());
 						if (Math.abs(diff) <= TURN_DEGREE_MIN) {
 							isSplit = true;
-						} else if (!straight && Math.abs(diff) < 100) {
+						} else if (!straight && Math.abs(diff) < SPLIT_TURN_DEGREE_NOT_STRAIGHT) {
 							isSplit = true;
 						}
 					}
@@ -1061,18 +1071,17 @@ public class RouteResultPreparation {
 		}
 		TurnType t = null;
 		if (prev != null) {
-			boolean noAttachedRoads = rr.getAttachedRoutes(rr.getStartPointIndex()).size() == 0;
 			// add description about turn
-			double mpi = MapUtils.degreesDiff(prev.getBearingEnd(), rr.getBearingBegin());
+			// avoid small zigzags is covered at (search for "zigzags")
-			if(noAttachedRoads){
+			float bearingDist = RouteSegmentResult.DIST_BEARING_DETECT;
-				// VICTOR : look at the comment inside direction route
+			// could be || noAttachedRoads, boolean noAttachedRoads = rr.getAttachedRoutes(rr.getStartPointIndex()).size() == 0;
-				// ? avoid small zigzags is covered at (search for "zigzags") 
+			if (UNMATCHED_HIGHWAY_TYPE.equals(rr.getObject().getHighway())) {
-//				double begin = rr.getObject().directionRoute(rr.getStartPointIndex(), rr.getStartPointIndex() < 
+				bearingDist = RouteSegmentResult.DIST_BEARING_DETECT_UNMATCHED;
-//						rr.getEndPointIndex(), 25);
-//				mpi = MapUtils.degreesDiff(prev.getBearingEnd(), begin);
 			}
+			double mpi = MapUtils.degreesDiff(prev.getBearingEnd(prev.getEndPointIndex(), bearingDist), 
+					rr.getBearingBegin(rr.getStartPointIndex(), bearingDist));
 			if (mpi >= TURN_DEGREE_MIN) {
-				if (mpi < 45) {
+				if (mpi < TURN_DEGREE_MIN) {
 					// Slight turn detection here causes many false positives where drivers would expect a "normal" TL. Best use limit-angle=TURN_DEGREE_MIN, this reduces TSL to the turn-lanes cases.
 					t = TurnType.valueOf(TurnType.TSLL, leftSide);
 				} else if (mpi < 120) {
@@ -1085,7 +1094,7 @@ public class RouteResultPreparation {
 				int[] lanes = getTurnLanesInfo(prev, t.getValue());
 				t.setLanes(lanes);
 			} else if (mpi < -TURN_DEGREE_MIN) {
-				if (mpi > -45) {
+				if (mpi > -TURN_DEGREE_MIN) {
 					t = TurnType.valueOf(TurnType.TSLR, leftSide);
 				} else if (mpi > -120) {
 					t = TurnType.valueOf(TurnType.TR, leftSide);

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

@@ -22,7 +22,10 @@ import gnu.trove.map.hash.TIntObjectHashMap;
 
 public class RouteSegmentResult implements StringExternalizable<RouteDataBundle> {
 	// this should be bigger (50-80m) but tests need to be fixed first
-	private static final float DIST_BEARING_DETECT = 5;
+	public static final float DIST_BEARING_DETECT = 5;
+	
+	public static final float DIST_BEARING_DETECT_UNMATCHED = 50;
+	
 	private RouteDataObject object;
 	private int startPointIndex;
 	private int endPointIndex;
@@ -446,19 +449,32 @@ public class RouteSegmentResult implements StringExternalizable<RouteDataBundle>
 	}
 	
 	public float getBearingBegin() {
-		return (float) (object.directionRoute(startPointIndex, startPointIndex < endPointIndex, DIST_BEARING_DETECT) / Math.PI * 180);
+		return getBearingBegin(startPointIndex, DIST_BEARING_DETECT);
 	}
 	
-	public float getBearing(int point, boolean plus) {
+	public float getBearingBegin(int point, float dist) {
-		return (float) (object.directionRoute(point, plus, DIST_BEARING_DETECT) / Math.PI * 180);
+		return getBearing(point, true, dist);
-	}
-	
-	public float getDistance(int point, boolean plus) {
-		return (float) (plus? object.distance(point, endPointIndex): object.distance(startPointIndex, point));
 	}
 	
 	public float getBearingEnd() {
-		return (float) (MapUtils.alignAngleDifference(object.directionRoute(endPointIndex, startPointIndex > endPointIndex, DIST_BEARING_DETECT) - Math.PI) / Math.PI * 180);
+		return getBearingEnd(endPointIndex, DIST_BEARING_DETECT);
+	}
+	
+	public float getBearingEnd(int point, float dist) {
+		return getBearing(point, false, dist);
+	}
+	
+	public float getBearing(int point, boolean begin, float dist) {
+		if (begin) {
+			return (float) (object.directionRoute(point, startPointIndex < endPointIndex, dist) / Math.PI * 180);
+		} else {
+			double dr = object.directionRoute(point, startPointIndex > endPointIndex, dist);
+			return (float) (MapUtils.alignAngleDifference(dr - Math.PI) / Math.PI * 180);
+		}
+	}
+	
+	public float getDistance(int point, boolean plus) {
+		return (float) (plus ? object.distance(point, endPointIndex) : object.distance(startPointIndex, point));
 	}
 	
 	public void setSegmentTime(float segmentTime) {

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

@@ -493,9 +493,9 @@ public class RouteCalculationResult {
 			Location current = locations.get(i);
 			float bearing = current.bearingTo(next);
 			// try to get close to current location if possible
-			while(prevBearingLocation < i - 1){
+			while (prevBearingLocation < i - 1) {
-				if(locations.get(prevBearingLocation + 1).distanceTo(current) > 70){
+				if (locations.get(prevBearingLocation + 1).distanceTo(current) > 70) {
-					prevBearingLocation ++;
+					prevBearingLocation++;
 				} else {
 					break;
 				}