Make nogo weight proportional to nogo distance

Compute the nogo cost as proportional to the length of the segment
inside the nogo area.

brouter-core/src/main/java/btools/router/OsmNodeNamed.java

@@ -6,6 +6,7 @@
 package btools.router;
 
 import btools.mapaccess.OsmNode;
+import btools.util.CheapRulerSingleton;
 
 public class OsmNodeNamed extends OsmNode
 {
@@ -24,6 +25,55 @@ public class OsmNodeNamed extends OsmNode
     }
   }
 
+  public double distanceWithinRadius(int lon1, int lat1, int lon2, int lat2, double totalSegmentLength) {
+    double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( (lat1 + lat2) >> 1 );
+
+    double realRadius = radius * 110984.;
+    boolean isFirstPointWithinCircle = CheapRulerSingleton.distance(lon1, lat1, ilon, ilat) < realRadius;
+    boolean isLastPointWithinCircle = CheapRulerSingleton.distance(lon2, lat2, ilon, ilat) < realRadius;
+    // First point is within the circle
+    if (isFirstPointWithinCircle) {
+      // Last point is within the circle
+      if (isLastPointWithinCircle) {
+        return totalSegmentLength;
+      }
+      // Last point is not within the circle
+      // Just swap points and go on with first first point not within the
+      // circle now.
+      // Swap longitudes
+      int tmp = lon2;
+      lon2 = lon1;
+      lon1 = tmp;
+      // Swap latitudes
+      tmp = lat2;
+      lat2 = lat1;
+      lat1 = tmp;
+      // Fix boolean values
+      isLastPointWithinCircle = isFirstPointWithinCircle;
+      isFirstPointWithinCircle = false;
+    }
+    // Distance between the initial point and projection of center of
+    // the circle on the current segment.
+    double initialToProject = (
+      (lon2 - lon1) * (ilon - lon1) * lonlat2m[0] * lonlat2m[0]
+      + (lat2 - lat1) * (ilat - lat1) * lonlat2m[1] * lonlat2m[1]
+    ) / totalSegmentLength;
+    // Distance between the initial point and the center of the circle.
+    double initialToCenter = CheapRulerSingleton.distance(ilon, ilat, lon1, lat1);
+    // Half length of the segment within the circle
+    double halfDistanceWithin = Math.sqrt(
+      realRadius*realRadius - (
+        initialToCenter*initialToCenter -
+        initialToProject*initialToProject
+      )
+    );
+    // Last point is within the circle
+    if (isLastPointWithinCircle) {
+      return halfDistanceWithin + (totalSegmentLength - initialToProject);
+    }
+    return 2 * halfDistanceWithin;
+  }
+
   public static OsmNodeNamed decodeNogo( String s )
   {
     OsmNodeNamed n = new OsmNodeNamed();

brouter-core/src/main/java/btools/router/OsmNogoPolygon.java

@@ -297,6 +297,90 @@ public class OsmNogoPolygon extends OsmNodeNamed
     return wn != 0;
   }
 
+  /**
+   * Compute the length of the segment within the polygon.
+   *
+   * @param lon1 Integer longitude of the first point of the segment.
+   * @param lat1 Integer latitude of the first point of the segment.
+   * @param lon2 Integer longitude of the last point of the segment.
+   * @param lat2 Integer latitude of the last point of the segment.
+   *
+   * @return The length, in meters, of the portion of the segment which is
+   *         included in the polygon.
+   */
+  public double distanceWithinPolygon(int lon1, int lat1, int lon2, int lat2) {
+    double distance = 0.;
+
+    // Extremities of the segments
+    final Point p1 = new Point (lon1, lat1);
+    final Point p2 = new Point (lon2, lat2);
+
+    Point previousIntersectionOnSegment = null;
+    if (isWithin(lon1, lat1))
+    {
+      // Start point of the segment is within the polygon, this is the first
+      // "intersection".
+      previousIntersectionOnSegment = p1;
+    }
+
+    // Loop over edges of the polygon to find intersections
+    int i_last = points.size() - 1;
+    for (int i = (isClosed ? 0 : 1), j = (isClosed ? i_last : 0); i <= i_last; j = i++)
+    {
+      Point edgePoint1 = points.get(j);
+      Point edgePoint2 = points.get(i);
+      int intersectsEdge = intersect2D_2Segments(p1, p2, edgePoint1, edgePoint2);
+      if (intersectsEdge == 1)
+      {
+        // Intersects in a single point
+        // Let's find this intersection point
+        int xdiffSegment = lon1 - lon2;
+        int xdiffEdge = edgePoint1.x - edgePoint2.x;
+        int ydiffSegment = lat1 - lat2;
+        int ydiffEdge = edgePoint1.y - edgePoint2.y;
+        int div = xdiffSegment * ydiffEdge - xdiffEdge * ydiffSegment;
+        long dSegment = (long) lon1 * (long) lat2 - (long) lon2 * (long) lat1;
+        long dEdge = (long) edgePoint1.x * (long) edgePoint2.y - (long) edgePoint2.x * (long) edgePoint1.y;
+        // Coordinates of the intersection
+        Point intersection = new Point(
+          (int) ((dSegment * xdiffEdge - dEdge * xdiffSegment) / div),
+          (int) ((dSegment * ydiffEdge - dEdge * ydiffSegment) / div)
+        );
+        if (
+          previousIntersectionOnSegment != null
+          && isWithin(
+            (intersection.x + previousIntersectionOnSegment.x) >> 1,
+            (intersection.y + previousIntersectionOnSegment.y) >> 1
+          )
+        ) {
+          // There was a previous match within the polygon and this part of the
+          // segment is within the polygon.
+          distance += CheapRulerSingleton.distance(
+            previousIntersectionOnSegment.x, previousIntersectionOnSegment.y,
+            intersection.x, intersection.y
+          );
+        }
+        previousIntersectionOnSegment = intersection;
+      }
+      else if (intersectsEdge == 2) {
+        // Segment and edge overlaps
+        distance += CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+      }
+    }
+
+    if (
+      previousIntersectionOnSegment != null
+      && isWithin(lon2, lat2)
+    ) {
+      // Last point is within the polygon, add the remaining missing distance.
+      distance += CheapRulerSingleton.distance(
+        previousIntersectionOnSegment.x, previousIntersectionOnSegment.y,
+        lon2, lat2
+      );
+    }
+    return distance;
+  }
+
 /* Copyright 2001 softSurfer, 2012 Dan Sunday, 2018 Norbert Truchsess
    This code may be freely used and modified for any purpose providing that
    this copyright notice is included with it. SoftSurfer makes no warranty for

brouter-core/src/main/java/btools/router/OsmPath.java

@@ -143,7 +143,7 @@ abstract class OsmPath implements OsmLinkHolder
 
     boolean recordTransferNodes = detailMode || rc.countTraffic;
 
-    rc.nogomatch = null;
+    rc.nogoCost = 0.;
 
     // extract the 3 positions of the first section
     int lon0 = origin.originLon;
@@ -425,13 +425,13 @@ abstract class OsmPath implements OsmLinkHolder
             originElement.message = message;
           }
         }
-        if ( rc.nogomatch != null )
+        if ( rc.nogoCost < 0)
         {
-          if ( Double.isNaN(rc.nogomatch.nogoWeight) ) {
+          cost = -1;
-            cost = -1;
+        }
-          } else {
+        else
-            cost += rc.nogomatch.nogoWeight;
+        {
-          }
+          cost += rc.nogoCost;
         }
         return;
       }
@@ -464,14 +464,14 @@ abstract class OsmPath implements OsmLinkHolder
     }
 
     // check for nogo-matches (after the *actual* start of segment)
-    if ( rc.nogomatch != null )
+    if ( rc.nogoCost < 0)
     {
-      if ( Double.isNaN(rc.nogomatch.nogoWeight) ) {
+      cost = -1;
-        cost = -1;
+      return;
-        return;
+    }
-      } else {
+    else
-        cost += rc.nogomatch.nogoWeight;
+    {
-      }
+      cost += rc.nogoCost;
     }
 
     // add target-node costs

brouter-core/src/main/java/btools/router/RoutingContext.java

@@ -194,7 +194,7 @@ public final class RoutingContext
   public boolean startDirectionValid;
 
   private double cosangle;
-  public OsmNodeNamed nogomatch = null;
+  public double nogoCost = 0.;
   public boolean isEndpoint = false;
 
   public boolean shortestmatch = false;
@@ -344,10 +344,19 @@ public final class RoutingContext
           }
           if ( nogo.isNogo )
           {
-            if (!(nogo instanceof OsmNogoPolygon)
+            if (Double.isNaN(nogo.nogoWeight))
-                || ((OsmNogoPolygon)nogo).intersects(lon1, lat1, lon2, lat2))
             {
-              nogomatch = nogo;
+                // Nogo is default nogo (ignore completely)
+                nogoCost = -1;
+            }
+            else if (!(nogo instanceof OsmNogoPolygon)) {
+              // nogo is a circle, compute distance within the circle
+              nogoCost = nogo.distanceWithinRadius(lon1, lat1, lon2, lat2, d) * nogo.nogoWeight;
+            }
+            else if (((OsmNogoPolygon)nogo).intersects(lon1, lat1, lon2, lat2))
+            {
+              // nogo is a polygon, compute distance within the polygon
+              nogoCost = ((OsmNogoPolygon)nogo).distanceWithinPolygon(lon1, lat1, lon2, lat2) * nogo.nogoWeight;
             }
           }
           else
@@ -388,7 +397,7 @@ public final class RoutingContext
             }
             else
             {
-              nogomatch = null;
+              nogoCost = 0.;
               lon1 = ilonshortest;
               lat1 = ilatshortest;
             }

brouter-core/src/test/java/btools/router/OsmNodeNamedTest.java

@@ -0,0 +1,117 @@
+package btools.router;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import org.junit.AfterClass;
+import org.junit.BeforeClass;
+import org.junit.Test;
+
+import btools.util.CheapRulerSingleton;
+
+public class OsmNodeNamedTest {
+  static int toOsmLon(double lon) {
+    return (int)( ( lon + 180. ) / CheapRulerSingleton.ILATLNG_TO_LATLNG + 0.5);
+  }
+
+  static int toOsmLat(double lat) {
+    return (int)( ( lat +  90. ) / CheapRulerSingleton.ILATLNG_TO_LATLNG + 0.5);
+  }
+
+  @Test
+  public void testDistanceWithinRadius() {
+    // Segment ends
+    int lon1, lat1, lon2, lat2;
+    // Circle definition
+    OsmNodeNamed node = new OsmNodeNamed();
+    // Center
+    node.ilon = toOsmLon(2.334243);
+    node.ilat = toOsmLat(48.824017);
+    // Radius
+    node.radius = 30 / 110984.;
+
+    // Check distance within radius is correctly computed if the segment passes through the center
+    lon1 = toOsmLon(2.332559);
+    lat1 = toOsmLat(48.823822);
+    lon2 = toOsmLon(2.335018);
+    lat2 = toOsmLat(48.824105);
+    double totalSegmentLength = CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+    assertEquals(
+      "Works for segment aligned with the nogo center",
+      2 * node.radius * 110984.,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.01 * (2 * node.radius * 110984.)
+    );
+
+    // Check distance within radius is correctly computed for a given circle
+    node.ilon = toOsmLon(2.33438);
+    node.ilat = toOsmLat(48.824275);
+    assertEquals(
+      "Works for a segment with no particular properties",
+      27.5,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.1 * 27.5
+    );
+
+    // Check distance within radius is the same if we reverse start and end point
+    assertEquals(
+      "Works if we switch firs and last point",
+      node.distanceWithinRadius(lon2, lat2, lon1, lat1, totalSegmentLength),
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.01
+    );
+
+    // Check distance within radius is correctly computed if a point is inside the circle
+    lon2 = toOsmLon(2.334495);
+    lat2 = toOsmLat(48.824045);
+    totalSegmentLength = CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+    assertEquals(
+      "Works if last point is within the circle",
+      17,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.1 * 17
+    );
+
+    lon1 = toOsmLon(2.334495);
+    lat1 = toOsmLat(48.824045);
+    lon2 = toOsmLon(2.335018);
+    lat2 = toOsmLat(48.824105);
+    totalSegmentLength = CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+    assertEquals(
+      "Works if first point is within the circle",
+      9,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.1 * 9
+    );
+
+    lon1 = toOsmLon(2.33427);
+    lat1 = toOsmLat(48.82402);
+    lon2 = toOsmLon(2.334587);
+    lat2 = toOsmLat(48.824061);
+    totalSegmentLength = CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+    assertEquals(
+      "Works if both points are within the circle",
+      25,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.1 * 25
+    );
+
+    // Check distance within radius is correctly computed if both points are on
+    // the same side of the center.
+    // Note: the only such case possible is with one point outside and one
+    // point within the circle, as we expect the segment to have a non-empty
+    // intersection with the circle.
+    lon1 = toOsmLon(2.332559);
+    lat1 = toOsmLat(48.823822);
+    lon2 = toOsmLon(2.33431);
+    lat2 = toOsmLat(48.824027);
+    totalSegmentLength = CheapRulerSingleton.distance(lon1, lat1, lon2, lat2);
+    assertEquals(
+      "Works if both points are on the same side of the circle center",
+      5,
+      node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength),
+      0.1 * 5
+    );
+  }
+}

brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java

@@ -30,8 +30,8 @@ import btools.util.CheapRulerSingleton;
 
 public class OsmNogoPolygonTest {
 
-  static final int offset_x = 11000000;
+  static final int OFFSET_X = 11000000;
-  static final int offset_y = 50000000;
+  static final int OFFSET_Y = 50000000;
 
   static OsmNogoPolygon polygon;
   static OsmNogoPolygon polyline;
@@ -39,11 +39,11 @@ public class OsmNogoPolygonTest {
   static final double[] lons = {  1.0,  1.0,  0.5, 0.5, 1.0, 1.0, -1.1, -1.0 };
   static final double[] lats = { -1.0, -0.1, -0.1, 0.1, 0.1, 1.0,  1.1, -1.0 };
 
-  static int toOsmLon(double lon) {
+  static int toOsmLon(double lon, int offset_x) {
     return (int)( ( lon + 180. ) *1000000. + 0.5)+offset_x; // see ServerHandler.readPosition()
   }
 
-  static int toOsmLat(double lat) {
+  static int toOsmLat(double lat, int offset_y) {
     return (int)( ( lat +  90. ) *1000000. + 0.5)+offset_y;
   }
 
@@ -51,11 +51,11 @@ public class OsmNogoPolygonTest {
   public static void setUp() throws Exception {
     polygon = new OsmNogoPolygon(true);
     for (int i = 0; i<lons.length; i++) {
-      polygon.addVertex(toOsmLon(lons[i]),toOsmLat(lats[i]));
+      polygon.addVertex(toOsmLon(lons[i], OFFSET_X),toOsmLat(lats[i], OFFSET_Y));
     }
     polyline = new OsmNogoPolygon(false);
     for (int i = 0; i<lons.length; i++) {
-      polyline.addVertex(toOsmLon(lons[i]),toOsmLat(lats[i]));
+      polyline.addVertex(toOsmLon(lons[i], OFFSET_X),toOsmLat(lats[i], OFFSET_Y));
     }
   }
 
@@ -72,8 +72,8 @@ public class OsmNogoPolygonTest {
     polygon.calcBoundingCircle();
     double r = polygon.radius;
     for (int i=0; i<lons.length; i++) {
-      double dpx = (toOsmLon(lons[i]) - polygon.ilon) * dlon2m;
+      double dpx = (toOsmLon(lons[i], OFFSET_X) - polygon.ilon) * dlon2m;
-      double dpy = (toOsmLon(lats[i]) - polygon.ilon) * dlat2m;
+      double dpy = (toOsmLat(lats[i], OFFSET_Y) - polygon.ilat) * dlat2m;
       double r1 = Math.sqrt(dpx * dpx + dpy * dpy);
       double diff = r-r1;
       assertTrue("i: "+i+" r("+r+") >= r1("+r1+")", diff >= 0);
@@ -81,8 +81,8 @@ public class OsmNogoPolygonTest {
     polyline.calcBoundingCircle();
     r = polyline.radius;
     for (int i=0; i<lons.length; i++) {
-      double dpx = (toOsmLon(lons[i]) - polyline.ilon) * dlon2m;
+      double dpx = (toOsmLon(lons[i], OFFSET_X) - polyline.ilon) * dlon2m;
-      double dpy = (toOsmLon(lats[i]) - polyline.ilon) * dlat2m;
+      double dpy = (toOsmLat(lats[i], OFFSET_Y) - polyline.ilat) * dlat2m;
       double r1 = Math.sqrt(dpx * dpx + dpy * dpy);
       double diff = r-r1;
       assertTrue("i: "+i+" r("+r+") >= r1("+r1+")", diff >= 0);
@@ -96,7 +96,7 @@ public class OsmNogoPolygonTest {
     boolean[] within = { true, false, false, false, false, true, true, true, true, true, };
 
     for (int i=0; i<plons.length; i++) {
-      assertEquals("("+plons[i]+","+plats[i]+")",within[i],polygon.isWithin(toOsmLon(plons[i]), toOsmLat(plats[i])));
+      assertEquals("("+plons[i]+","+plats[i]+")",within[i],polygon.isWithin(toOsmLon(plons[i], OFFSET_X), toOsmLat(plats[i], OFFSET_Y)));
     }
   }
 
@@ -109,7 +109,7 @@ public class OsmNogoPolygonTest {
     boolean[] within = { false, false, false, true, true, true, false, true, true, true };
 
     for (int i=0; i<p0lons.length; i++) {
-      assertEquals("("+p0lons[i]+","+p0lats[i]+")-("+p1lons[i]+","+p1lats[i]+")",within[i],polygon.intersects(toOsmLon(p0lons[i]), toOsmLat(p0lats[i]), toOsmLon(p1lons[i]), toOsmLat(p1lats[i])));
+      assertEquals("("+p0lons[i]+","+p0lats[i]+")-("+p1lons[i]+","+p1lats[i]+")",within[i],polygon.intersects(toOsmLon(p0lons[i], OFFSET_X), toOsmLat(p0lats[i], OFFSET_Y), toOsmLon(p1lons[i], OFFSET_X), toOsmLat(p1lats[i], OFFSET_Y)));
     }
   }
 
@@ -122,7 +122,7 @@ public class OsmNogoPolygonTest {
     boolean[] within = { false, false, false, true, true, true, false, true, true, false };
 
     for (int i=0; i<p0lons.length; i++) {
-      assertEquals("("+p0lons[i]+","+p0lats[i]+")-("+p1lons[i]+","+p1lats[i]+")",within[i],polyline.intersects(toOsmLon(p0lons[i]), toOsmLat(p0lats[i]), toOsmLon(p1lons[i]), toOsmLat(p1lats[i])));
+      assertEquals("("+p0lons[i]+","+p0lats[i]+")-("+p1lons[i]+","+p1lats[i]+")",within[i],polyline.intersects(toOsmLon(p0lons[i], OFFSET_X), toOsmLat(p0lats[i], OFFSET_Y), toOsmLon(p1lons[i], OFFSET_X), toOsmLat(p1lats[i], OFFSET_Y)));
     }
   }
 
@@ -144,4 +144,57 @@ public class OsmNogoPolygonTest {
     assertFalse(OsmNogoPolygon.isOnLine(0,-10, 10,10, 20,30));
     assertFalse(OsmNogoPolygon.isOnLine(30,50, 10,10, 20,30));
   }
+
+  @Test
+  public void testDistanceWithinPolygon() {
+      // Testing polygon
+      final double[] lons = { 2.333523, 2.333432, 2.333833, 2.333983, 2.334815, 2.334766, 2.333523 };
+      final double[] lats = { 48.823778, 48.824091, 48.82389, 48.824165, 48.824232, 48.82384, 48.823778 };
+      OsmNogoPolygon polygon = new OsmNogoPolygon(true);
+      for (int i = 0; i < lons.length; i++) {
+          polygon.addVertex(toOsmLon(lons[i], 0), toOsmLat(lats[i], 0));
+      }
+
+      // Check with a segment with a single intersection with the polygon
+      int lon1 = toOsmLon(2.33308732509613, 0);
+      int lat1 = toOsmLat(48.8238790443901, 0);
+      int lon2 = toOsmLon(2.33378201723099, 0);
+      int lat2 = toOsmLat(48.8239585098974, 0);
+      assertEquals(
+        "Should give the correct length for a segment with a single intersection",
+        17.5,
+        polygon.distanceWithinPolygon(lon1, lat1, lon2, lat2),
+        0.05 * 17.5
+      );
+
+      // Check with a segment crossing multiple times the polygon
+      lon2 = toOsmLon(2.33488172292709, 0);
+      lat2 = toOsmLat(48.8240891862353, 0);
+      assertEquals(
+        "Should give the correct length for a segment with multiple intersections",
+        85,
+        polygon.distanceWithinPolygon(lon1, lat1, lon2, lat2),
+        0.05 * 85
+      );
+
+      // Check that it works when a point is within the polygon
+      lon2 = toOsmLon(2.33433187007904, 0);
+      lat2 = toOsmLat(48.8240238480664, 0);
+      assertEquals(
+        "Should give the correct length when last point is within the polygon",
+        50,
+        polygon.distanceWithinPolygon(lon1, lat1, lon2, lat2),
+        0.05 * 50
+      );
+      lon1 = toOsmLon(2.33433187007904, 0);
+      lat1 = toOsmLat(48.8240238480664, 0);
+      lon2 = toOsmLon(2.33488172292709, 0);
+      lat2 = toOsmLat(48.8240891862353, 0);
+      assertEquals(
+        "Should give the correct length when first point is within the polygon",
+        35,
+        polygon.distanceWithinPolygon(lon1, lat1, lon2, lat2),
+        0.05 * 35
+      );
+  }
 }

brouter-mem-router/src/main/java/btools/memrouter/ScheduledRouter.java

@@ -305,9 +305,9 @@ System.out.println( "*** finishedOffsets = " + finishedOffsets );
     }
 
     // calc distance and check nogos
-    rc.nogomatch = null;
+    rc.nogoCost = 0.;
     int distance = rc.calcDistance( currentNode.ilon, currentNode.ilat, node.ilon, node.ilat );
-    if ( rc.nogomatch != null )
+    if ( Math.abs(rc.nogoCost) > 0.)
     {
       return;
     }