From d6219648631aa99b3bb895209f8f69582e6d97df Mon Sep 17 00:00:00 2001 From: "Phyks (Lucas Verney)" Date: Tue, 4 Dec 2018 15:54:50 +0100 Subject: [PATCH] Radius are now stored in meters, to easily account for different radiuses on X and Y axes. --- .../java/btools/router/MatchedWaypoint.java | 4 +- .../main/java/btools/router/OsmNodeNamed.java | 6 +-- .../java/btools/router/OsmNogoPolygon.java | 38 ++++++++----------- .../src/main/java/btools/router/OsmPath.java | 3 +- .../java/btools/router/RoutingContext.java | 33 +++++++--------- .../java/btools/router/SearchBoundary.java | 19 ++++++---- .../btools/router/WaypointMatcherImpl.java | 4 +- .../java/btools/routingapp/BRouterView.java | 7 +++- .../java/btools/util/CheapRulerSingleton.java | 13 ++++--- 9 files changed, 60 insertions(+), 67 deletions(-) diff --git a/brouter-core/src/main/java/btools/router/MatchedWaypoint.java b/brouter-core/src/main/java/btools/router/MatchedWaypoint.java index 055ccd9..14441f5 100644 --- a/brouter-core/src/main/java/btools/router/MatchedWaypoint.java +++ b/brouter-core/src/main/java/btools/router/MatchedWaypoint.java @@ -17,7 +17,7 @@ final class MatchedWaypoint public OsmNode node2; public OsmNodeNamed crosspoint; public OsmNodeNamed waypoint; - public double radius; + public double radius; // radius in meters public boolean hasUpdate; public void writeToStream( DataOutput dos ) throws IOException @@ -40,7 +40,7 @@ final class MatchedWaypoint mwp.node2 = new OsmNode(); mwp.crosspoint = new OsmNodeNamed(); mwp.waypoint = new OsmNodeNamed(); - + mwp.node1.ilat = dis.readInt(); mwp.node1.ilon = dis.readInt(); mwp.node2.ilat = dis.readInt(); diff --git a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java b/brouter-core/src/main/java/btools/router/OsmNodeNamed.java index 1566174..5433446 100644 --- a/brouter-core/src/main/java/btools/router/OsmNodeNamed.java +++ b/brouter-core/src/main/java/btools/router/OsmNodeNamed.java @@ -10,15 +10,15 @@ import btools.mapaccess.OsmNode; public class OsmNodeNamed extends OsmNode { public String name; - public double radius; // radius of nogopoint + public double radius; // radius of nogopoint (in meters) public boolean isNogo = false; - + @Override public String toString() { return ilon + "," + ilat + "," + name; } - + 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 2b6c72a..57cdaf3 100644 --- a/brouter-core/src/main/java/btools/router/OsmNogoPolygon.java +++ b/brouter-core/src/main/java/btools/router/OsmNogoPolygon.java @@ -103,7 +103,7 @@ public class OsmNogoPolygon extends OsmNodeNamed int cx = (cxmax+cxmin) / 2; // center of circle int cy = (cymax+cymin) / 2; - + double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( cy ); double dlon2m = lonlat2m[0]; double dlat2m = lonlat2m[1]; @@ -111,28 +111,24 @@ public class OsmNogoPolygon extends OsmNodeNamed double rad = 0; // radius double rad2 = 0; // radius squared; - double dpx = 0; // x-xomponent of vector from center to point - double dpy = 0; // y-component - double dmax2 = 0; // squared lenght of vector from center to point + double dmax = 0; // length of vector from center to point int i_max = -1; do - { // now identify the point outside of the circle that has the greatest distance - for (int i = 0; i < points.size();i++) + { + // now identify the point outside of the circle that has the greatest distance + for (int i = 0; i < points.size(); i++) { final Point p = points.get(i); - final double dpix = (p.x - cx) * dlon2m; - final double dpiy = (p.y - cy) * dlat2m; - final double dist2 = dpix * dpix + dpiy * dpiy; - if (dist2 <= rad2) + final double dist = CheapRulerSingleton.distance(p.x, p.y, (int) cx, (int) cy); + if (dist <= rad) { continue; } - if (dist2 > dmax2) + if (dist > dmax) { - dmax2 = dist2; // new maximum distance found - dpx = dpix / dlon2m; - dpy = dpiy / dlat2m; + // new maximum distance found + dmax = dist; i_max = i; } } @@ -140,17 +136,13 @@ public class OsmNogoPolygon extends OsmNodeNamed { break; // leave loop when no point outside the circle is found any more. } - final double dist = Math.sqrt(dmax2); - final double dd = 0.5 * (dist - rad) / dist; - - cx += (int)(dd * dpx + 0.5); // shift center toward point - cy += (int)(dd * dpy + 0.5); + final double dd = 0.5 * (1 - rad / dmax); final Point p = points.get(i_max); // calculate new radius to just include this point - final double dpix = (p.x - cx) * dlon2m; - final double dpiy = (p.y - cy) * dlat2m; - dmax2 = rad2 = dpix * dpix + dpiy * dpiy; - rad = Math.sqrt(rad2); + cx += (int)(dd * (p.x - cx) + 0.5); // shift center toward point + cy += (int)(dd * (p.y - cy) + 0.5); + + dmax = rad = CheapRulerSingleton.distance(p.x, p.y, (int) cx, (int) cy); i_max = -1; } while (true); diff --git a/brouter-core/src/main/java/btools/router/OsmPath.java b/brouter-core/src/main/java/btools/router/OsmPath.java index 4133cf4..c4b954e 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/brouter-core/src/main/java/btools/router/OsmPath.java @@ -346,7 +346,8 @@ abstract class OsmPath implements OsmLinkHolder if ( rc.startDirectionValid ) { double dir = rc.startDirection.intValue() / CheapRulerSingleton.DEG_TO_RAD; - lon0 = lon1 - (int) ( 1000. * Math.sin( dir ) / rc.getCosLat() ); + double coslat = Math.cos(lat1); + lon0 = lon1 - (int) ( 1000. * Math.sin( dir ) / coslat ); lat0 = lat1 - (int) ( 1000. * Math.cos( dir ) ); } else diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 9adbffa..480c900 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/brouter-core/src/main/java/btools/router/RoutingContext.java @@ -193,7 +193,6 @@ public final class RoutingContext public Integer startDirection; public boolean startDirectionValid; - private double coslat; private double cosangle; public boolean nogomatch = false; public boolean isEndpoint = false; @@ -248,8 +247,8 @@ public final class RoutingContext try { ir = Integer.parseInt( s.substring( 4 ) ); } catch( Exception e ) { /* ignore */ } } - // TODO[Phyks] - nogo.radius = ir / 110984.; // 6378000. / 57.3; + // Radius of the nogo point in meters + nogo.radius = ir; } } @@ -259,12 +258,10 @@ public final class RoutingContext List nogos = new ArrayList(); for( OsmNodeNamed nogo : nogopoints ) { - // TODO[Phyks] - int radiusInMeter = (int)(nogo.radius * 111894.); boolean goodGuy = true; for( OsmNodeNamed wp : waypoints ) { - if ( wp.calcDistance( nogo ) < radiusInMeter + if ( wp.calcDistance( nogo ) < nogo.radius && (!(nogo instanceof OsmNogoPolygon) || (((OsmNogoPolygon)nogo).isClosed ? ((OsmNogoPolygon)nogo).isWithin(wp.ilon, wp.ilat) @@ -288,6 +285,7 @@ public final class RoutingContext OsmNodeNamed nogo = nogopoints.get(i); cs[0] += nogo.ilon; cs[1] += nogo.ilat; + // 10 is an arbitrary constant to get sub-integer precision in the checksum cs[2] += (long) ( nogo.radius*10.); } return cs; @@ -342,7 +340,7 @@ public final class RoutingContext if ( s2 > 0. ) { radius = Math.sqrt( s1 < s2 ? r12 : r22 ); - if ( radius > nogo.radius ) continue; // 20m ^ 2 + if ( radius > nogo.radius ) continue; } if ( nogo.isNogo ) { @@ -404,28 +402,23 @@ public final class RoutingContext return (int)(d + 1.0 ); } - // assumes that calcDistance/calcCosAngle called in sequence, so coslat valid public double getCosAngle() { return cosangle; } - public double getCosLat() - { - return coslat; - } - public double calcAngle( int lon0, int lat0, int lon1, int lat1, int lon2, int lat2 ) { - double dlat1 = (lat1 - lat0); - double dlon1 = (lon1 - lon0) * coslat; - double dlat2 = (lat2 - lat1); - double dlon2 = (lon2 - lon1) * coslat; + double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( lat1 ); + double dy10 = (lat1 - lat0) * lonlat2m[1]; + double dx10 = (lon1 - lon0) * lonlat2m[0]; + double dy21 = (lat2 - lat1) * lonlat2m[1]; + double dx21 = (lon2 - lon1) * lonlat2m[0]; - double dd = Math.sqrt( (dlat1*dlat1 + dlon1*dlon1)*(dlat2*dlat2 + dlon2*dlon2) ); + double dd = Math.sqrt( (dx10*dx10 + dy10*dy10)*(dx21*dx21 + dy21*dy21) ); if ( dd == 0. ) { cosangle = 1.; return 0.; } - double sinp = (dlat1*dlon2 - dlon1*dlat2)/dd; - double cosp = (dlat1*dlat2 + dlon1*dlon2)/dd; + double sinp = (dy10*dy21 - dx10*dx21)/dd; + double cosp = (dy10*dy21 + dx10*dx21)/dd; cosangle = cosp; double p; diff --git a/brouter-core/src/main/java/btools/router/SearchBoundary.java b/brouter-core/src/main/java/btools/router/SearchBoundary.java index f40aec1..1d976f9 100644 --- a/brouter-core/src/main/java/btools/router/SearchBoundary.java +++ b/brouter-core/src/main/java/btools/router/SearchBoundary.java @@ -22,19 +22,22 @@ public final class SearchBoundary private int maxlat; private int radius; private OsmNode p; - + int direction; - + + /** + * @param radius Search radius in meters. + */ public SearchBoundary( OsmNode n, int radius, int direction ) { this.radius = radius; this.direction = direction; p = new OsmNode( n.ilon, n.ilat ); - + int lon = (n.ilon / 5000000 ) * 5000000; int lat = (n.ilat / 5000000 ) * 5000000; - + minlon0 = lon - 5000000; minlat0 = lat - 5000000; maxlon0 = lon + 10000000; @@ -45,12 +48,12 @@ public final class SearchBoundary maxlon = lon + 6000000; maxlat = lat + 6000000; } - + public static String getFileName( OsmNode n ) { int lon = (n.ilon / 5000000 ) * 5000000; int lat = (n.ilat / 5000000 ) * 5000000; - + int dlon = lon / 1000000 -180; int dlat = lat / 1000000 - 90; @@ -58,7 +61,7 @@ public final class SearchBoundary String slat = dlat < 0 ? "S" + (-dlat) : "N" + dlat; return slon + "_" + slat + ".trf"; } - + public boolean isInBoundary( OsmNode n, int cost ) { if ( radius > 0 ) @@ -83,5 +86,5 @@ public final class SearchBoundary default: throw new IllegalArgumentException( "undefined direction: "+ direction ); } } - + } diff --git a/brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java b/brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java index 82000f9..ee76707 100644 --- a/brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java +++ b/brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java @@ -33,8 +33,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher this.islandPairs = islandPairs; for ( MatchedWaypoint mwp : waypoints ) { - // TODO[Phyks] - mwp.radius = maxDistance * 110984.; // 6378000. / 57.3; + mwp.radius = maxDistance; } } @@ -49,6 +48,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher double dx = ( lon2 - lon1 ) * dlon2m; double dy = ( lat2 - lat1 ) * dlat2m; double d = Math.sqrt( dy * dy + dx * dx ); + if ( d == 0. ) return; diff --git a/brouter-routing-app/src/main/java/btools/routingapp/BRouterView.java b/brouter-routing-app/src/main/java/btools/routingapp/BRouterView.java index 33d2672..f9a1fb0 100644 --- a/brouter-routing-app/src/main/java/btools/routingapp/BRouterView.java +++ b/brouter-routing-app/src/main/java/btools/routingapp/BRouterView.java @@ -635,7 +635,9 @@ public class BRouterView extends View int lat = n.ilat - centerLat; int x = imgw / 2 + (int) ( scaleLon * lon ); int y = imgh / 2 - (int) ( scaleLat * lat ); - int ir = (int) ( n.radius * 1000000. * scaleLat ); + + double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( centerLat ); + int ir = (int) ( n.radius * scaleLat / lonlat2m[1]); if ( ir > minradius ) { Paint paint = new Paint(); @@ -660,7 +662,8 @@ public class BRouterView extends View private void paintPolygon( Canvas canvas, OsmNogoPolygon p, int minradius ) { - final int ir = (int) ( p.radius * 1000000. * scaleLat ); + double[] lonlat2m = CheapRulerSingleton.getLonLatToMeterScales( centerLat ); + final int ir = (int) ( p.radius * scaleLat / lonlat2m[1] ); if ( ir > minradius ) { Paint paint = new Paint(); diff --git a/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java b/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java index 3935282..7a825aa 100644 --- a/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java +++ b/brouter-util/src/main/java/btools/util/CheapRulerSingleton.java @@ -17,10 +17,10 @@ public final class CheapRulerSingleton { public final static int KILOMETERS_TO_METERS = 1000; public final static double DEG_TO_RAD = Math.PI / 180.; - // Cosine cache constants + // Scale cache constants private final static int SCALE_CACHE_LENGTH = 1800; private final static int SCALE_CACHE_INCREMENT = 100000; - // COS_CACHE_LENGTH cached values between 0 and COS_CACHE_MAX_DEGREES degrees. + // SCALE_CACHE_LENGTH cached values between 0 and COS_CACHE_MAX_DEGREES degrees. private final static double[][] SCALE_CACHE = new double[SCALE_CACHE_LENGTH][]; /** @@ -33,7 +33,7 @@ public final class CheapRulerSingleton { } private static double[] calcKxKyFromILat(int ilat) { - double lat = DEG_TO_RAD*(ilat-90000000)/1000000.; + 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; @@ -50,8 +50,8 @@ public final class CheapRulerSingleton { /** * Calculate the degree->meter scale for given latitude - * - * @result [lon->meter,lat->meter] + * + * @result [lon->meter,lat->meter] */ public static double[] getLonLatToMeterScales( int ilat ) { return SCALE_CACHE[ ilat / SCALE_CACHE_INCREMENT ]; @@ -65,12 +65,13 @@ public final class CheapRulerSingleton { * @param ilat1 Integer latitude for the start point, this is (latitude + 90) * 1e6. * @param ilon2 Integer longitude for the end point, this is (longitude + 180) * 1e6. * @param ilat2 Integer latitude for the end point, this is (latitude + 90) * 1e6. + * @return The distance between the two points, in meters. * * @note Integer longitude is ((longitude in degrees) + 180) * 1e6. * Integer latitude is ((latitude in degrees) + 90) * 1e6. */ public static double distance(int ilon1, int ilat1, int ilon2, int ilat2) { - double[] kxky = getLonLatToMeterScales( ( ilat1 + ilat2 ) >> 1 ); + double[] kxky = getLonLatToMeterScales( ( ilat1 + ilat2 ) >> 1 ); double dlon = (ilon1 - ilon2) * kxky[0]; double dlat = (ilat1 - ilat2) * kxky[1]; return Math.sqrt(dlat * dlat + dlon * dlon); // in m