Merge pull request #128 from Phyks/distance

Better distance computation - early merge, will do QS later...

brouter-core/src/main/java/btools/router/OsmNogoPolygon.java

@@ -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
@@ -40,9 +42,9 @@ public class OsmNogoPolygon extends OsmNodeNamed
   }
 
   public final List<Point> points = new ArrayList<Point>();
-  
+
   public final boolean isClosed;
-  
+
   public OsmNogoPolygon(boolean closed)
   {
     this.isClosed = closed;
@@ -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
@@ -74,7 +67,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
   * with each iteration.
   * This is done to ensure the calculated radius being used
   * in RoutingContext.calcDistance will actually contain the whole polygon.
-  * 
+  *
   * For reasonable distributed vertices the implemented algorithm runs in O(n*ln(n)).
   * As this is only run once on initialization of OsmNogoPolygon this methods
   * overall usage of cpu is neglegible in comparism to the cpu-usage of the
@@ -82,10 +75,12 @@ 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;
-    
+
     // first calculate a starting center point as center of boundingbox
     for (int i = 0; i < points.size(); i++)
     {
@@ -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;
 
@@ -120,7 +115,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
     int i_max = -1;
 
     do
-    { // now identify the point outside of the circle that has the greatest distance 
+    { // 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);
@@ -145,10 +140,10 @@ public class OsmNogoPolygon extends OsmNodeNamed
       }
       final double dist = Math.sqrt(dmax2);
       final double dd = 0.5 * (dist - rad) / dist;
-  
+
       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;
@@ -158,13 +153,13 @@ public class OsmNogoPolygon extends OsmNodeNamed
       i_max = -1;
     }
     while (true);
-    
+
     ilon = (int) Math.round(cx);
     ilat = (int) Math.round(cy);
     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;
   }
 
@@ -174,7 +169,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
   * the polygon. For this test the winding-number algorithm is
   * being used. That means a point being within an overlapping region of the
   * polygon is also taken as being 'inside' the polygon.
-  * 
+  *
   * @param lon0 longitude of start point
   * @param lat0 latitude of start point
   * @param lon1 longitude of end point
@@ -215,14 +210,14 @@ public class OsmNogoPolygon extends OsmNodeNamed
     }
     return false;
   }
-  
+
   public static boolean isOnLine( long px, long py, long p0x, long p0y, long p1x, long p1y )
   {
     final double v10x = px-p0x;
     final double v10y = py-p0y;
     final double v12x = p1x-p0x;
     final double v12y = p1y-p0y;
-    
+
     if ( v10x == 0 ) // P0->P1 vertical?
     {
       if ( v10y == 0 ) // P0 == P1?
@@ -231,7 +226,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
       }
       if ( v12x != 0 ) // P1->P2 not vertical?
       {
-        return false; 
+        return false;
       }
       return ( v12y / v10y ) >= 1; // P1->P2 at least as long as P1->P0?
     }
@@ -239,7 +234,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
     {
       if ( v12y != 0 ) // P1->P2 not horizontal?
       {
-        return false; 
+        return false;
       }
       // if ( P10x == 0 ) // P0 == P1? already tested
       return ( v12x / v10x ) >= 1; // P1->P2 at least as long as P1->P0?
@@ -260,7 +255,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
    their application. */
  /**
   * winding number test for a point in a polygon
-  * 
+  *
   * @param p a point
   * @param v list of vertex points forming a polygon. This polygon
   *          is implicitly closed connecting the last and first point.
@@ -275,7 +270,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
     final Point p0 = points.get(isClosed ? i_last : 0);
     long p0x = p0.x; // need to use long to avoid overflow in products
     long p0y = p0.y;
-    
+
     for (int i = isClosed ? 0 : 1; i <= i_last; i++) // edge from v[i] to v[i+1]
     {
       final Point p1 = points.get(i);
@@ -299,7 +294,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
         }
       }
       else // start y > p.y (no test needed)
-      {         
+      {
         if (p1y <= py) // a downward crossing
         {              // p right of edge
           if (((p1x - p0x) * (py - p0y) - (px - p0x) * (p1y - p0y)) < 0)
@@ -322,7 +317,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
    their application. */
  /**
   * inSegment(): determine if a point is inside a segment
-  * 
+  *
   * @param p a point
   * @param seg_p0 starting point of segment
   * @param seg_p1 ending point of segment
@@ -333,7 +328,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
   {
     final int sp0x = seg_p0.x;
     final int sp1x = seg_p1.x;
-    
+
     if (sp0x != sp1x) // S is not vertical
     {
       final int px = p.x;
@@ -351,7 +346,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
       final int sp0y = seg_p0.y;
       final int sp1y = seg_p1.y;
       final int py = p.y;
-      
+
       if (sp0y <= py && py <= sp1y)
       {
         return true;
@@ -363,7 +358,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
     }
     return false;
   }
-  
+
 /* 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
@@ -371,7 +366,7 @@ public class OsmNogoPolygon extends OsmNodeNamed
    resulting from its use. Users of this code must verify correctness for
    their application. */
  /**
-  * intersect2D_2Segments(): find the 2D intersection of 2 finite segments 
+  * intersect2D_2Segments(): find the 2D intersection of 2 finite segments
   * @param s1p0 start point of segment 1
   * @param s1p1 end point of segment 1
   * @param s2p0 start point of segment 2
@@ -381,14 +376,14 @@ public class OsmNogoPolygon extends OsmNodeNamed
   *         2=overlap in segment from I0 to I1
   */
   private static int intersect2D_2Segments( final Point s1p0, final Point s1p1, final Point s2p0, final Point s2p1 )
-  {     
+  {
     final long ux = s1p1.x - s1p0.x; // vector u = S1P1-S1P0 (segment 1)
     final long uy = s1p1.y - s1p0.y;
     final long vx = s2p1.x - s2p0.x; // vector v = S2P1-S2P0 (segment 2)
     final long vy = s2p1.y - s2p0.y;
     final long wx = s1p0.x - s2p0.x; // vector w = S1P0-S2P0 (from start of segment 2 to start of segment 1
     final long wy = s1p0.y - s2p0.y;
-    
+
     final double d = ux * vy - uy * vx;
 
     // test if  they are parallel (includes either being a point)
@@ -441,13 +436,13 @@ public class OsmNogoPolygon extends OsmNodeNamed
       }
       t0 = t0<0? 0 : t0;               // clip to min 0
       t1 = t1>1? 1 : t1;               // clip to max 1
-    
+
       return (t0 == t1) ? 1 : 2;        // return 1 if intersect is a point
     }
 
     // the segments are skew and may intersect in a point
     // get the intersect parameter for S1
-    
+
     final double sI = (vx * wy - vy * wx) / d;
     if (sI < 0 || sI > 1)               // no intersect with S1
     {

brouter-core/src/main/java/btools/router/OsmPath.java

@@ -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 ) );
         }

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.CheapRulerSingleton;
 
 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,8 @@ public class OsmPathElement implements OsmPos
 
   public final int calcDistance( OsmPos p )
   {
-    double l = (ilat-90000000) * 0.00000001234134;
+    CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
-    double l2 = l*l;
+    return (int)(cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
-    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 +103,7 @@ public class OsmPathElement implements OsmPos
     pe.origin = origin;
     return pe;
   }
-  
+
   protected OsmPathElement()
   {
   }
@@ -122,7 +116,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

@@ -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);
+      OsmNodeNamed nogo = nogopoints.get(i);
-    	cs[0] += nogo.ilon;
+      cs[0] += nogo.ilon;
-    	cs[1] += nogo.ilat;
+      cs[1] += nogo.ilat;
-    	cs[2] += (long) ( nogo.radius*111894.*10.);
+      // 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;
+    CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
-    double l2 = l*l;
+
-    double l4 = l2*l2;
+    coslat = cr.cosIlat(lat2);
-    coslat = 1.- l2 + l4 / 6.;
+    double coslat6 = coslat*cr.ILATLNG_TO_LATLNG;
-    double coslat6 = coslat*0.000001;
 
     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)(cr.distance(lon1, lat1, lon2, lat2) + 1.0 );
-    return (int)(dd + 1.0 );
   }
 
   // assumes that calcDistance/calcCosAngle called in sequence, so coslat valid

brouter-core/src/main/java/btools/router/StdPath.java

@@ -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)
     {

brouter-core/src/main/java/btools/router/WaypointMatcherImpl.java

@@ -5,11 +5,12 @@ 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
  * already check for allowed access according to the current routing profile
- * 
+ *
  * It matches these geometries against the list of waypoints to find the best
  * match for each waypoint
  */
@@ -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;
+    CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
-    double l2 = l * l;
+    double coslat = cr.cosIlat(lat2);
-    double l4 = l2 * l2;
+    double coslat6 = coslat * cr.ILATLNG_TO_LATLNG;
-    double coslat = 1. - l2 + l4 / 6.;
-    double coslat6 = coslat * 0.000001;
 
     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 )
         {

brouter-core/src/test/java/btools/router/OsmNogoPolygonTest.java

@@ -26,12 +26,13 @@ import org.junit.BeforeClass;
 import org.junit.Test;
 
 import btools.router.OsmNogoPolygon.Point;
+import btools.util.CheapRulerSingleton;
 
 public class OsmNogoPolygonTest {
 
   static final int offset_x = 11000000;
   static final int offset_y = 50000000;
-  
+
   static OsmNogoPolygon polygon;
   static OsmNogoPolygon polyline;
 
@@ -41,20 +42,11 @@ public class OsmNogoPolygonTest {
   static int toOsmLon(double lon) {
     return (int)( ( lon + 180. ) *1000000. + 0.5)+offset_x; // see ServerHandler.readPosition()
   }
-  
+
   static int toOsmLat(double lat) {
     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);
@@ -66,20 +58,22 @@ public class OsmNogoPolygonTest {
       polyline.addVertex(toOsmLon(lons[i]),toOsmLat(lats[i]));
     }
   }
-  
+
   @AfterClass
   public static void tearDown() throws Exception {
   }
 
   @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);
     }
@@ -97,7 +91,7 @@ public class OsmNogoPolygonTest {
 
   @Test
   public void testIsWithin() {
-    double[] plons   = {  0.0,   0.5,   1.0,  -1.5,  -0.5,  1.0,  1.0,  0.5,  0.5,  0.5, }; 
+    double[] plons   = {  0.0,   0.5,   1.0,  -1.5,  -0.5,  1.0,  1.0,  0.5,  0.5,  0.5, };
     double[] plats   = {  0.0,   1.5,   0.0,   0.5,  -1.5, -1.0, -0.1, -0.1,  0.0,  0.1, };
     boolean[] within = { true, false, false, false, false, true, true, true, true, true, };
 

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

@@ -1,5 +1,6 @@
 package btools.mapcreator;
 
+import btools.util.CheapRulerSingleton;
 import btools.util.ReducedMedianFilter;
 
 /**
@@ -26,7 +27,7 @@ public class SrtmRaster
   {
     double lon = ilon / 1000000. - 180.;
     double lat = ilat / 1000000. - 90.;
-    
+
     if ( usingWeights )
     {
       return getElevationFromShiftWeights( lon, lat );
@@ -50,7 +51,7 @@ public class SrtmRaster
              + (   wrow)*(1.-wcol)*get(row+1,col  )
              + (1.-wrow)*(   wcol)*get(row  ,col+1)
              + (   wrow)*(   wcol)*get(row+1,col+1);
-// System.out.println( "eval=" + eval );    
+// System.out.println( "eval=" + eval );
     return missingData ? Short.MIN_VALUE : (short)(eval*4);
   }
 
@@ -60,7 +61,7 @@ public class SrtmRaster
     if ( e == Short.MIN_VALUE ) missingData = true;
     return e;
   }
-  
+
   private short getElevationFromShiftWeights( double lon, double lat )
   {
     // calc lat-idx and -weight
@@ -68,7 +69,7 @@ public class SrtmRaster
     alat /= 5.;
     int latIdx = (int)alat;
     double wlat = alat - latIdx;
-  
+
     double dcol = (lon - xllcorner)/cellsize;
     double drow = (lat - yllcorner)/cellsize;
     int row = (int)drow;
@@ -78,7 +79,7 @@ public class SrtmRaster
     double dgy = (drow-row)*gridSteps;
 
 //      System.out.println( "wrow=" + wrow + " wcol=" + wcol + " row=" + row + " col=" + col );
-      
+
     int gx = (int)(dgx);
     int gy = (int)(dgy);
 
@@ -89,12 +90,12 @@ public class SrtmRaster
     double w01 = (1.-wx)*(   wy);
     double w10 = (   wx)*(1.-wy);
     double w11 = (   wx)*(   wy);
-    
+
     Weights[][] w0 = getWeights( latIdx   );
     Weights[][] w1 = getWeights( latIdx+1 );
 
     missingData = false;
-    
+
     double m0 = w00*getElevation( w0[gx  ][gy  ], row, col )
               + w01*getElevation( w0[gx  ][gy+1], row, col )
               + w10*getElevation( w0[gx+1][gy  ], row, col )
@@ -110,7 +111,7 @@ public class SrtmRaster
   }
 
   private ReducedMedianFilter rmf = new ReducedMedianFilter( 256 );
-  
+
   private double getElevation( Weights w, int row, int col )
   {
     if ( missingData )
@@ -186,7 +187,7 @@ public class SrtmRaster
       return weights[ iy*nx + ix ];
     }
   }
-  
+
   private static int gridSteps = 10;
   private static Weights[][][] allShiftWeights = new Weights[17][][];
 
@@ -208,15 +209,15 @@ public class SrtmRaster
       System.out.println( "using filterCenterFraction = " + filterCenterFraction );
     }
   }
-  
+
-  
+
   // calculate interpolation weights from the overlap of a probe disc of given radius at given latitude
   // ( latIndex = 0 -> 0 deg, latIndex = 16 -> 80 degree)
 
   private static Weights[][] getWeights( int latIndex )
   {
     int idx = latIndex < 16 ? latIndex : 16;
-  
+
     Weights[][] res = allShiftWeights[idx];
     if ( res == null )
     {
@@ -228,23 +229,24 @@ 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;
     double rx = ry / coslat;
-    
+
     // gridsize is 2*radius + 1 cell
     int nx = ((int)rx) *2 + 3;
     int ny = ((int)ry) *2 + 3;
 
     System.out.println( "nx="+ nx + " ny=" + ny );
-    
+
     int mx = nx / 2; // mean pixels
     int my = ny / 2;
 
     // create a matrix for the relative intergrid-position
-    
+
     Weights[][] shiftWeights = new Weights[gridSteps+1][];
 
     // loop the intergrid-position
@@ -262,11 +264,11 @@ public class SrtmRaster
         shiftWeights[gx][gy] = weights;
 
         double sampleStep = 0.001;
-        
+
         for( double x = -1. + sampleStep/2.; x < 1.; x += sampleStep )
         {
           double mx2 = 1. - x*x;
-          
+
           int  x_idx = (int)(x0 + x*rx);
 
           for( double y = -1. + sampleStep/2.; y < 1.; y += sampleStep )

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.CheapRulerSingleton;
 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,8 @@ public class OsmNode extends OsmLink implements OsmPos
 
   public final int calcDistance( OsmPos p )
   {
-    double l = ( ilat - 90000000 ) * 0.00000001234134;
+    CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
-    double l2 = l * l;
+    return (int) (cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0);
-    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 +125,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 +143,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 +227,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.CheapRulerSingleton;
 
 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,8 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
   @Override
   public int calcDistance( OsmPos p )
   {
-    double l = (ilat-90000000) * 0.00000001234134;
+    CheapRulerSingleton cr = CheapRulerSingleton.getInstance();
-    double l2 = l*l;
+    return (int)(cr.distance(ilon, ilat, p.getILon(), p.getILat()) + 1.0 );
-    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 +144,7 @@ public class OsmNodeP extends OsmLinkP implements Comparable<OsmNodeP>, OsmPos
   {
     return in; // do nothing (StationNode overrides)
   }
-  
+
   public String getName()
   {
     return "<waynode>";

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

@@ -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
 {
@@ -247,7 +248,7 @@ public class BRouterView extends View
         if ( fileName.equals( "lookups.dat" ) )
           lookupsFound = true;
       }
-      
+
       // add a "last timeout" dummy profile
       File lastTimeoutFile = new File( modesDir + "/timeoutdata.txt" );
       long lastTimeoutTime = lastTimeoutFile.lastModified();
@@ -255,7 +256,7 @@ public class BRouterView extends View
       {
         profiles.add( 0, "<repeat timeout>" );
       }
-      
+
       if ( !lookupsFound )
       {
         throw new IllegalArgumentException( "The profile-directory " + profileDir + " does not contain the lookups.dat file."
@@ -339,7 +340,7 @@ public class BRouterView extends View
       {
         msg = "Error reading waypoints: " + e.toString();
       }
-      
+
       int size = cor.allpoints.size();
       if ( size < 1 )
         msg = "coordinate source does not contain any waypoints!";
@@ -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;
 
@@ -522,7 +524,7 @@ public class BRouterView extends View
       startTime = System.currentTimeMillis();
       RoutingContext.prepareNogoPoints( nogoList );
       rc.nogopoints = nogoList;
-      
+
       rc.memoryclass = memoryClass;
       if ( memoryClass < 16 )
       {
@@ -532,9 +534,9 @@ public class BRouterView extends View
       {
         rc.memoryclass = 256;
       }
-        
 
-      // for profile remote, use ref-track logic same as service interface 
+
+      // for profile remote, use ref-track logic same as service interface
       rc.rawTrackPath = rawTrackPath;
 
       cr = new RoutingEngine( tracksDir + "/brouter", null, segmentDir, wpList, rc );
@@ -642,7 +644,7 @@ public class BRouterView extends View
       canvas.drawCircle( (float) x, (float) y, (float) ir, paint );
     }
   }
-  
+
   private void paintLine( Canvas canvas, final int ilon0, final int ilat0, final int ilon1, final int ilat1, final Paint paint )
   {
     final int lon0 = ilon0 - centerLon;
@@ -655,7 +657,7 @@ public class BRouterView extends View
     final int y1 = imgh / 2 - (int) ( scaleLat * lat1 );
     canvas.drawLine( (float) x0, (float) y0,  (float) x1,  (float) y1, paint );
   }
-  
+
   private void paintPolygon( Canvas canvas, OsmNogoPolygon p, int minradius )
   {
     final int ir = (int) ( p.radius * 1000000. * scaleLat );
@@ -666,7 +668,7 @@ public class BRouterView extends View
       paint.setStyle( Paint.Style.STROKE );
 
       Point p0 = p.isClosed ? p.points.get(p.points.size()-1) : null;
-  
+
       for ( final Point p1 : p.points )
       {
         if (p0 != null)
@@ -865,7 +867,7 @@ public class BRouterView extends View
         {
           paintPolygon( canvas, (OsmNogoPolygon)n, 4 );
         }
-        else 
+        else
         {
           int color = 0xff0000;
           paintCircle( canvas, n, color, 4 );

brouter-util/src/main/java/btools/util/CheapRulerSingleton.java

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