Add a basic implementation of CheapRuler and use it across code.

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;
+    return (int)(CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
-    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 );
   }
 
   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 );

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;

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;
+    return (int) (CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0);
-    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 );
   }
 
   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;

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<OsmNodeP>, OsmPos
 {
@@ -17,7 +18,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
  public OsmNodeP()
  {
  }
- 
+
  /**
    * The latitude
    */
@@ -36,7 +37,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, 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<OsmNodeP>, OsmPos
   @Override
   public int calcDistance( OsmPos p )
   {
-    double l = (ilat-90000000) * 0.00000001234134;
+    return (int)(CheapRuler.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
-    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 );
   }
 
   @Override
@@ -150,7 +143,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
   {
     return in; // do nothing (StationNode overrides)
   }
-  
+
   public String getName()
   {
     return "<waynode>";

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
+  }
+}