Merge pull request #674 from afischerdev/rerouting

App: Rerouting

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);
           }

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<OsmPath> 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<String> 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) {

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());
     }
-    
+
   }
 
 

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;
+  }
 }