diff --git a/brouter-core/src/main/java/btools/router/OsmPathElement.java b/brouter-core/src/main/java/btools/router/OsmPathElement.java index 388140d..e4ec377 100644 --- a/brouter-core/src/main/java/btools/router/OsmPathElement.java +++ b/brouter-core/src/main/java/btools/router/OsmPathElement.java @@ -2,6 +2,7 @@ package btools.router; import btools.mapaccess.OsmNode; import btools.mapaccess.OsmPos; +import btools.util.CheapRuler; import java.io.DataInput; import java.io.DataOutput; @@ -18,7 +19,7 @@ public class OsmPathElement implements OsmPos private int ilat; // latitude private int ilon; // longitude private short selev; // longitude - + public MessageData message = null; // description public int cost; @@ -77,15 +78,7 @@ 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 ); + return (int)(CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 ); } public OsmPathElement origin; @@ -109,7 +102,7 @@ public class OsmPathElement implements OsmPos pe.origin = origin; return pe; } - + protected OsmPathElement() { } @@ -122,7 +115,7 @@ public class OsmPathElement implements OsmPos { return ilon + "_" + ilat; } - + public void writeToStream( DataOutput dos ) throws IOException { dos.writeInt( ilat ); diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 8db475f..877a8f8 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/brouter-core/src/main/java/btools/router/RoutingContext.java @@ -307,6 +307,7 @@ public final class RoutingContext public int calcDistance( int lon1, int lat1, int lon2, int lat2 ) { + // TODO[Phyks] double l = (lat2 - 90000000) * 0.00000001234134; double l2 = l*l; double l4 = l2*l2; diff --git a/brouter-mapaccess/src/main/java/btools/mapaccess/OsmNode.java b/brouter-mapaccess/src/main/java/btools/mapaccess/OsmNode.java index 26b0c45..c001714 100644 --- a/brouter-mapaccess/src/main/java/btools/mapaccess/OsmNode.java +++ b/brouter-mapaccess/src/main/java/btools/mapaccess/OsmNode.java @@ -9,6 +9,7 @@ import btools.codec.MicroCache; import btools.codec.MicroCache2; import btools.expressions.BExpressionContextWay; import btools.util.ByteArrayUnifier; +import btools.util.CheapRuler; import btools.util.IByteArrayUnifier; public class OsmNode extends OsmLink implements OsmPos @@ -32,7 +33,7 @@ public class OsmNode extends OsmLink implements OsmPos * The node-tags, if any */ public byte[] nodeDescription; - + public TurnRestriction firstRestriction; /** @@ -102,15 +103,7 @@ 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 ); + return (int) (CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0); } public String toString() @@ -131,7 +124,7 @@ public class OsmNode extends OsmLink implements OsmPos public final void parseNodeBody2( MicroCache2 mc, OsmNodesMap hollowNodes, IByteArrayUnifier expCtxWay ) { ByteArrayUnifier abUnifier = hollowNodes.getByteArrayUnifier(); - + // read turn restrictions while( mc.readBoolean() ) { @@ -149,7 +142,7 @@ public class OsmNode extends OsmLink implements OsmPos selev = mc.readShort(); int nodeDescSize = mc.readVarLengthUnsigned(); nodeDescription = nodeDescSize == 0 ? null : mc.readUnified( nodeDescSize, abUnifier ); - + OsmLink link0 = firstlink; while (mc.hasMoreData()) @@ -233,7 +226,7 @@ public class OsmNode extends OsmLink implements OsmPos public final void unlinkLink( OsmLink link ) { OsmLink n = link.clear( this ); - + if ( link == firstlink ) { firstlink = n; diff --git a/brouter-mem-router/src/main/java/btools/memrouter/OsmNodeP.java b/brouter-mem-router/src/main/java/btools/memrouter/OsmNodeP.java index 5c7c2f8..90f8e11 100644 --- a/brouter-mem-router/src/main/java/btools/memrouter/OsmNodeP.java +++ b/brouter-mem-router/src/main/java/btools/memrouter/OsmNodeP.java @@ -6,6 +6,7 @@ package btools.memrouter; import btools.mapaccess.OsmPos; +import btools.util.CheapRuler; public class OsmNodeP extends OsmLinkP implements Comparable, OsmPos { @@ -17,7 +18,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable, OsmPos public OsmNodeP() { } - + /** * The latitude */ @@ -36,7 +37,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable, OsmPos public final static int NO_BRIDGE_BIT = 1; public final static int NO_TUNNEL_BIT = 2; - + public byte wayBits = 0; // interface OsmPos @@ -102,15 +103,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable, 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 ); + return (int)(CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 ); } @Override @@ -150,7 +143,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable, OsmPos { return in; // do nothing (StationNode overrides) } - + public String getName() { return ""; diff --git a/brouter-util/src/main/java/btools/util/CheapRuler.java b/brouter-util/src/main/java/btools/util/CheapRuler.java new file mode 100644 index 0000000..da090b2 --- /dev/null +++ b/brouter-util/src/main/java/btools/util/CheapRuler.java @@ -0,0 +1,42 @@ +package btools.util; + +public final class CheapRuler { + /** + * 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. + */ + static int KILOMETERS_TO_METERS = 1000; + static double ILATLNG_TO_LATLNG = 1e-6; + static double DEG_TO_RAD = Math.PI / 180.; + + /* + * @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. + */ + static public double distance(int ilon1, int ilat1, int ilon2, int ilat2) { + double lat1 = ilat1 * ILATLNG_TO_LATLNG - 90.; // Real latitude, in degrees + double cos = Math.cos(lat1 * DEG_TO_RAD); + 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 + } +}