diff --git a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java b/brouter-core/src/main/java/btools/router/OsmNodeNamed.java index 5433446..0f75fad 100644 --- a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java +++ b/brouter-core/src/main/java/btools/router/OsmNodeNamed.java @@ -6,17 +6,71 @@ package btools.router; import btools.mapaccess.OsmNode; +import btools.util.CheapRulerSingleton; public class OsmNodeNamed extends OsmNode { public String name; public double radius; // radius of nogopoint (in meters) + public double nogoWeight; // weight for nogopoint public boolean isNogo = false; @Override public String toString() { - return ilon + "," + ilat + "," + name; + if ( Double.isNaN(nogoWeight ) ) { + return ilon + "," + ilat + "," + name; + } else { + return ilon + "," + ilat + "," + name + "," + nogoWeight; + } + } + + public double distanceWithinRadius(int lon1, int lat1, int lon2, int lat2, double totalSegmentLength) { + double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( (lat1 + lat2) >> 1 ); + + boolean isFirstPointWithinCircle = CheapRulerSingleton.distance(lon1, lat1, ilon, ilat) < radius; + boolean isLastPointWithinCircle = CheapRulerSingleton.distance(lon2, lat2, ilon, ilat) < radius; + // 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( + radius*radius - ( + 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 ) @@ -26,7 +80,14 @@ public class OsmNodeNamed extends OsmNode n.ilon = Integer.parseInt( s.substring( 0, idx1 ) ); int idx2 = s.indexOf( ',', idx1+1 ); n.ilat = Integer.parseInt( s.substring( idx1+1, idx2 ) ); - n.name = s.substring( idx2+1 ); + int idx3 = s.indexOf( ',', idx2+1 ); + if ( idx3 == -1) { + n.name = s.substring( idx2 + 1 ); + n.nogoWeight = Double.NaN; + } else { + n.name = s.substring( idx2+1, idx3 ); + n.nogoWeight = Double.parseDouble( s.substring( idx3 + 1 ) ); + } n.isNogo = true; return n; } 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 c8f3f00..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 = false; + rc.nogoCost = 0.; // extract the 3 positions of the first section int lon0 = origin.originLon; @@ -425,10 +425,14 @@ abstract class OsmPath implements OsmLinkHolder originElement.message = message; } } - if ( rc.nogomatch ) + if ( rc.nogoCost < 0) { cost = -1; } + else + { + cost += rc.nogoCost; + } return; } @@ -460,11 +464,15 @@ abstract class OsmPath implements OsmLinkHolder } // check for nogo-matches (after the *actual* start of segment) - if ( rc.nogomatch ) + if ( rc.nogoCost < 0) { cost = -1; return; } + else + { + cost += rc.nogoCost; + } // add target-node costs double targetCost = processTargetNode( rc ); diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 480c900..3db5d1d 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 boolean nogomatch = false; + 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 = true; + // 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 = false; + nogoCost = 0.; lon1 = ilonshortest; lat1 = ilatshortest; } @@ -417,7 +426,7 @@ public final class RoutingContext double dd = Math.sqrt( (dx10*dx10 + dy10*dy10)*(dx21*dx21 + dy21*dy21) ); if ( dd == 0. ) { cosangle = 1.; return 0.; } - double sinp = (dy10*dy21 - dx10*dx21)/dd; + double sinp = (dx10*dy21 - dy10*dx21)/dd; double cosp = (dy10*dy21 + dx10*dx21)/dd; cosangle = cosp; diff --git a/brouter-core/src/main/java/btools/router/StdPath.java b/brouter-core/src/main/java/btools/router/StdPath.java index c299f04..9501681 100644 --- a/brouter-core/src/main/java/btools/router/StdPath.java +++ b/brouter-core/src/main/java/btools/router/StdPath.java @@ -180,7 +180,6 @@ final class StdPath extends OsmPath return 0.; } - @Override public int elevationCorrection( RoutingContext rc ) { 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..cc31b52 --- /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; + + // 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, + node.distanceWithinRadius(lon1, lat1, lon2, lat2, totalSegmentLength), + 0.01 * (2 * node.radius) + ); + + // 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 trips = new ArrayList(); private static long oldChecksum = 0; - + private String startEndText() { return (start == null ? "unmatched" : start.getName() ) + "->" + (end == null ? "unmatched" : end.getName() ); @@ -69,11 +69,11 @@ final class ScheduledRouter // check for identical params long[] nogocheck = rc.getNogoChecksums(); - + long checksum = nogocheck[0] + nogocheck[1] + nogocheck[2]; checksum += startPos.getILat() + startPos.getILon() + endPos.getILat() + endPos.getILon(); checksum += rc.localFunction.hashCode(); - + if ( checksum != oldChecksum ) { trips = _findRoute( startPos, endPos, false ); @@ -86,7 +86,7 @@ final class ScheduledRouter if ( linksProcessed + linksReProcessed > 5000000 ) throw new RuntimeException( "5 million links limit reached" ); else throw new RuntimeException( "no track found! (" + startEndText() + ")" ); } - + if ( alternativeIdx == 0 ) // = result overview { List details = new ArrayList(); @@ -101,15 +101,15 @@ final class ScheduledRouter t.iternity = details; return t; } - + int idx = alternativeIdx > trips.size() ? trips.size()-1 : alternativeIdx-1; Iternity iternity = trips.get( idx ); OsmTrack t = iternity.track; t.iternity = iternity.details; return t; - } - - + } + + private List _findRoute( OsmPos startPos, OsmPos endPos, boolean fastStop ) throws Exception { List iternities = new ArrayList(); @@ -120,7 +120,7 @@ final class ScheduledRouter end = graph.matchNodeForPosition( endPos, rc.expctxWay, rc.transitonly ); if ( end == null ) throw new IllegalArgumentException( "unmatched end: " + endPos ); - + time0 = System.currentTimeMillis() + (long) ( rc.starttimeoffset * 60000L ); long minutes0 = ( time0 + 59999L ) / 60000L; time0 = minutes0 * 60000L; @@ -170,8 +170,8 @@ final class ScheduledRouter OffsetSet offsets = trip.offsets.ensureMaxOffset( maxOffset ); if ( offsets == null ) continue; - */ - + */ + OffsetSet offsets = trip.offsets; // check global closure @@ -240,7 +240,7 @@ System.out.println( "*** added track to result list !**** "); } } System.out.println( "*** finishedOffsets = " + finishedOffsets ); - + } for ( OsmLinkP link = currentNode.getFirstLink(); link != null; link = link.getNext( currentNode ) ) { @@ -305,9 +305,9 @@ System.out.println( "*** finishedOffsets = " + finishedOffsets ); } // calc distance and check nogos - rc.nogomatch = false; + rc.nogoCost = 0.; int distance = rc.calcDistance( currentNode.ilon, currentNode.ilat, node.ilon, node.ilat ); - if ( rc.nogomatch ) + if ( Math.abs(rc.nogoCost) > 0.) { return; } @@ -483,7 +483,7 @@ System.out.println( "*** finishedOffsets = " + finishedOffsets ); int distance = 0; long departure = 0; - + ScheduledTrip itrip = null; String profile = extractProfile( rc.localFunction ); @@ -496,7 +496,7 @@ System.out.println( "*** finishedOffsets = " + finishedOffsets ); while (current != null) { departure = current.departure; - + // System.out.println( "trip=" + current ); OsmNodeP node = current.getTargetNode(); OsmPathElement pe = OsmPathElement.create( node.ilon, node.ilat, node.selev, null, false ); @@ -569,18 +569,18 @@ System.out.println( "*** finishedOffsets = " + finishedOffsets ); iternity.details.add( "depart: " + df.format( d0 ) + " " + stationName ); iternity.details.add( " --- " + lineName + " ---" ); iternity.details.add( "arrive: " + df.format( d1 ) + " " + nextStationName ); - + if ( !iternity.lines.contains( lineName ) ) { iternity.lines.add( lineName ); - } + } } } iternity.track = track; iternity.arrivaltime = time0 + 60000L * offset + trip.arrival; iternity.departtime = time0 + 60000L * offset + departure; - + return iternity; } diff --git a/brouter-server/src/main/java/btools/server/request/ServerHandler.java b/brouter-server/src/main/java/btools/server/request/ServerHandler.java index 12c3621..23febaf 100644 --- a/brouter-server/src/main/java/btools/server/request/ServerHandler.java +++ b/brouter-server/src/main/java/btools/server/request/ServerHandler.java @@ -222,24 +222,29 @@ public class ServerHandler extends RequestHandler { for (int i = 0; i < lonLatRadList.length; i++) { String[] lonLatRad = lonLatRadList[i].split(","); - nogoList.add(readNogo(lonLatRad[0], lonLatRad[1], lonLatRad[2])); + String nogoWeight = "NaN"; + if (lonLatRad.length > 3) { + nogoWeight = lonLatRad[3]; + } + nogoList.add(readNogo(lonLatRad[0], lonLatRad[1], lonLatRad[2], nogoWeight)); } return nogoList; } - private static OsmNodeNamed readNogo( String lon, String lat, String radius ) + private static OsmNodeNamed readNogo( String lon, String lat, String radius, String nogoWeight ) { - return readNogo(Double.parseDouble( lon ), Double.parseDouble( lat ), Integer.parseInt( radius ) ); + return readNogo(Double.parseDouble( lon ), Double.parseDouble( lat ), Integer.parseInt( radius ), Double.parseDouble( nogoWeight )); } - private static OsmNodeNamed readNogo( double lon, double lat, int radius ) + private static OsmNodeNamed readNogo( double lon, double lat, int radius, double nogoWeight ) { OsmNodeNamed n = new OsmNodeNamed(); n.name = "nogo" + radius; n.ilon = (int)( ( lon + 180. ) *1000000. + 0.5); n.ilat = (int)( ( lat + 90. ) *1000000. + 0.5); n.isNogo = true; + n.nogoWeight = nogoWeight; return n; } diff --git a/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java b/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java index 7872390..8138e00 100644 --- a/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java +++ b/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java @@ -33,7 +33,7 @@ public final class CheapRulerSingleton { } private static double[] calcKxKyFromILat(int ilat) { - double lat = DEG_TO_RAD*ilat*ILATLNG_TO_LATLNG - 90; + double lat = DEG_TO_RAD*(ilat*ILATLNG_TO_LATLNG - 90); double cos = Math.cos(lat); double cos2 = 2 * cos * cos - 1; double cos3 = 2 * cos * cos2 - cos;