More precise calculation of connected points

OsmAnd-java/src/main/java/net/osmand/binary/RouteDataObject.java

@@ -1070,8 +1070,8 @@ public class RouteDataObject {
 	public String toString() {
 		String name = getName();
 		String rf = getRef("", false, true);
-		return MessageFormat.format("Road id {0} name {1} ref {2}", (getId() / 64) + "", name == null ? "" : name,
-				rf == null ? "" : rf);
+		return String.format("Road id (%d), name ('%s'), ref ('%s')", id / 64, name, rf);
+//		return String.format("Road [%d, '%s', '%s'] - [%s, %s]", id / 64, name, rf, Arrays.toString(pointsX), Arrays.toString(pointsY));
 	}
 
 	public boolean hasNameTagStartsWith(String tagStartsWith) {
@@ -1117,4 +1117,15 @@ public class RouteDataObject {
 		}
 		restrictionsVia[k] = viaWay;
 	}
+
+	public void setPointTypes(int pntInd, int[] array) {
+		if (pointTypes == null || pointTypes.length <= pntInd) {
+			int[][] npointTypes = new int[pntInd + 1][];
+			for (int k = 0; pointTypes != null && k < pointTypes.length; k++) {
+				npointTypes[k] = pointTypes[k];
+			}
+			pointTypes = npointTypes;
+		}
+		pointTypes[pntInd] = array;
+	}
 }

OsmAnd-java/src/main/java/net/osmand/osm/edit/Node.java

@@ -1,12 +1,11 @@
 package net.osmand.osm.edit;
 
-import net.osmand.Location;
-import net.osmand.data.LatLon;
-import net.osmand.util.Algorithms;
-
 import java.io.Serializable;
 import java.util.Map;
 
+import net.osmand.Location;
+import net.osmand.data.LatLon;
+
 public class Node extends Entity implements Serializable {
 
 	private static final long serialVersionUID = -2981499160640211082L;

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

@@ -903,7 +903,7 @@ public class BinaryRoutePlanner {
 	}
 
 	public static class RouteSegment {
-		final short segStart;
+		short segStart;
 		final RouteDataObject road;
 		// needed to store intersection of routes
 		RouteSegment next = null;

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

@@ -1,8 +1,10 @@
 package net.osmand.router;
 
+import net.osmand.GPXUtilities.WptPt;
 import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteRegion;
 import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteTypeRule;
 import net.osmand.binary.RouteDataObject;
+import net.osmand.data.QuadTree;
 import net.osmand.router.BinaryRoutePlanner.RouteSegment;
 import net.osmand.util.Algorithms;
 import net.osmand.util.MapUtils;
@@ -76,6 +78,7 @@ public class GeneralRouter implements VehicleRouter {
 	private float maxVehicleSpeed;
 
 	private TLongHashSet impassableRoads;
+	
 	private GeneralRouterProfile profile;
 	
 	Map<RouteRegion, Map<IntHolder, Float>>[] evalCache;	
@@ -268,11 +271,11 @@ public class GeneralRouter implements VehicleRouter {
 	@Override
 	public boolean acceptLine(RouteDataObject way) {
 		Float res = getCache(RouteDataObjectAttribute.ACCESS, way);
-		if(res == null) {
+		if (res == null) {
 			res = (float) getObjContext(RouteDataObjectAttribute.ACCESS).evaluateInt(way, 0);
 			putCache(RouteDataObjectAttribute.ACCESS, way, res);
 		}
-		if(impassableRoads != null && impassableRoads.contains(way.id)) {
+		if (impassableRoads != null && impassableRoads.contains(way.id)) {
 			return false;
 		}
 		return res >= 0;

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

@@ -1,21 +1,29 @@
 package net.osmand.router;
 
-import net.osmand.PlatformUtil;
-import net.osmand.router.GeneralRouter.GeneralRouterProfile;
-import net.osmand.router.GeneralRouter.RouteAttributeContext;
-import net.osmand.router.GeneralRouter.RouteDataObjectAttribute;
-import net.osmand.util.Algorithms;
+import java.io.IOException;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Set;
+import java.util.Stack;
 
 import org.xmlpull.v1.XmlPullParser;
 import org.xmlpull.v1.XmlPullParserException;
 
-import java.io.IOException;
-import java.io.InputStream;
-import java.util.HashSet;
-import java.util.LinkedHashMap;
-import java.util.Map;
-import java.util.Set;
-import java.util.Stack;
+import gnu.trove.list.array.TIntArrayList;
+import net.osmand.PlatformUtil;
+import net.osmand.binary.RouteDataObject;
+import net.osmand.data.QuadRect;
+import net.osmand.data.QuadTree;
+import net.osmand.osm.edit.Node;
+import net.osmand.router.GeneralRouter.GeneralRouterProfile;
+import net.osmand.router.GeneralRouter.RouteAttributeContext;
+import net.osmand.router.GeneralRouter.RouteDataObjectAttribute;
+import net.osmand.util.Algorithms;
+import net.osmand.util.MapUtils;
 
 public class RoutingConfiguration {
 
@@ -49,15 +57,38 @@ public class RoutingConfiguration {
 	// 1.6 Time to calculate all access restrictions based on conditions
 	public long routeCalculationTime = 0;
 	
+	
+	// extra points to be inserted in ways (quad tree is based on 31 coords)
+	private QuadTree<DirectionPoint> directionPoints;
+	
+	public int directionPointsRadius = 30; // 30 m
+	
+	public QuadTree<DirectionPoint> getDirectionPoints() {
+		return directionPoints;
+	}
+
+	public static class DirectionPoint extends Node {
+		private static final long serialVersionUID = -7496599771204656505L;
+		public double distance = Double.MAX_VALUE;
+		public RouteDataObject connected;
+		public int pointIndex;
+		public TIntArrayList types = new TIntArrayList();
+
+		public DirectionPoint(Node n) {
+			super(n, n.getId());
+		}
+		
+	}
+	
 	public static class Builder {
 		// Design time storage
 		private String defaultRouter = "";
 		private Map<String, GeneralRouter> routers = new LinkedHashMap<>();
 		private Map<String, String> attributes = new LinkedHashMap<>();
 		private Set<Long> impassableRoadLocations = new HashSet<>();
+		private QuadTree<Node> directionPointsBuilder;
 
 		public Builder() {
-
 		}
 
 		public Builder(Map<String, String> defaultAttributes) {
@@ -106,10 +137,26 @@ public class RoutingConfiguration {
 				i.memoryLimitation = memoryLimitMB * (1l << 20);
 			}
 			i.planRoadDirection = parseSilentInt(getAttribute(i.router, "planRoadDirection"), i.planRoadDirection);
+			if (directionPointsBuilder != null) {
+				QuadRect rect = new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE);
+				List<net.osmand.osm.edit.Node> lst = directionPointsBuilder.queryInBox(rect, new ArrayList<Node>());
+				i.directionPoints = new QuadTree<>(rect, 14, 0.5f);
+				for(Node n : lst) {
+					DirectionPoint dp = new DirectionPoint(n);
+					int x = MapUtils.get31TileNumberX(dp.getLongitude());
+					int y = MapUtils.get31TileNumberY(dp.getLatitude());
+					i.directionPoints.insert(dp, new QuadRect(x, y, x, y));
+				}
+			}
 //			i.planRoadDirection = 1;
 			return i;
 		}
 		
+		public Builder setDirectionPoints(QuadTree<Node> directionPoints) {
+			this.directionPointsBuilder = directionPoints;
+			return this;
+		}
+
 		public Set<Long> getImpassableRoadLocations() {
 			return impassableRoadLocations;
 		}

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

@@ -3,6 +3,7 @@ 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.Date;
@@ -28,10 +29,14 @@ import net.osmand.binary.BinaryMapRouteReaderAdapter;
 import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteRegion;
 import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteSubregion;
 import net.osmand.binary.RouteDataObject;
+import net.osmand.data.QuadPoint;
+import net.osmand.data.QuadRect;
 import net.osmand.router.BinaryRoutePlanner.FinalRouteSegment;
 import net.osmand.router.BinaryRoutePlanner.RouteSegment;
 import net.osmand.router.BinaryRoutePlanner.RouteSegmentVisitor;
 import net.osmand.router.RoutePlannerFrontEnd.RouteCalculationMode;
+import net.osmand.router.RoutingConfiguration.DirectionPoint;
+import net.osmand.util.MapUtils;
 
 
 public class RoutingContext {
@@ -272,27 +277,43 @@ public class RoutingContext {
 		int ucount = ts.getUnloadCont();
 		if (nativeLib == null) {
 			long now = System.nanoTime();
+			List<DirectionPoint> points = Collections.emptyList();
+			if (config.getDirectionPoints() != null) {
+				points = config.getDirectionPoints().queryInBox(
+						new QuadRect(ts.subregion.left, ts.subregion.top, ts.subregion.right, ts.subregion.bottom), new ArrayList<>());
+				for (DirectionPoint d : points) {
+					// use temporary types
+					d.types.clear();
+					for (Entry<String, String> e : d.getTags().entrySet()) {
+						int type = ts.subregion.routeReg.searchRouteEncodingRule(e.getKey(), e.getValue());
+						if (type != -1) {
+							d.types.add(type);
+						}
+					}
+				}
+			}
 			try {
 				BinaryMapIndexReader reader = reverseMap.get(ts.subregion.routeReg);
 				ts.setLoadedNonNative();
 				List<RouteDataObject> res = reader.loadRouteIndexData(ts.subregion);
 				
-				if(toLoad != null) {
+				if (toLoad != null) {
 					toLoad.addAll(res);
 				} else {
-					for(RouteDataObject ro : res){
-						if(ro != null) {
-							if(config.routeCalculationTime != 0) {
+					for (RouteDataObject ro : res) {
+						if (ro != null) {
+							if (config.routeCalculationTime != 0) {
 								ro.processConditionalTags(config.routeCalculationTime);
 							}
-							if(config.router.acceptLine(ro)) {
-								if(excludeNotAllowed != null && !excludeNotAllowed.contains(ro.getId())) {
+							if (config.router.acceptLine(ro)) {
+								if (excludeNotAllowed != null && !excludeNotAllowed.contains(ro.getId())) {
+									connectPoint(ts, ro, points);
 									ts.add(ro);
 								}
 							}
-							if(excludeNotAllowed != null && ro.getId() > 0){
+							if (excludeNotAllowed != null && ro.getId() > 0) {
 								excludeNotAllowed.add(ro.getId());
-								if(ts.excludedIds == null ){
+								if (ts.excludedIds == null) {
 									ts.excludedIds = new TLongHashSet();
 								}
 								ts.excludedIds.add(ro.getId());
@@ -338,6 +359,8 @@ public class RoutingContext {
 		global.size += ts.tileStatistics.size;
 	}
 
+	
+
 	private List<RoutingSubregionTile> loadTileHeaders(final int x31, final int y31) {
 		final int zoomToLoad = 31 - config.ZOOM_TO_LOAD_TILES;
 		int tileX = x31 >> zoomToLoad;
@@ -451,8 +474,9 @@ public class RoutingContext {
 	@SuppressWarnings("unused")
 	private long getRoutingTile(int x31, int y31, long memoryLimit) {
 		// long now = System.nanoTime();
-		long xloc = x31 >> (31 - config.ZOOM_TO_LOAD_TILES);
-		long yloc = y31 >> (31 - config.ZOOM_TO_LOAD_TILES);
+		int zmShift = 31 - config.ZOOM_TO_LOAD_TILES;
+		long xloc = x31 >> zmShift;
+		long yloc = y31 >> zmShift;
 		long tileId = (xloc << config.ZOOM_TO_LOAD_TILES) + yloc;
 		if (memoryLimit == 0) {
 			memoryLimit = config.memoryLimitation;
@@ -513,6 +537,67 @@ public class RoutingContext {
 	}
 
 	
+	private void connectPoint(final RoutingSubregionTile ts, RouteDataObject ro, List<DirectionPoint> points) {
+		for (DirectionPoint np : points) {
+			if (np.types.size() == 0) {
+				continue;
+			}	
+			boolean sameRoadId = np.connected != null && np.connected.getId() == ro.getId() && np.connected != ro;
+			int wptX = MapUtils.get31TileNumberX(np.getLongitude());
+			int wptY = MapUtils.get31TileNumberY(np.getLatitude());
+			
+			int x = ro.getPoint31XTile(0);
+			int y = ro.getPoint31YTile(0);
+			for(int i = 1; i < ro.getPointsLength(); i++) {
+				int nx = ro.getPoint31XTile(i);
+				int ny = ro.getPoint31YTile(i);
+				// TODO wptX != x || wptY != y this check is questionable
+				boolean sameRoadIdIndex = sameRoadId && np.pointIndex == i && (wptX != x || wptY != y);
+				
+				boolean sgnx = nx - wptX > 0; 
+				boolean sgx = x - wptX > 0;
+				boolean sgny = ny - wptY > 0; 
+				boolean sgy = y - wptY > 0;
+				double dist;
+				if (sgny == sgy && sgx == sgnx) {
+					// point outside of rect (line is diagonal) distance is likely be bigger
+					// TODO this can be speed up without projection!
+					dist = MapUtils.squareRootDist31(wptX, wptY, Math.abs(nx - wptX) < Math.abs(x - wptX) ? nx : x,
+							Math.abs(ny - wptY) < Math.abs(y - wptY) ? ny : y);
+					if (dist < config.directionPointsRadius) {
+						QuadPoint pnt = MapUtils.getProjectionPoint31(wptX, wptY, x, y, nx, ny);
+						dist = MapUtils.squareRootDist31(wptX, wptY, (int) pnt.x, (int) pnt.y);
+					}
+				} else {
+					QuadPoint pnt = MapUtils.getProjectionPoint31(wptX, wptY, x, y, nx, ny);
+					dist = MapUtils.squareRootDist31(wptX, wptY, (int) pnt.x, (int) pnt.y);
+				}
+
+				if ((dist < np.distance && dist < config.directionPointsRadius) || sameRoadIdIndex) {
+					System.out.println(String.format("INSERT %s %s (%d-%d) %.0f m [%.5f, %.5f] ",  ts.subregion.hashCode() + "",
+							ro, i, i + 1, dist, MapUtils.get31LatitudeY(wptY), MapUtils.get31LongitudeX(wptX)));
+					if (np.connected != null && !sameRoadIdIndex) {
+						// clear old connected
+						np.connected.setPointTypes(np.pointIndex, new int[0]);
+					}
+					ro.insert(i, wptX, wptY);
+					// ro.insert(i, (int) pnt.x, (int) pnt.y); // TODO more correct
+					ro.setPointTypes(i, np.types.toArray());
+					np.distance = dist;
+					np.connected = ro;
+					np.pointIndex = i;
+					i++;
+				}
+				
+				x = nx;
+				y = ny;
+				
+			}
+		}
+	}
+	
+
+
 
 	public boolean checkIfMemoryLimitCritical(long memoryLimit) {
 		return getCurrentEstimatedSize() > 0.9 * memoryLimit;