diff --git a/brouter-core/src/main/java/btools/router/OsmTrack.java b/brouter-core/src/main/java/btools/router/OsmTrack.java index 98d5e93bc..ac278d18e 100644 --- a/brouter-core/src/main/java/btools/router/OsmTrack.java +++ b/brouter-core/src/main/java/btools/router/OsmTrack.java @@ -285,6 +285,7 @@ public static OsmTrack readBinary(String filename, OsmNodeNamed newEp, long[] no } dis.close(); } catch (Exception e) { + t = null; if (debugInfo != null) { debugInfo.append("Error reading rawTrack: " + e); } diff --git a/brouter-core/src/main/java/btools/router/RoutingEngine.java b/brouter-core/src/main/java/btools/router/RoutingEngine.java index 105ed0952..f940f2f48 100644 --- a/brouter-core/src/main/java/btools/router/RoutingEngine.java +++ b/brouter-core/src/main/java/btools/router/RoutingEngine.java @@ -20,6 +20,8 @@ import btools.mapaccess.OsmLinkHolder; import btools.mapaccess.OsmNode; import btools.mapaccess.OsmNodePairSet; +import btools.util.CheapAngleMeter; +import btools.util.CheapRuler; import btools.util.CompactLongMap; import btools.util.SortedHeap; import btools.util.StackSampler; @@ -30,6 +32,8 @@ public class RoutingEngine extends Thread { public final static int BROUTER_ENGINEMODE_SEED = 1; public final static int BROUTER_ENGINEMODE_GETELEV = 2; + public final static int BROUTER_ENGINEMODE_PREPARE_REROUTE = 6; + private NodesCache nodesCache; private SortedHeap openSet = new SortedHeap<>(); private boolean finished = false; @@ -158,6 +162,7 @@ public void doRun(long maxRunningTime) { switch (engineMode) { case BROUTER_ENGINEMODE_ROUTING: + case BROUTER_ENGINEMODE_PREPARE_REROUTE: if (waypoints.size() < 2) { throw new IllegalArgumentException("we need two lat/lon points at least!"); } @@ -189,6 +194,9 @@ public void doRouting(long maxRunningTime) { ArrayList messageList = new ArrayList<>(); for (int i = 0; ; i++) { track = findTrack(refTracks, lastTracks); + + if (engineMode==BROUTER_ENGINEMODE_PREPARE_REROUTE) break; // no output for rerouting prepare + track.message = "track-length = " + track.distance + " filtered ascend = " + track.ascend + " plain-ascend = " + track.plainAscend + " cost=" + track.cost; if (track.energy != 0) { @@ -575,6 +583,10 @@ private OsmTrack tryFindTrack(OsmTrack[] refTracks, OsmTrack[] lastTracks) { boolean dirty = found && nearbyTrack.isDirty; logInfo("read referenceTrack, found=" + found + " dirty=" + dirty + " " + debugInfo); } + if (nearbyTrack != null && + engineMode==BROUTER_ENGINEMODE_PREPARE_REROUTE) { + return null; // already rerouting prepared + } } if (matchedWaypoints == null) { // could exist from the previous alternative level @@ -587,6 +599,22 @@ private OsmTrack tryFindTrack(OsmTrack[] refTracks, OsmTrack[] lastTracks) { matchedWaypoints.add(mwp); } matchWaypointsToNodes(matchedWaypoints); + if (routingContext.startDirection != null) { + // add a nogo not to turn back + double angle = CheapAngleMeter.normalize(180 + routingContext.startDirection); + int[] np = CheapRuler.destination(matchedWaypoints.get(0).crosspoint.ilon, matchedWaypoints.get(0).crosspoint.ilat, 10, angle); + OsmNodeNamed n = new OsmNodeNamed(); + n.name = "nogo8"; + n.ilon = np[0]; + n.ilat = np[1]; + n.isNogo = true; + n.radius = 8; + n.nogoWeight = 9999; + if (routingContext.nogopoints == null) { + routingContext.nogopoints = new ArrayList<>(); + } + routingContext.nogopoints.add(n); + } routingContext.checkMatchedWaypointAgainstNogos(matchedWaypoints); @@ -631,7 +659,8 @@ private OsmTrack tryFindTrack(OsmTrack[] refTracks, OsmTrack[] lastTracks) { routingContext.inverseDirection = false; wptIndex = i + 1; } else { - seg = searchTrack(matchedWaypoints.get(i), matchedWaypoints.get(i + 1), i == matchedWaypoints.size() - 2 ? nearbyTrack : null, refTracks[i]); + //seg = searchTrack(matchedWaypoints.get(i), matchedWaypoints.get(i + 1), i == matchedWaypoints.size() - 2 ? nearbyTrack : null, refTracks[i]); + seg = searchTrack(matchedWaypoints.get(i), matchedWaypoints.get(i + 1), nearbyTrack, refTracks[i]); wptIndex = i; } if (seg == null) @@ -1048,7 +1077,18 @@ private OsmTrack searchRoutedTrack(MatchedWaypoint startWp, MatchedWaypoint endW track.nogoChecksums = routingContext.getNogoChecksums(); track.profileTimestamp = routingContext.profileTimestamp; track.isDirty = isDirty; - foundRawTrack = track; + if (foundRawTrack == null) { + foundRawTrack = track; + } else { + for (OsmPathElement n : track.nodes) { + foundRawTrack.nodes.add(n); + } + foundRawTrack.endPoint = endWp; + } + if (engineMode==BROUTER_ENGINEMODE_PREPARE_REROUTE) { + return null; // rerouting prepared + } + } if (!wasClean && isDirty) { diff --git a/brouter-server/src/main/java/btools/server/BRouter.java b/brouter-server/src/main/java/btools/server/BRouter.java index a6dfe1037..1600c7e3f 100644 --- a/brouter-server/src/main/java/btools/server/BRouter.java +++ b/brouter-server/src/main/java/btools/server/BRouter.java @@ -105,13 +105,28 @@ public static void main(String[] args) throws Exception { if (engineMode == RoutingEngine.BROUTER_ENGINEMODE_GETELEV) { re = new RoutingEngine("testinfo", null, new File(args[0]), wplist, rc, engineMode); } else { + rc.rawTrackPath = "testtrack.raw"; re = new RoutingEngine("testtrack", null, new File(args[0]), wplist, rc, engineMode); } re.doRun(0); + + if (engineMode == RoutingEngine.BROUTER_ENGINEMODE_ROUTING || + engineMode == RoutingEngine.BROUTER_ENGINEMODE_PREPARE_REROUTE) { + // store new reference track if any + // (can exist for timed-out search) + if (re.getFoundRawTrack() != null) { + try { + re.getFoundRawTrack().writeBinary(rc.rawTrackPath); + } catch (Exception e) { + e.printStackTrace(); + } + } + } + } catch (Exception e) { System.out.println(e.getMessage()); } - + } diff --git a/brouter-util/src/main/java/btools/util/CheapRuler.java b/brouter-util/src/main/java/btools/util/CheapRuler.java index 7a7dc8f31..bdd93607f 100644 --- a/brouter-util/src/main/java/btools/util/CheapRuler.java +++ b/brouter-util/src/main/java/btools/util/CheapRuler.java @@ -77,4 +77,21 @@ public static double distance(int ilon1, int ilat1, int ilon2, int ilat2) { double dlat = (ilat1 - ilat2) * kxky[1]; return Math.sqrt(dlat * dlat + dlon * dlon); // in m } + + public static int[] destination(int lon1, int lat1, double distance, double angle) { + + double[] lonlat2m = CheapRuler.getLonLatToMeterScales(lat1); + double lon2m = lonlat2m[0]; + double lat2m = lonlat2m[1]; + angle = 90. - angle; + double st = Math.sin(angle * Math.PI / 180.); + double ct = Math.cos(angle * Math.PI / 180.); + + int lon2 = (int) (0.5 + lon1 + ct * distance / lon2m); + int lat2 = (int) (0.5 + lat1 + st * distance / lat2m); + int[] ret = new int[2]; + ret[0] = lon2; + ret[1] = lat2; + return ret; + } }