Fabio/brouter
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 1

Merge pull request #128 from Phyks/distance

Browse source 
Better distance computation - early merge, will do QS later...
This commit is contained in:
abrensch 2018-12-03 23:53:18 +01:00 committed by GitHub
commit e7afb236a6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 240 additions and 151 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
brouter-core/src/main/java/btools/router/OsmNogoPolygon.java
View file

@ -25,6 +25,8 @@ package btools.router;
import java.util.ArrayList;
import java.util.List;
import btools.util.CheapRulerSingleton;
public class OsmNogoPolygon extends OsmNodeNamed
{
public final static class Point
@ -55,15 +57,6 @@ public class OsmNogoPolygon extends OsmNodeNamed
points.add(new Point(lon, lat));
}
private final static double coslat(double lat)
{
final double l = (lat - 90000000) * 0.00000001234134; // 0.01234134 = Pi/(sqrt(2)*180)
final double l2 = l*l;
final double l4 = l2*l2;
// final double l6 = l4*l2;
return 1.- l2 + l4 / 6.; // - l6 / 90;
}
/**
* calcBoundingCircle is inspired by the algorithm described on
* http://geomalgorithms.com/a08-_containers.html
@ -82,6 +75,8 @@ public class OsmNogoPolygon extends OsmNodeNamed
*/
public void calcBoundingCircle()
{
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
int cxmin, cxmax, cymin, cymax;
cxmin = cymin = Integer.MAX_VALUE;
cxmax = cymax = Integer.MIN_VALUE;
@ -110,7 +105,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
double cx = (cxmax+cxmin) / 2.0; // center of circle
double cy = (cymax+cymin) / 2.0;
double ccoslat = coslat(cy); // cosin at latitude of center
double ccoslat = cr.cosIlat((int) cy); // cosin at latitude of center
double rad = 0; // radius
double rad2 = 0; // radius squared;
@ -148,7 +143,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
cx = cx + dd * dpx; // shift center toward point
cy = cy + dd * dpy;
ccoslat = coslat(cy);
ccoslat = cr.cosIlat((int) cy);
final Point p = points.get(i_max); // calculate new radius to just include this point
final double dpix = (p.x - cx) * ccoslat;
@ -164,7 +159,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
dpx = cx - ilon; // rounding error
dpy = cy - ilat;
// compensate rounding error of center-point
radius = (rad + Math.sqrt(dpx * dpx + dpy * dpy)) * 0.000001;
radius = (rad + Math.sqrt(dpx * dpx + dpy * dpy)) * cr.ILATLNG_TO_LATLNG;
return;
}

3
brouter-core/src/main/java/btools/router/OsmPath.java
View file

@ -12,6 +12,7 @@ import btools.mapaccess.OsmLinkHolder;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmTransferNode;
import btools.mapaccess.TurnRestriction;
import btools.util.CheapRulerSingleton;
abstract class OsmPath implements OsmLinkHolder
{
@ -344,7 +345,7 @@ abstract class OsmPath implements OsmLinkHolder
{
if ( rc.startDirectionValid )
{
double dir = rc.startDirection.intValue() / 57.29578;
double dir = rc.startDirection.intValue() / CheapRulerSingleton.DEG_TO_RAD;
lon0 = lon1 - (int) ( 1000. * Math.sin( dir ) / rc.getCosLat() );
lat0 = lat1 - (int) ( 1000. * Math.cos( dir ) );
}

12
brouter-core/src/main/java/btools/router/OsmPathElement.java
View file

@ -2,6 +2,7 @@ package btools.router;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmPos;
import btools.util.CheapRulerSingleton;
import java.io.DataInput;
import java.io.DataOutput;
@ -77,15 +78,8 @@ public class OsmPathElement implements OsmPos
public final int calcDistance( OsmPos p )
{
double l = (ilat-90000000) * 0.00000001234134;
double l2 = l*l;
double l4 = l2*l2;
double coslat = 1.- l2 + l4 / 6.;
double dlat = (ilat - p.getILat() )/1000000.;
double dlon = (ilon - p.getILon() )/1000000. * coslat;
double d = Math.sqrt( dlat*dlat + dlon*dlon ) * 110984.; // 6378000. / 57.3;
return (int)(d + 1.0 );
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
return (int)(cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
}
public OsmPathElement origin;

34
brouter-core/src/main/java/btools/router/RoutingContext.java
View file

@ -16,6 +16,7 @@ import btools.expressions.BExpressionContextNode;
import btools.expressions.BExpressionContextWay;
import btools.mapaccess.GeometryDecoder;
import btools.mapaccess.OsmLink;
import btools.util.CheapRulerSingleton;
public final class RoutingContext
{
@ -247,6 +248,7 @@ public final class RoutingContext
try { ir = Integer.parseInt( s.substring( 4 ) ); }
catch( Exception e ) { /* ignore */ }
}
// TODO[Phyks]
nogo.radius = ir / 110984.; // 6378000. / 57.3;
}
}
@ -257,6 +259,7 @@ public final class RoutingContext
List<OsmNodeNamed> nogos = new ArrayList<OsmNodeNamed>();
for( OsmNodeNamed nogo : nogopoints )
{
// TODO[Phyks]
int radiusInMeter = (int)(nogo.radius * 111894.);
boolean goodGuy = true;
for( OsmNodeNamed wp : waypoints )
@ -282,10 +285,11 @@ public final class RoutingContext
int n = nogopoints == null ? 0 : nogopoints.size();
for( int i=0; i<n; i++ )
{
OsmNodeNamed nogo = nogopoints.get(i);
cs[0] += nogo.ilon;
cs[1] += nogo.ilat;
cs[2] += (long) ( nogo.radius*111894.*10.);
OsmNodeNamed nogo = nogopoints.get(i);
cs[0] += nogo.ilon;
cs[1] += nogo.ilat;
// TODO[Phyks]
cs[2] += (long) ( nogo.radius*111894.*10.);
}
return cs;
}
@ -307,14 +311,13 @@ public final class RoutingContext
public int calcDistance( int lon1, int lat1, int lon2, int lat2 )
{
double l = (lat2 - 90000000) * 0.00000001234134;
double l2 = l*l;
double l4 = l2*l2;
coslat = 1.- l2 + l4 / 6.;
double coslat6 = coslat*0.000001;
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
coslat = cr.cosIlat(lat2);
double coslat6 = coslat*cr.ILATLNG_TO_LATLNG;
double dx = (lon2 - lon1 ) * coslat6;
double dy = (lat2 - lat1 ) * 0.000001;
double dy = (lat2 - lat1 ) * cr.ILATLNG_TO_LATLNG;
double d = Math.sqrt( dy*dy + dx*dx );
shortestmatch = false;
@ -325,9 +328,9 @@ public final class RoutingContext
{
OsmNodeNamed nogo = nogopoints.get(ngidx);
double x1 = (lon1 - nogo.ilon) * coslat6;
double y1 = (lat1 - nogo.ilat) * 0.000001;
double y1 = (lat1 - nogo.ilat) * cr.ILATLNG_TO_LATLNG;
double x2 = (lon2 - nogo.ilon) * coslat6;
double y2 = (lat2 - nogo.ilat) * 0.000001;
double y2 = (lat2 - nogo.ilat) * cr.ILATLNG_TO_LATLNG;
double r12 = x1*x1 + y1*y1;
double r22 = x2*x2 + y2*y2;
double radius = Math.abs( r12 < r22 ? y1*dx - x1*dy : y2*dx - x2*dy ) / d;
@ -363,7 +366,7 @@ public final class RoutingContext
double xm = x2 - wayfraction*dx;
double ym = y2 - wayfraction*dy;
ilonshortest = (int)(xm / coslat6 + nogo.ilon);
ilatshortest = (int)(ym / 0.000001 + nogo.ilat);
ilatshortest = (int)(ym / cr.ILATLNG_TO_LATLNG + nogo.ilat);
}
else if ( s1 > s2 )
{
@ -395,14 +398,13 @@ public final class RoutingContext
lat1 = ilatshortest;
}
dx = (lon2 - lon1 ) * coslat6;
dy = (lat2 - lat1 ) * 0.000001;
dy = (lat2 - lat1 ) * cr.ILATLNG_TO_LATLNG;
d = Math.sqrt( dy*dy + dx*dx );
}
}
}
}
double dd = d * 110984.; // 6378000. / 57.3;
return (int)(dd + 1.0 );
return (int)(cr.distance(lon1, lat1, lon2, lat2) + 1.0 );
}
// assumes that calcDistance/calcCosAngle called in sequence, so coslat valid

9
brouter-core/src/main/java/btools/router/StdPath.java
View file

@ -9,6 +9,7 @@ import btools.mapaccess.OsmLink;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmTransferNode;
import btools.mapaccess.TurnRestriction;
import btools.util.CheapRulerSingleton;
final class StdPath extends OsmPath
{
@ -183,6 +184,8 @@ final class StdPath extends OsmPath
// @Override
protected void xxxaddAddionalPenalty(OsmTrack refTrack, boolean detailMode, OsmPath origin, OsmLink link, RoutingContext rc )
{
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
byte[] description = link.descriptionBitmap;
if ( description == null ) throw new IllegalArgumentException( "null description for: " + link );
@ -361,10 +364,10 @@ final class StdPath extends OsmPath
// apply a start-direction if appropriate (by faking the origin position)
if ( lon0 == -1 && lat0 == -1 )
{
double coslat = Math.cos( ( lat1 - 90000000 ) * 0.00000001234134 );
double coslat = cr.cosIlat(lat1);
if ( rc.startDirectionValid && coslat > 0. )
{
double dir = rc.startDirection.intValue() / 57.29578;
double dir = rc.startDirection.intValue() * 180. / Math.PI;
lon0 = lon1 - (int) ( 1000. * Math.sin( dir ) / coslat );
lat0 = lat1 - (int) ( 1000. * Math.cos( dir ) );
}
@ -646,7 +649,7 @@ final class StdPath extends OsmPath
if (rc.footMode )
{
// Use Tobler's hiking function for walking sections
speed = 6 * Math.exp(-3.5 * Math.abs( incline + 0.05)) / 3.6;
speed = 6 * exp(-3.5 * Math.abs( incline + 0.05)) / 3.6;
}
else if (rc.bikeMode)
{

18
brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java
View file

@ -5,6 +5,7 @@ import java.util.List;
import btools.codec.WaypointMatcher;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmNodePairSet;
import btools.util.CheapRulerSingleton;
/**
* the WaypointMatcher is feeded by the decoder with geoemtries of ways that are
@ -32,6 +33,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher
this.islandPairs = islandPairs;
for ( MatchedWaypoint mwp : waypoints )
{
// TODO[Phyks]
mwp.radius = maxDistance * 110984.; // 6378000. / 57.3;
}
}
@ -40,14 +42,12 @@ public final class WaypointMatcherImpl implements WaypointMatcher
{
// todo: bounding-box pre-filter
double l = ( lat2 - 90000000 ) * 0.00000001234134;
double l2 = l * l;
double l4 = l2 * l2;
double coslat = 1. - l2 + l4 / 6.;
double coslat6 = coslat * 0.000001;
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
double coslat = cr.cosIlat(lat2);
double coslat6 = coslat * cr.ILATLNG_TO_LATLNG;
double dx = ( lon2 - lon1 ) * coslat6;
double dy = ( lat2 - lat1 ) * 0.000001;
double dy = ( lat2 - lat1 ) * cr.ILATLNG_TO_LATLNG;
double d = Math.sqrt( dy * dy + dx * dx );
if ( d == 0. )
return;
@ -57,9 +57,9 @@ public final class WaypointMatcherImpl implements WaypointMatcher
OsmNodeNamed wp = mwp.waypoint;
double x1 = ( lon1 - wp.ilon ) * coslat6;
double y1 = ( lat1 - wp.ilat ) * 0.000001;
double y1 = ( lat1 - wp.ilat ) * cr.ILATLNG_TO_LATLNG;
double x2 = ( lon2 - wp.ilon ) * coslat6;
double y2 = ( lat2 - wp.ilat ) * 0.000001;
double y2 = ( lat2 - wp.ilat ) * cr.ILATLNG_TO_LATLNG;
double r12 = x1 * x1 + y1 * y1;
double r22 = x2 * x2 + y2 * y2;
double radius = Math.abs( r12 < r22 ? y1 * dx - x1 * dy : y2 * dx - x2 * dy ) / d;
@ -93,7 +93,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher
double xm = x2 - wayfraction * dx;
double ym = y2 - wayfraction * dy;
mwp.crosspoint.ilon = (int) ( xm / coslat6 + wp.ilon );
mwp.crosspoint.ilat = (int) ( ym / 0.000001 + wp.ilat );
mwp.crosspoint.ilat = (int) ( ym / cr.ILATLNG_TO_LATLNG + wp.ilat );
}
else if ( s1 > s2 )
{

20
brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java
View file

@ -26,6 +26,7 @@ import org.junit.BeforeClass;
import org.junit.Test;
import btools.router.OsmNogoPolygon.Point;
import btools.util.CheapRulerSingleton;
public class OsmNogoPolygonTest {
@ -46,15 +47,6 @@ public class OsmNogoPolygonTest {
return (int)( ( lat + 90. ) *1000000. + 0.5)+offset_y;
}
static double coslat(int lat) // see RoutingContext.calcDistance()
{
final double l = (lat - 90000000) * 0.00000001234134; // 0.01234134 = Pi/(sqrt(2)*180)
final double l2 = l*l;
final double l4 = l2*l2;
// final double l6 = l4*l2;
return 1.- l2 + l4 / 6.; // - l6 / 90;
}
@BeforeClass
public static void setUp() throws Exception {
polygon = new OsmNogoPolygon(true);
@ -73,13 +65,15 @@ public class OsmNogoPolygonTest {
@Test
public void testCalcBoundingCircle() {
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
polygon.calcBoundingCircle();
double r = polygon.radius;
for (int i=0; i<lons.length; i++) {
double py = toOsmLat(lats[i]);
double dpx = (toOsmLon(lons[i]) - polygon.ilon) * coslat(polygon.ilat);
double dpx = (toOsmLon(lons[i]) - polygon.ilon) * cr.cosIlat(polygon.ilat);
double dpy = py - polygon.ilat;
double r1 = Math.sqrt(dpx * dpx + dpy * dpy) * 0.000001;
double r1 = Math.sqrt(dpx * dpx + dpy * dpy) * cr.ILATLNG_TO_LATLNG;
double diff = r-r1;
assertTrue("i: "+i+" r("+r+") >= r1("+r1+")", diff >= 0);
}
@ -87,9 +81,9 @@ public class OsmNogoPolygonTest {
r = polyline.radius;
for (int i=0; i<lons.length; i++) {
double py = toOsmLat(lats[i]);
double dpx = (toOsmLon(lons[i]) - polyline.ilon) * coslat(polyline.ilat);
double dpx = (toOsmLon(lons[i]) - polyline.ilon) * cr.cosIlat(polyline.ilat);
double dpy = py - polyline.ilat;
double r1 = Math.sqrt(dpx * dpx + dpy * dpy) * 0.000001;
double r1 = Math.sqrt(dpx * dpx + dpy * dpy) * cr.ILATLNG_TO_LATLNG;
double diff = r-r1;
assertTrue("i: "+i+" r("+r+") >= r1("+r1+")", diff >= 0);
}

4
brouter-map-creator/src/main/java/btools/mapcreator/SrtmRaster.java
View file

@ -1,5 +1,6 @@
package btools.mapcreator;
import btools.util.CheapRulerSingleton;
import btools.util.ReducedMedianFilter;
/**
@ -228,7 +229,8 @@ public class SrtmRaster
private static Weights[][] calcWeights( int latIndex )
{
double coslat = Math.cos( latIndex * 5. / 57.3 );
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
double coslat = cr.cosLat(latIndex * 5.);
// radius in pixel units
double ry = filterDiscRadius;

12
brouter-mapaccess/src/main/java/btools/mapaccess/OsmNode.java
View file

@ -9,6 +9,7 @@ import btools.codec.MicroCache;
import btools.codec.MicroCache2;
import btools.expressions.BExpressionContextWay;
import btools.util.ByteArrayUnifier;
import btools.util.CheapRulerSingleton;
import btools.util.IByteArrayUnifier;
public class OsmNode extends OsmLink implements OsmPos
@ -102,15 +103,8 @@ public class OsmNode extends OsmLink implements OsmPos
public final int calcDistance( OsmPos p )
{
double l = ( ilat - 90000000 ) * 0.00000001234134;
double l2 = l * l;
double l4 = l2 * l2;
double coslat = 1. - l2 + l4 / 6.;
double dlat = ( ilat - p.getILat() );
double dlon = ( ilon - p.getILon() ) * coslat;
double d = Math.sqrt( dlat * dlat + dlon * dlon ) * 0.110984; // 6378000. / 57.3;
return (int) ( d + 1.0 );
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
return (int) (cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0);
}
public String toString()

12
brouter-mem-router/src/main/java/btools/memrouter/OsmNodeP.java
View file

@ -6,6 +6,7 @@
package btools.memrouter;
import btools.mapaccess.OsmPos;
import btools.util.CheapRulerSingleton;
public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
{
@ -102,15 +103,8 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
@Override
public int calcDistance( OsmPos p )
{
double l = (ilat-90000000) * 0.00000001234134;
double l2 = l*l;
double l4 = l2*l2;
double coslat = 1.- l2 + l4 / 6.;
double dlat = (ilat - p.getILat() )/1000000.;
double dlon = (ilon - p.getILon() )/1000000. * coslat;
double d = Math.sqrt( dlat*dlat + dlon*dlon ) * 110984.; // 6378000. / 57.3;
return (int)(d + 1.0 );
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
return (int)(cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
}
@Override

4
brouter-routing-app/src/main/java/btools/routingapp/BRouterView.java
View file

@ -38,6 +38,7 @@ import btools.router.OsmTrack;
import btools.router.RoutingContext;
import btools.router.RoutingEngine;
import btools.router.RoutingHelper;
import btools.util.CheapRulerSingleton;
public class BRouterView extends View
{
@ -508,7 +509,8 @@ public class BRouterView extends View
centerLon = ( maxlon + minlon ) / 2;
centerLat = ( maxlat + minlat ) / 2;
double coslat = Math.cos( ( ( centerLat / 1000000. ) - 90. ) / 57.3 );
CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
double coslat = cr.cosIlat(centerLat);
double difflon = maxlon - minlon;
double difflat = maxlat - minlat;

108
brouter-util/src/main/java/btools/util/CheapRulerSingleton.java Normal file
View file

@ -0,0 +1,108 @@
package btools.util;
public final class CheapRulerSingleton {
/**
* Cheap-Ruler Java implementation
* See
* https://blog.mapbox.com/fast-geodesic-approximations-with-cheap-ruler-106f229ad016
* for more details.
*
* Original code is at https://github.com/mapbox/cheap-ruler under ISC license.
*
* This is implemented as a Singleton to have a unique cache for the cosine
* values across all the code.
*/
private static volatile CheapRulerSingleton instance = null;
// Conversion constants
public final static double ILATLNG_TO_LATLNG = 1e-6; // From integer to degrees
public final static int KILOMETERS_TO_METERS = 1000;
public final static double DEG_TO_RAD = Math.PI / 180.;
// Cosine cache constants
private final static int COS_CACHE_LENGTH = 8192;
private final static double COS_CACHE_MAX_DEGREES = 90.0;
// COS_CACHE_LENGTH cached values between 0 and COS_CACHE_MAX_DEGREES degrees.
double[] COS_CACHE = new double[COS_CACHE_LENGTH];
/**
* Helper to build the cache of cosine values.
*/
private void buildCosCache() {
double increment = DEG_TO_RAD * COS_CACHE_MAX_DEGREES / COS_CACHE_LENGTH;
for (int i = 0; i < COS_CACHE_LENGTH; i++) {
COS_CACHE[i] = Math.cos(i * increment);
}
}
private CheapRulerSingleton() {
super();
// Build the cache of cosine values.
buildCosCache();
}
/**
* Get an instance of this singleton class.
*/
public final static CheapRulerSingleton getInstance() {
if (CheapRulerSingleton.instance == null) {
synchronized(CheapRulerSingleton.class) {
if (CheapRulerSingleton.instance == null) {
CheapRulerSingleton.instance = new CheapRulerSingleton();
}
}
}
return CheapRulerSingleton.instance;
}
/**
* Helper to compute the cosine of an integer latitude.
*/
public double cosIlat(int ilat) {
double latDegrees = ilat * ILATLNG_TO_LATLNG;
if (ilat > 90000000) {
// Use the symmetry of the cosine.
latDegrees -= 90;
}
return COS_CACHE[(int) (latDegrees * COS_CACHE_LENGTH / COS_CACHE_MAX_DEGREES)];
}
/**
* Helper to compute the cosine of a latitude (in degrees).
*/
public double cosLat(double lat) {
if (lat < 0) {
lat += 90.;
}
return COS_CACHE[(int) (lat * COS_CACHE_LENGTH / COS_CACHE_MAX_DEGREES)];
}
/**
* Compute the distance (in meters) between two points represented by their
* (integer) latitude and longitude.
*
* @param ilon1 Integer longitude for the start point. this is (longitude in degrees + 180) * 1e6.
* @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.
*
* @note Integer longitude is ((longitude in degrees) + 180) * 1e6.
* Integer latitude is ((latitude in degrees) + 90) * 1e6.
*/
public double distance(int ilon1, int ilat1, int ilon2, int ilat2) {
double cos = cosIlat(ilat1);
double cos2 = 2 * cos * cos - 1;
double cos3 = 2 * cos * cos2 - cos;
double cos4 = 2 * cos * cos3 - cos2;
double cos5 = 2 * cos * cos4 - cos3;
// Multipliers for converting integer longitude and latitude into distance
// (http://1.usa.gov/1Wb1bv7)
double kx = (111.41513 * cos - 0.09455 * cos3 + 0.00012 * cos5) * ILATLNG_TO_LATLNG * KILOMETERS_TO_METERS;
double ky = (111.13209 - 0.56605 * cos2 + 0.0012 * cos4) * ILATLNG_TO_LATLNG * KILOMETERS_TO_METERS;
double dlat = (ilat1 - ilat2) * ky;
double dlon = (ilon1 - ilon2) * kx;
return Math.sqrt(dlat * dlat + dlon * dlon); // in m
}
}