add direction on matching wpts

brouter-mapaccess/src/main/java/btools/mapaccess/WaypointMatcherImpl.java

@@ -1,9 +1,12 @@
 package btools.mapaccess;
 
 import java.util.List;
+import java.util.Collections;
+import java.util.Comparator;
 
 import btools.codec.WaypointMatcher;
 import btools.util.CheapRuler;
+import btools.util.CheapAngleMeter;
 
 /**
  * the WaypointMatcher is feeded by the decoder with geoemtries of ways that are
@@ -13,6 +16,8 @@ import btools.util.CheapRuler;
  * match for each waypoint
  */
 public final class WaypointMatcherImpl implements WaypointMatcher {
+  private static final int MAX_POINTS = 5;
+
   private List<MatchedWaypoint> waypoints;
   private OsmNodePairSet islandPairs;
 
@@ -24,12 +29,37 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
   private int lonLast;
   private int latLast;
 
+  private Comparator<MatchedWaypoint> comparator;
+
   public WaypointMatcherImpl(List<MatchedWaypoint> waypoints, double maxDistance, OsmNodePairSet islandPairs) {
     this.waypoints = waypoints;
     this.islandPairs = islandPairs;
+    MatchedWaypoint last = null;
     for (MatchedWaypoint mwp : waypoints) {
       mwp.radius = maxDistance;
+      if (last != null && mwp.directionToNext == -1) {
+        last.directionToNext = CheapAngleMeter.getDirection(last.waypoint.ilon, last.waypoint.ilat, mwp.waypoint.ilon, mwp.waypoint.ilat);
+      }
+      last = mwp;
     }
+    // last point has no angle so we are looking back
+    int lastidx = waypoints.size() - 2;
+    if (lastidx < 0) {
+      last.directionToNext = -1;
+    } else {
+      last.directionToNext = CheapAngleMeter.getDirection(last.waypoint.ilon, last.waypoint.ilat, waypoints.get(lastidx).waypoint.ilon, waypoints.get(lastidx).waypoint.ilat);
+    }
+
+    // sort result list
+    comparator = new Comparator<MatchedWaypoint>() {
+      @Override
+      public int compare(MatchedWaypoint mw1, MatchedWaypoint mw2) {
+        int cmpDist = Double.compare(mw1.radius, mw2.radius);
+        if (cmpDist != 0) return cmpDist;
+        return Double.compare(mw1.directionDiff, mw2.directionDiff);
+      }
+    };
+
   }
 
   private void checkSegment(int lon1, int lat1, int lon2, int lat2) {
@@ -46,8 +76,8 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
     if (d == 0.)
       return;
 
-    int len = waypoints.size();
+    //for ( MatchedWaypoint mwp : waypoints )
-    for (int i = 0; i < len; i++) {
+    for (int i = 0; i < waypoints.size(); i++) {
       MatchedWaypoint mwp = waypoints.get(i);
 
       if (mwp.direct &&
@@ -65,7 +95,6 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
       }
 
       OsmNode wp = mwp.waypoint;
-
       double x1 = (lon1 - wp.ilon) * dlon2m;
       double y1 = (lat1 - wp.ilat) * dlat2m;
       double x2 = (lon2 - wp.ilon) * dlon2m;
@@ -74,7 +103,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
       double r22 = x2 * x2 + y2 * y2;
       double radius = Math.abs(r12 < r22 ? y1 * dx - x1 * dy : y2 * dx - x2 * dy) / d;
 
-      if (radius < mwp.radius) {
+      if (radius <= mwp.radius) {
         double s1 = x1 * dx + y1 * dy;
         double s2 = x2 * dx + y2 * dy;
 
@@ -141,11 +170,67 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
     if (anyUpdate) {
       for (MatchedWaypoint mwp : waypoints) {
         if (mwp.hasUpdate) {
+          double angle = CheapAngleMeter.getDirection(lonStart, latStart, lonTarget, latTarget);
+          double diff = CheapAngleMeter.getDifferenceFromDirection(mwp.directionToNext, angle);
+
           mwp.hasUpdate = false;
-          mwp.node1 = new OsmNode(lonStart, latStart);
+
-          mwp.node2 = new OsmNode(lonTarget, latTarget);
+          MatchedWaypoint mw = new MatchedWaypoint();
+          mw.waypoint = new OsmNode();
+          mw.waypoint.ilon = mwp.waypoint.ilon;
+          mw.waypoint.ilat = mwp.waypoint.ilat;
+          mw.crosspoint = new OsmNode();
+          mw.crosspoint.ilon = mwp.crosspoint.ilon;
+          mw.crosspoint.ilat = mwp.crosspoint.ilat;
+          mw.node1 = new OsmNode(lonStart, latStart);
+          mw.node2 = new OsmNode(lonTarget, latTarget);
+          mw.name = mwp.name + "_w_" + mwp.crosspoint.hashCode();
+          mw.radius = mwp.radius;
+          mw.directionDiff = diff;
+          mw.directionToNext = mwp.directionToNext;
+
+          updateWayList(mwp.wayNearest, mw);
+
+          // revers
+          angle = CheapAngleMeter.getDirection(lonTarget, latTarget, lonStart, latStart);
+          diff = CheapAngleMeter.getDifferenceFromDirection(mwp.directionToNext, angle);
+          mw = new MatchedWaypoint();
+          mw.waypoint = new OsmNode();
+          mw.waypoint.ilon = mwp.waypoint.ilon;
+          mw.waypoint.ilat = mwp.waypoint.ilat;
+          mw.crosspoint = new OsmNode();
+          mw.crosspoint.ilon = mwp.crosspoint.ilon;
+          mw.crosspoint.ilat = mwp.crosspoint.ilat;
+          mw.node1 = new OsmNode(lonTarget, latTarget);
+          mw.node2 = new OsmNode(lonStart, latStart);
+          mw.name = mwp.name + "_w2_" + mwp.crosspoint.hashCode();
+          mw.radius = mwp.radius;
+          mw.directionDiff = diff;
+          mw.directionToNext = mwp.directionToNext;
+
+          updateWayList(mwp.wayNearest, mw);
+
+          MatchedWaypoint way = mwp.wayNearest.get(0);
+          mwp.crosspoint.ilon = way.crosspoint.ilon;
+          mwp.crosspoint.ilat = way.crosspoint.ilat;
+          mwp.node1 = new OsmNode(way.node1.ilon, way.node1.ilat);
+          mwp.node2 = new OsmNode(way.node2.ilon, way.node2.ilat);
+          mwp.directionDiff = way.directionDiff;
+          mwp.radius = way.radius;
+
         }
       }
     }
   }
+
+  // check limit of list size (avoid long runs)
+  void updateWayList(List<MatchedWaypoint> ways, MatchedWaypoint mw) {
+    ways.add(mw);
+    // use only shortest distances by smallest direction difference
+    Collections.sort(ways, comparator);
+    if (ways.size() > MAX_POINTS) ways.remove(MAX_POINTS);
+
+  }
+
+
 }

brouter-util/src/main/java/btools/util/CheapAngleMeter.java

@@ -46,4 +46,30 @@ public final class CheapAngleMeter {
     }
     return offset + sinp * (57.4539 + s2 * (9.57565 + s2 * (4.30904 + s2 * 2.56491)));
   }
+
+  public static double getAngle(int lon1, int lat1, int lon2, int lat2) {
+    double res = 0;
+    double xdiff = lat2 - lat1;
+    double ydiff = lon2 - lon1;
+    res = Math.toDegrees(Math.atan2(ydiff, xdiff));
+    return res;
+  }
+
+  public static double getDirection(int lon1, int lat1, int lon2, int lat2) {
+    double res = getAngle(lon1, lat1, lon2, lat2);
+    return normalize(res);
+  }
+
+  public static double normalize(double a) {
+    return a >= 360 ? a - (360 * (int) (a / 360))
+      : a < 0 ? a - (360 * ((int) (a / 360) - 1)) : a;
+  }
+
+  public static double getDifferenceFromDirection(double b1, double b2) {
+    double r = (b2 - b1) % 360.0;
+    if (r < -180.0) r += 360.0;
+    if (r >= 180.0) r -= 360.0;
+    return Math.abs(r);
+  }
+
 }