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,
+//		return String.format("Road [%d, '%s', '%s']", id / 64, name, rf, poin);
-				rf == null ? "" : 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) {
@@ -1119,7 +1119,7 @@ public class RouteDataObject {
 	}
 
 	public void setPointTypes(int pntInd, int[] array) {
-		if (pointTypes == null || pointTypes.length < pntInd) {
+		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];

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/RoutingConfiguration.java

@@ -2,8 +2,10 @@ package net.osmand.router;
 
 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;
@@ -13,6 +15,7 @@ import org.xmlpull.v1.XmlPullParserException;
 
 import net.osmand.GPXUtilities.WptPt;
 import net.osmand.PlatformUtil;
+import net.osmand.data.QuadRect;
 import net.osmand.data.QuadTree;
 import net.osmand.router.GeneralRouter.GeneralRouterProfile;
 import net.osmand.router.GeneralRouter.RouteAttributeContext;
@@ -22,6 +25,7 @@ import net.osmand.util.Algorithms;
 public class RoutingConfiguration {
 
 	public static final int DEFAULT_MEMORY_LIMIT = 30;
+	static final String ATTACHED_INFO_WPT_ID = "AID";
 	public final float DEVIATION_RADIUS = 3000;
 	public Map<String, String> attributes = new LinkedHashMap<String, String>();
 
@@ -53,18 +57,22 @@ public class RoutingConfiguration {
 	
 	
 	// extra points to be inserted in ways (quad tree is based on 31 coords)
-	public QuadTree<WptPt> directionPoints;
+	private QuadTree<WptPt> directionPoints;
 	public int directionPointsRadius = 30; // 30 m
 	
+	public QuadTree<WptPt> getDirectionPoints() {
+		return directionPoints;
+	}
+	
 	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<WptPt> directionPointsBuilder;
 
 		public Builder() {
-
 		}
 
 		public Builder(Map<String, String> defaultAttributes) {
@@ -113,9 +121,24 @@ public class RoutingConfiguration {
 				i.memoryLimitation = memoryLimitMB * (1l << 20);
 			}
 			i.planRoadDirection = parseSilentInt(getAttribute(i.router, "planRoadDirection"), i.planRoadDirection);
+			i.directionPoints = this.directionPointsBuilder;
 //			i.planRoadDirection = 1;
 			return i;
 		}
+		
+		public Builder setDirectionPoints(QuadTree<WptPt> directionPoints) {
+			if (directionPoints != null) {
+				List<WptPt> lst = directionPoints.queryInBox(new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE),
+						new ArrayList<WptPt>());
+				for (WptPt l : lst) {
+					if (l.getExtensionsToRead().containsKey(ATTACHED_INFO_WPT_ID)) {
+						l.getExtensionsToWrite().remove(ATTACHED_INFO_WPT_ID);
+					}
+				}
+			}
+			this.directionPointsBuilder = directionPoints;
+			return this;
+		}
 
 		public Set<Long> getImpassableRoadLocations() {
 			return impassableRoadLocations;

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

@@ -11,7 +11,6 @@ import java.util.LinkedHashMap;
 import java.util.List;
 import java.util.Map;
 import java.util.Map.Entry;
-import java.util.Set;
 
 import org.apache.commons.logging.Log;
 
@@ -31,6 +30,7 @@ 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;
@@ -512,7 +512,7 @@ public class RoutingContext {
 						}
 					}
 					// connect direction points
-					if(config.directionPoints != null) {
+					if(config.getDirectionPoints() != null) {
 						connectDirectionPoints(ts, (int) (xloc << zmShift), (int) (yloc << zmShift), 
 							(int) ((xloc + 1) << zmShift), (int) ((yloc + 1) << zmShift));
 					}
@@ -524,67 +524,134 @@ public class RoutingContext {
 	}
 
 	
-	private static final String ATTACHED_ID = "AID";
 	private void connectDirectionPoints(RoutingSubregionTile ts, int minx, int miny, int maxx, int maxy) {
-		List<WptPt> points = config.directionPoints.queryInBox(new QuadRect(minx, miny, maxx, maxy), new ArrayList<>());
+		List<WptPt> points = config.getDirectionPoints().queryInBox(new QuadRect(minx, miny, maxx, maxy), new ArrayList<>());
 		for (WptPt connectPoint : points) {
 			TIntArrayList types = new TIntArrayList();
 			for (Entry<String, String> e : connectPoint.getExtensionsToRead().entrySet()) {
-				if(e.getKey().equals(ATTACHED_ID)) {
+				if (e.getKey().equals(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) {
-					types.clear();
+					continue;
-					break;
+				} 
-				}
 				int type = ts.subregion.routeReg.searchRouteEncodingRule(e.getKey(), e.getValue());
-				if(type != -1) {
+				if (type != -1) {
 					types.add(type);
 				}
 			}
 			// don't attach empty points
-			if(types.size() == 0) {
+			if (types.size() == 0) {
 				continue;
 			}
-			int y = MapUtils.get31TileNumberY(connectPoint.lat);
+			// search road to insert
-			int x = MapUtils.get31TileNumberX(connectPoint.lon);
+			if (!connectPoint.getExtensionsToRead().containsKey(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) {
-			double closest = Integer.MAX_VALUE;
+				searchRoadToInsert(ts, connectPoint);
-			int xc = 0, yc = 0;
-			long[] keys = ts.routes.keys();
-			for (long k : keys) {
-				// long k = (((long) x31) << 31) + (long) y31;
-				int xp = (int) (k >> 31);
-				int yp = (int) (k - (xp << 31));
-				double dist = MapUtils.squareRootDist31(x, y, xp, yp);
-				if (dist < closest) {
-					xc = xp;
-					yc = yp;
-					closest = dist;
-				}
 			}
-			// config.directionPointsRadius
+			if (!connectPoint.getExtensionsToRead().containsKey(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) {
-			if (closest < config.directionPointsRadius) {
+				continue;
-				long k = (((long) xc) << 31) + (long) yc;
+			}
-				RouteSegment segment = ts.routes.get(k);
+			String[] vls = connectPoint.getExtensionsToRead().get(RoutingConfiguration.ATTACHED_INFO_WPT_ID).split(":");
-				int pntInd = -1;
+			if (vls.length != 3) {
-				for(int i = 0; i < segment.getRoad().getPointsLength() - 1; i ++) {
+				continue;
-					if(xc == segment.getRoad().getPoint31XTile(i) && yc == segment.getRoad().getPoint31YTile(i)) {
+			}
-						pntInd = i;
+			long roadId = Long.parseLong(vls[0]);
-					}
+			int xins = Integer.parseInt(vls[1]);
-				}
+			int yins = Integer.parseInt(vls[2]);
-				if (pntInd != -1) {
+			// insert points if needed 
-					System.out.println(
+			if (roadId != -1) {
-							"INSERT INTO " + segment.getRoad() + " " + connectPoint.getLatitude() + " " + connectPoint.getLongitude());
+				int wptX = MapUtils.get31TileNumberX(connectPoint.lon);
-					segment.getRoad().insert(pntInd, x, y);
+				int wptY = MapUtils.get31TileNumberY(connectPoint.lat);
-					connectPoint.getExtensionsToWrite().put(ATTACHED_ID, segment.getRoad().getId() +"");
+				insertPoint(ts, roadId, xins, yins, xins, yins, types, wptX, wptY);
-					if (types.size() > 0) {
-						System.out.println(" >>> INSERT INTO " + segment.getRoad() + " " + connectPoint.getLatitude() + " "
-								+ connectPoint.getLongitude());
-						segment.getRoad().setPointTypes(pntInd, types.toArray());
-					}
-				}
-				
 			}
 		}
 	}
 
+	private void searchRoadToInsert(RoutingSubregionTile ts, WptPt connectPoint) {
+		long roadId = -1;
+		int wptX = MapUtils.get31TileNumberX(connectPoint.lon);
+		int wptY = MapUtils.get31TileNumberY(connectPoint.lat);
+		double closest = Integer.MAX_VALUE;
+		int xc = 0, yc = 0;
+		long[] keys = ts.routes.keys();
+		RouteSegment cl = null;
+		// faster search via all roadObjects?
+		for (long k : keys) {
+			// long k = (((long) x31) << 31) + (long) y31;
+			int xp = (int) (k >> 31);
+			int yp = (int) (k - (xp << 31));
+			RouteSegment sg = ts.routes.get(k);
+			while (sg != null) {
+				if (sg.getRoad().getPointsLength() > sg.segStart + 1) {
+					QuadPoint pnt = MapUtils.getProjectionPoint31(wptX, wptY, sg.getRoad().getPoint31XTile(sg.segStart),
+							sg.getRoad().getPoint31YTile(sg.segStart), sg.getRoad().getPoint31XTile(sg.segStart + 1),
+							sg.getRoad().getPoint31YTile(sg.segStart + 1));
+					double dist = MapUtils.squareRootDist31(wptX, wptY, (int) pnt.x, (int) pnt.y);
+					if (dist < closest) {
+						cl = sg;
+						xc = xp;
+						yc = yp;
+						closest = dist;
+					}
+
+				}
+				sg = sg.next;
+			}
+			
+		}
+		// TODO search proper where insert
+		if (closest < config.directionPointsRadius) {
+			System.out.println(cl.getRoad() + " " + closest);
+			connectPoint.getExtensionsToWrite().put(RoutingConfiguration.ATTACHED_INFO_WPT_ID,
+					cl.getRoad().getId() + ":" + xc + ":" + yc);
+		}
+	}
+
+	private RouteSegment insertPoint(RoutingSubregionTile ts, long roadId, int xat, int yat, 
+			int xins, int yins, TIntArrayList wptTypes, int wptX, int wptY) {
+		long k = (((long) xat) << 31) + (long) yat;
+		RouteSegment s = ts.routes.get(k);
+		RouteSegment res = null;
+		while (s != null) {
+			if (s.getRoad().getId() == roadId) {
+				res = s;
+				break;
+			}
+			s = s.next;
+		}
+		if (res != null) {
+			for (int i = 0; i < res.getRoad().getPointsLength(); i++) {
+				if (xins == res.getRoad().getPoint31XTile(i) && yins == res.getRoad().getPoint31YTile(i)) {
+					// TODO not correct for multiple points
+					boolean after = i != res.getRoad().getPointsLength() - 1;
+					if (wptX != res.getRoad().getPoint31XTile(after ? i + 1 : i - 1) || wptY != res.getRoad().getPoint31YTile(after ? i + 1 : i - 1)) {
+						System.out.println(String.format("INSERT %s %s [%.5f, %.5f] ", ts.subregion.hashCode() + "",  res.getRoad(), MapUtils.get31LatitudeY(wptY), MapUtils.get31LongitudeX(wptX)));
+						res.getRoad().insert(after ? i + 1 : i, wptX, wptY);
+						res.getRoad().setPointTypes(after ? i + 1 : i, wptTypes.toArray());
+						// iterate and insert in all segment points if needed
+						for (int j = i + 1; j < res.getRoad().getPointsLength(); j++) {
+							int xts = res.getRoad().getPoint31XTile(j);
+							int yts = res.getRoad().getPoint31YTile(j);
+							long ks = (((long) xts) << 31) + (long) yts;
+							RouteSegment ss = ts.routes.get(ks);
+							RouteSegment sres = null;
+							while (ss != null) {
+								if (ss.getRoad().getId() == roadId) {
+									sres = ss;
+									break;
+								}
+								ss = ss.next;
+							}
+							if (sres != null) {
+								sres.segStart++;
+							}
+						}
+						System.out.println(String.format("INSERA %s %s [%.5f, %.5f] ", ts.subregion.hashCode() + "",  res.getRoad(), MapUtils.get31LatitudeY(wptY), MapUtils.get31LongitudeX(wptX)));
+					}
+					break;
+				}
+			}
+		}
+		return res;
+	}
+
 	public boolean checkIfMemoryLimitCritical(long memoryLimit) {
 		return getCurrentEstimatedSize() > 0.9 * memoryLimit;
 	}