From 2591f22348f9cad255b0adf1ab233b9327d51e68 Mon Sep 17 00:00:00 2001 From: "Phyks (Lucas Verney)" Date: Mon, 3 Dec 2018 00:34:21 +0100 Subject: [PATCH] Make nogo weight proportional to nogo distance Compute the nogo cost as proportional to the length of the segment inside the nogo area. --- .../main/java/btools/router/OsmNodeNamed.java | 50 ++++++++ .../java/btools/router/OsmNogoPolygon.java | 84 +++++++++++++ .../src/main/java/btools/router/OsmPath.java | 28 ++--- .../java/btools/router/RoutingContext.java | 19 ++- .../java/btools/router/OsmNodeNamedTest.java | 117 ++++++++++++++++++ .../btools/router/OsmNogoPolygonTest.java | 79 ++++++++++-- .../btools/memrouter/ScheduledRouter.java | 4 +- 7 files changed, 347 insertions(+), 34 deletions(-) create mode 100644 brouter-core/src/test/java/btools/router/OsmNodeNamedTest.java diff --git a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java b/brouter-core/src/main/java/btools/router/OsmNodeNamed.java index 5c4f6f1..64de5d2 100644 --- a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java +++ b/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(); diff --git a/brouter-core/src/main/java/btools/router/OsmNogoPolygon.java b/brouter-core/src/main/java/btools/router/OsmNogoPolygon.java index 02991da..598b673 100644 --- a/brouter-core/src/main/java/btools/router/OsmNogoPolygon.java +++ b/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 diff --git a/brouter-core/src/main/java/btools/router/OsmPath.java b/brouter-core/src/main/java/btools/router/OsmPath.java index 475027d..75dda0a 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/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; - } else { - cost += rc.nogomatch.nogoWeight; - } + cost = -1; + } + else + { + 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; - return; - } else { - cost += rc.nogomatch.nogoWeight; - } + cost = -1; + return; + } + else + { + cost += rc.nogoCost; } // add target-node costs diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 88eefe2..c43e4bd 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/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) - || ((OsmNogoPolygon)nogo).intersects(lon1, lat1, lon2, lat2)) + if (Double.isNaN(nogo.nogoWeight)) { - 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; } diff --git a/brouter-core/src/test/java/btools/router/OsmNodeNamedTest.java b/brouter-core/src/test/java/btools/router/OsmNodeNamedTest.java new file mode 100644 index 0000000..8a899cf --- /dev/null +++ b/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 + ); + } +} diff --git a/brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java b/brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java index 7eae51f..08b4338 100644 --- a/brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java +++ b/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_y = 50000000; + static final int OFFSET_X = 11000000; + 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= r1("+r1+")", diff >= 0); @@ -81,8 +81,8 @@ public class OsmNogoPolygonTest { polyline.calcBoundingCircle(); r = polyline.radius; for (int i=0; i= 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