From 7f44fc3c6ea942a17f5633263a4afecfc400e927 Mon Sep 17 00:00:00 2001 From: Victor Shcherb Date: Thu, 25 Mar 2021 16:54:36 +0100 Subject: [PATCH] More precise calculation of connected points --- .../net/osmand/binary/RouteDataObject.java | 6 +- .../net/osmand/router/BinaryRoutePlanner.java | 2 +- .../osmand/router/RoutingConfiguration.java | 27 ++- .../net/osmand/router/RoutingContext.java | 161 +++++++++++++----- 4 files changed, 143 insertions(+), 53 deletions(-) diff --git a/OsmAnd-java/src/main/java/net/osmand/binary/RouteDataObject.java b/OsmAnd-java/src/main/java/net/osmand/binary/RouteDataObject.java index ae54564cb5..d732efc0d9 100644 --- a/OsmAnd-java/src/main/java/net/osmand/binary/RouteDataObject.java +++ b/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 [%d, '%s', '%s']", id / 64, name, rf, poin); + 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]; diff --git a/OsmAnd-java/src/main/java/net/osmand/router/BinaryRoutePlanner.java b/OsmAnd-java/src/main/java/net/osmand/router/BinaryRoutePlanner.java index 0769e1390c..7e937fa5f3 100644 --- a/OsmAnd-java/src/main/java/net/osmand/router/BinaryRoutePlanner.java +++ b/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; diff --git a/OsmAnd-java/src/main/java/net/osmand/router/RoutingConfiguration.java b/OsmAnd-java/src/main/java/net/osmand/router/RoutingConfiguration.java index 65ea507434..d56d249603 100644 --- a/OsmAnd-java/src/main/java/net/osmand/router/RoutingConfiguration.java +++ b/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 attributes = new LinkedHashMap(); @@ -53,18 +57,22 @@ public class RoutingConfiguration { // extra points to be inserted in ways (quad tree is based on 31 coords) - public QuadTree directionPoints; + private QuadTree directionPoints; public int directionPointsRadius = 30; // 30 m + public QuadTree getDirectionPoints() { + return directionPoints; + } + public static class Builder { // Design time storage private String defaultRouter = ""; private Map routers = new LinkedHashMap<>(); private Map attributes = new LinkedHashMap<>(); private Set impassableRoadLocations = new HashSet<>(); + private QuadTree directionPointsBuilder; public Builder() { - } public Builder(Map 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 directionPoints) { + if (directionPoints != null) { + List lst = directionPoints.queryInBox(new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE), + new ArrayList()); + 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 getImpassableRoadLocations() { return impassableRoadLocations; diff --git a/OsmAnd-java/src/main/java/net/osmand/router/RoutingContext.java b/OsmAnd-java/src/main/java/net/osmand/router/RoutingContext.java index 265a0a07c3..84f4f1c1df 100644 --- a/OsmAnd-java/src/main/java/net/osmand/router/RoutingContext.java +++ b/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 points = config.directionPoints.queryInBox(new QuadRect(minx, miny, maxx, maxy), new ArrayList<>()); + List points = config.getDirectionPoints().queryInBox(new QuadRect(minx, miny, maxx, maxy), new ArrayList<>()); for (WptPt connectPoint : points) { TIntArrayList types = new TIntArrayList(); for (Entry e : connectPoint.getExtensionsToRead().entrySet()) { - if(e.getKey().equals(ATTACHED_ID)) { - types.clear(); - break; - } + if (e.getKey().equals(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) { + continue; + } 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); - int x = MapUtils.get31TileNumberX(connectPoint.lon); - double closest = Integer.MAX_VALUE; - 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; - } + // search road to insert + if (!connectPoint.getExtensionsToRead().containsKey(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) { + searchRoadToInsert(ts, connectPoint); } - // config.directionPointsRadius - if (closest < config.directionPointsRadius) { - long k = (((long) xc) << 31) + (long) yc; - RouteSegment segment = ts.routes.get(k); - int pntInd = -1; - for(int i = 0; i < segment.getRoad().getPointsLength() - 1; i ++) { - if(xc == segment.getRoad().getPoint31XTile(i) && yc == segment.getRoad().getPoint31YTile(i)) { - pntInd = i; - } - } - if (pntInd != -1) { - System.out.println( - "INSERT INTO " + segment.getRoad() + " " + connectPoint.getLatitude() + " " + connectPoint.getLongitude()); - segment.getRoad().insert(pntInd, x, y); - connectPoint.getExtensionsToWrite().put(ATTACHED_ID, segment.getRoad().getId() +""); - if (types.size() > 0) { - System.out.println(" >>> INSERT INTO " + segment.getRoad() + " " + connectPoint.getLatitude() + " " - + connectPoint.getLongitude()); - segment.getRoad().setPointTypes(pntInd, types.toArray()); - } - } - + if (!connectPoint.getExtensionsToRead().containsKey(RoutingConfiguration.ATTACHED_INFO_WPT_ID)) { + continue; + } + String[] vls = connectPoint.getExtensionsToRead().get(RoutingConfiguration.ATTACHED_INFO_WPT_ID).split(":"); + if (vls.length != 3) { + continue; + } + long roadId = Long.parseLong(vls[0]); + int xins = Integer.parseInt(vls[1]); + int yins = Integer.parseInt(vls[2]); + // insert points if needed + if (roadId != -1) { + int wptX = MapUtils.get31TileNumberX(connectPoint.lon); + int wptY = MapUtils.get31TileNumberY(connectPoint.lat); + insertPoint(ts, roadId, xins, yins, xins, yins, types, wptX, wptY); } } } + 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; }