Repository: graphhopper/graphhopper
Pull request #1944 (2020-03-03 02:00:39)

Refactor distance calculation utilities to be elevation-aware
In preparation for making Douglas Peucker polyline simplification elevation-aware ([discussion](https://discuss.graphhopper.com/t/improving-elevation-profiles-for-short-routes/5244), [larger diff I'm breaking into separate reviews](https://github.com/graphhopper/graphhopper/compare/master...msbarry:3d-polyline-simplification?expand=1)), this review implements `calcNormalizedEdgeDistance3D` on each `DistanceCalc` implementation so 3D Douglas Peucker can call it from [here](https://github.com/graphhopper/graphhopper/compare/master...msbarry:3d-polyline-simplification?expand=1#diff-5b705c3852c005af6602a800351dfb28R147) and refactors a bit:

- Now that each implementation is 3D-aware, remove `DistanceCalc3D` and add a 3D `calcDist` method onto each implementation
- Rename `DistanceCalc2D` to `DistanceCalcEuclidean` since it also handles elevation


Patch:
diff --git a/api/src/main/java/com/graphhopper/util/DistanceCalc.java b/api/src/main/java/com/graphhopper/util/DistanceCalc.java
index 13418e866..03f753bd0 100644
--- a/api/src/main/java/com/graphhopper/util/DistanceCalc.java
+++ b/api/src/main/java/com/graphhopper/util/DistanceCalc.java
@@ -38,6 +38,11 @@ public interface DistanceCalc {
      */
     double calcDist(double fromLat, double fromLon, double toLat, double toLon);
 
+    /**
+     * Calculates 3d distance of (from, to) in meter.
+     */
+    double calcDist3D(double fromLat, double fromLon, double fromEle, double toLat, double toLon, double toEle);
+
     /**
      * Returns the specified length in normalized meter.
      */
@@ -85,6 +90,15 @@ public interface DistanceCalc {
                                       double a_lat_deg, double a_lon_deg,
                                       double b_lat_deg, double b_lon_deg);
 
+    /**
+     * This method calculates the distance from r to edge (a, b) where the crossing point is c including elevation
+     *
+     * @return the distance in normalized meter
+     */
+    double calcNormalizedEdgeDistance3D(double r_lat_deg, double r_lon_deg, double r_ele_m,
+                                        double a_lat_deg, double a_lon_deg, double a_ele_m,
+                                        double b_lat_deg, double b_lon_deg, double b_ele_m);
+
     /**
      * @return the crossing point c of the vertical line from r to line (a, b)
      */
diff --git a/api/src/main/java/com/graphhopper/util/DistanceCalcEarth.java b/api/src/main/java/com/graphhopper/util/DistanceCalcEarth.java
index 4c7bdbac5..5262a5dc7 100644
--- a/api/src/main/java/com/graphhopper/util/DistanceCalcEarth.java
+++ b/api/src/main/java/com/graphhopper/util/DistanceCalcEarth.java
@@ -39,6 +39,7 @@ public class DistanceCalcEarth implements DistanceCalc {
      */
     public final static double C = 2 * PI * R;
     public final static double KM_MILE = 1.609344;
+    public final static double METERS_PER_DEGREE = C / 360.0;
 
     /**
      * Calculates distance of (from, to) in meter.
@@ -52,6 +53,20 @@ public class DistanceCalcEarth implements DistanceCalc {
         return R * 2 * asin(sqrt(normedDist));
     }
 
+    /**
+     * This implements a rather quick solution to calculate 3D distances on earth using euclidean
+     * geometry mixed with Haversine formula used for the on earth distance. The haversine formula makes
+     * not so much sense as it is only important for large distances where then the rather smallish
+     * heights would becomes negligible.
+     */
+    @Override
+    public double calcDist3D(double fromLat, double fromLon, double fromHeight,
+                             double toLat, double toLon, double toHeight) {
+        double eleDelta = hasElevationDiff(fromHeight, toHeight) ? (toHeight - fromHeight) : 0;
+        double len = calcDist(fromLat, fromLon, toLat, toLon);
+        return Math.sqrt(eleDelta * eleDelta + len * len);
+    }
+
     @Override
     public double calcDenormalizedDist(double normedDist) {
         return R * 2 * asin(sqrt(normedDist));
@@ -107,6 +122,41 @@ public class DistanceCalcEarth implements DistanceCalc {
         return calcNormalizedEdgeDistanceNew(r_lat_deg, r_lon_deg, a_lat_deg, a_lon_deg, b_lat_deg, b_lon_deg, false);
     }
 
+    @Override
+    public double calcNormalizedEdgeDistance3D(double r_lat_deg, double r_lon_deg, double r_ele_m,
+                                               double a_lat_deg, double a_lon_deg, double a_ele_m,
+                                               double b_lat_deg, double b_lon_deg, double b_ele_m) {
+        if (Double.isNaN(r_ele_m) || Double.isNaN(a_ele_m) || Double.isNaN(b_ele_m))
+            return calcNormalizedEdgeDistance(r_lat_deg, r_lon_deg, a_lat_deg, a_lon_deg, b_lat_deg, b_lon_deg);
+
+        double shrinkFactor = calcShrinkFactor(a_lat_deg, b_lat_deg);
+
+        double a_lat = a_lat_deg;
+        double a_lon = a_lon_deg * shrinkFactor;
+        double a_ele = a_ele_m / METERS_PER_DEGREE;
+
+        double b_lat = b_lat_deg;
+        double b_lon = b_lon_deg * shrinkFactor;
+        double b_ele = b_ele_m / METERS_PER_DEGREE;
+
+        double r_lat = r_lat_deg;
+        double r_lon = r_lon_deg * shrinkFactor;
+        double r_ele = r_ele_m / METERS_PER_DEGREE;
+
+        double delta_lon = b_lon - a_lon;
+        double delta_lat = b_lat - a_lat;
+        double delta_ele = b_ele - a_ele;
+
+        double norm = delta_lon * delta_lon + delta_lat * delta_lat + delta_ele * delta_ele;
+        double factor = ((r_lon - a_lon) * delta_lon + (r_lat - a_lat) * delta_lat + (r_ele - a_ele) * delta_ele) / norm;
+
+        // x,y,z is projection of r onto segment a-b
+        double c_lon = a_lon + factor * delta_lon;
+        double c_lat = a_lat + factor * delta_lat;
+        double c_ele_m = (a_ele + factor * delta_ele) * METERS_PER_DEGREE;
+        return calcNormalizedDist(c_lat, c_lon / shrinkFactor, r_lat_deg, r_lon_deg) + calcNormalizedDist(r_ele_m - c_ele_m);
+    }
+
     /**
      * New edge distance calculation where no validEdgeDistance check would be necessary
      * <p>
@@ -116,7 +166,8 @@ public class DistanceCalcEarth implements DistanceCalc {
      */
     public double calcNormalizedEdgeDistanceNew(double r_lat_deg, double r_lon_deg,
                                                 double a_lat_deg, double a_lon_deg,
-                                                double b_lat_deg, double b_lon_deg, boolean reduceToSegment) {
+                                                double b_lat_deg, double b_lon_deg,
+                                                boolean reduceToSegment) {
         double shrinkFactor = calcShrinkFactor(a_lat_deg, b_lat_deg);
 
         double a_lat = a_lat_deg;
@@ -136,7 +187,7 @@ public class DistanceCalcEarth implements DistanceCalc {
             return calcNormalizedDist(a_lat_deg, r_lon_deg, r_lat_deg, r_lon_deg);
 
         if (delta_lon == 0)
-            // special case: vertical edge        
+            // special case: vertical edge
             return calcNormalizedDist(r_lat_deg, a_lon_deg, r_lat_deg, r_lon_deg);
 
         double norm = delta_lon * delta_lon + delta_lat * delta_lat;
@@ -254,6 +305,10 @@ public class DistanceCalcEarth implements DistanceCalc {
         return abs(lon1 - lon2) > 300;
     }
 
+    protected boolean hasElevationDiff(double a, double b) {
+        return a != b && !Double.isNaN(a) && !Double.isNaN(b);
+    }
+
     @Override
     public String toString() {
         return "EXACT";
diff --git a/api/src/main/java/com/graphhopper/util/DistancePlaneProjection.java b/api/src/main/java/com/graphhopper/util/DistancePlaneProjection.java
index 74088b39d..fd040d296 100644
--- a/api/src/main/java/com/graphhopper/util/DistancePlaneProjection.java
+++ b/api/src/main/java/com/graphhopper/util/DistancePlaneProjection.java
@@ -36,14 +36,18 @@ import static java.lang.Math.*;
 public class DistancePlaneProjection extends DistanceCalcEarth {
     @Override
     public double calcDist(double fromLat, double fromLon, double toLat, double toLon) {
-        double dLat = toRadians(toLat - fromLat);
-        double dLon = toRadians(toLon - fromLon);
-        // use mean latitude as reference point for delta_lon
-        double tmp = cos(toRadians((fromLat + toLat) / 2)) * dLon;
-        double normedDist = dLat * dLat + tmp * tmp;
+        double normedDist = calcNormalizedDist(fromLat, fromLon, toLat, toLon);
         return R * sqrt(normedDist);
     }
 
+    @Override
+    public double calcDist3D(double fromLat, double fromLon, double fromHeight,
+                             double toLat, double toLon, double toHeight) {
+        double dEleNorm = hasElevationDiff(fromHeight, toHeight) ? calcNormalizedDist(toHeight - fromHeight) : 0;
+        double normedDist = calcNormalizedDist(fromLat, fromLon, toLat, toLon);
+        return R * sqrt(normedDist + dEleNorm);
+    }
+
     @Override
     public double calcDenormalizedDist(double normedDist) {
         return R * sqrt(normedDist);
diff --git a/api/src/main/java/com/graphhopper/util/Helper.java b/api/src/main/java/com/graphhopper/util/Helper.java
index 60b3b32c6..8695f3ec4 100644
--- a/api/src/main/java/com/graphhopper/util/Helper.java
+++ b/api/src/main/java/com/graphhopper/util/Helper.java
@@ -36,7 +36,6 @@ import java.util.Map.Entry;
  */
 public class Helper {
     public static final DistanceCalc DIST_EARTH = new DistanceCalcEarth();
-    public static final DistanceCalc3D DIST_3D = new DistanceCalc3D();
     public static final DistancePlaneProjection DIST_PLANE = new DistancePlaneProjection();
     public static final AngleCalc ANGLE_CALC = new AngleCalc();
     public static final Charset UTF_CS = Charset.forName("UTF-8");
diff --git a/api/src/main/java/com/graphhopper/util/PointList.java b/api/src/main/java/com/graphhopper/util/PointList.java
index 69aec762e..77410bf04 100644
--- a/api/src/main/java/com/graphhopper/util/PointList.java
+++ b/api/src/main/java/com/graphhopper/util/PointList.java
@@ -152,7 +152,6 @@ public class PointList implements Iterable<GHPoint3D>, PointAccess {
         }
     };
 
-    private final static DistanceCalc3D distCalc3D = new DistanceCalc3D();
     final static String ERR_MSG = "Tried to access PointList with too big index!";
     protected int size = 0;
     protected boolean is3D;
@@ -549,7 +548,7 @@ public class PointList implements Iterable<GHPoint3D>, PointAccess {
         for (int i = 0; i < size(); i++) {
             if (i > 0) {
                 if (is3D())
-                    dist += distCalc3D.calcDist(prevLat, prevLon, prevEle, getLat(i), getLon(i), getEle(i));
+                    dist += calc.calcDist3D(prevLat, prevLon, prevEle, getLat(i), getLon(i), getEle(i));
                 else
                     dist += calc.calcDist(prevLat, prevLon, getLat(i), getLon(i));
             }
diff --git a/benchmark/benchmark.sh b/benchmark/benchmark.sh
index 0548f7cf7..90f5098dc 100755
--- a/benchmark/benchmark.sh
+++ b/benchmark/benchmark.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 # usage:
-# benchmark/benchmark.sh <data_dir> <results_dir> <small_osm_map_path> <big_osm_map_path> <use_measurement_time_as_ref_time>
+# benchmark/benchmark.sh <data_dir> <results_dir> <small_osm_map_path> <big_osm_map_path> <use_measurement_time_as_ref_time> <spatial_rule_borders_dir>
 #
 # where:
 # <data_dir> = base directory used to store results
@@ -8,6 +8,7 @@
 # <small_osm_map_path> = path to osm map the measurement is run on for slow measurements
 # <big_osm_map_path> = path to osm map the measurement is run on for fast measurements
 # <use_measurement_time_as_ref_time> = true/false, false by default, meaning the git commit time will be used as reference
+# <spatial_rule_borders_dir> = base directory to load borders of spatial rules from
 
 # make this script exit if a command fails, a variable is missing etc.
 set -euo pipefail
@@ -17,6 +18,7 @@ defaultSingleResultsDir=measurements/results/$(date '+%d-%m-%Y-%s%N')/
 defaultSmallMap=core/files/andorra.osm.pbf
 defaultBigMap=core/files/andorra.osm.pbf
 defaultUseMeasurementTimeAsRefTime=false
+defaultBordersDirectory=core/files/spatialrules
 
 # this is the directory where we read/write data from/to
 DATA_DIR=${1:-$defaultDataDir}
@@ -26,6 +28,7 @@ SINGLE_RESULTS_DIR=${2:-$defaultSingleResultsDir}
 SMALL_OSM_MAP=${3:-$defaultSmallMap}
 BIG_OSM_MAP=${4:-$defaultBigMap}
 USE_MEASUREMENT_TIME_AS_REF_TIME=${5:-$defaultUseMeasurementTimeAsRefTime}
+BORDERS_DIRECTORY=${6:-$defaultBordersDirectory}
 
 # create directories
 mkdir -p ${DATA_DIR}
@@ -51,6 +54,7 @@ measurement.lm=false \
 graph.location=${TMP_DIR}measurement-small-gh \
 prepare.min_network_size=10000 \
 prepare.min_oneway_network_size=10000 \
+spatial_rules.borders_directory=${BORDERS_DIRECTORY} \
 measurement.json=true \
 measurement.count=5000 \
 measurement.use_measurement_time_as_ref_time=${USE_MEASUREMENT_TIME_AS_REF_TIME}
@@ -72,6 +76,7 @@ measurement.lm=true \
 graph.location=${TMP_DIR}measurement-big-gh \
 prepare.min_network_size=10000 \
 prepare.min_oneway_network_size=10000 \
+spatial_rules.borders_directory=${BORDERS_DIRECTORY} \
 measurement.json=true \
 measurement.count=5000 \
 measurement.use_measurement_time_as_ref_time=${USE_MEASUREMENT_TIME_AS_REF_TIME}
diff --git a/core/src/main/java/com/graphhopper/GraphHopper.java b/core/src/main/java/com/graphhopper/GraphHopper.java
index 31494d6ad..f2f76a827 100644
--- a/core/src/main/java/com/graphhopper/GraphHopper.java
+++ b/core/src/main/java/com/graphhopper/GraphHopper.java
@@ -595,9 +595,11 @@ public class GraphHopper implements GraphHopperAPI {
     private static ElevationProvider createElevationProvider(GraphHopperConfig ghConfig) {
         String eleProviderStr = toLowerCase(ghConfig.get("graph.elevation.provider", "noop"));
 
-        // keep fallback until 0.8
-        boolean eleCalcMean = ghConfig.has("graph.elevation.calcmean")
-                ? ghConfig.getBool("graph.elevation.calcmean", false)
+        if (ghConfig.has("graph.elevation.calcmean"))
+            throw new IllegalArgumentException("graph.elevation.calcmean is deprecated, use graph.elevation.interpolate");
+
+        boolean interpolate = ghConfig.has("graph.elevation.interpolate")
+                ? "bilinear".equals(ghConfig.get("graph.elevation.interpolate", "none"))
                 : ghConfig.getBool("graph.elevation.calc_mean", false);
 
         String cacheDirStr = ghConfig.get("graph.elevation.cache_dir", "");
@@ -626,7 +628,7 @@ public class GraphHopper implements GraphHopperAPI {
         }
 
         elevationProvider.setAutoRemoveTemporaryFiles(removeTempElevationFiles);
-        elevationProvider.setCalcMean(eleCalcMean);
+        elevationProvider.setInterpolate(interpolate);
         if (!baseURL.isEmpty())
             elevationProvider.setBaseURL(baseURL);
         elevationProvider.setDAType(elevationDAType);
@@ -1188,7 +1190,7 @@ public class GraphHopper implements GraphHopperAPI {
         GHPoint lastPoint = points.get(0);
         GHPoint point;
         double dist;
-        DistanceCalc calc = DIST_3D;
+        DistanceCalc calc = DIST_EARTH;
         for (int i = 1; i < points.size(); i++) {
             point = points.get(i);
             dist = calc.calcDist(lastPoint.getLat(), lastPoint.getLon(), point.getLat(), point.getLon());
diff --git a/core/src/main/java/com/graphhopper/reader/dem/AbstractElevationProvider.java b/core/src/main/java/com/graphhopper/reader/dem/AbstractElevationProvider.java
index 733a2e1c2..676eab263 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/AbstractElevationProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/AbstractElevationProvider.java
@@ -39,7 +39,7 @@ public abstract class AbstractElevationProvider implements ElevationProvider {
     String baseUrl;
     Directory dir;
     DAType daType = DAType.MMAP;
-    boolean calcMean = false;
+    boolean interpolate = false;
     boolean autoRemoveTemporary = true;
     long sleep = 2000;
 
@@ -55,8 +55,8 @@ public abstract class AbstractElevationProvider implements ElevationProvider {
     }
 
     @Override
-    public void setCalcMean(boolean eleCalcMean) {
-        calcMean = eleCalcMean;
+    public void setInterpolate(boolean interpolate) {
+        this.interpolate = interpolate;
     }
 
     void setSleep(long sleep) {
@@ -97,7 +97,7 @@ public abstract class AbstractElevationProvider implements ElevationProvider {
             return dir;
 
         logger.info(this.toString() + " Elevation Provider, from: " + baseUrl + ", to: " + cacheDir + ", as: " + daType +
-                " using calcmean: " + calcMean);
+                " using interpolate: " + interpolate);
         return dir = new GHDirectory(cacheDir.getAbsolutePath(), daType);
     }
 
diff --git a/core/src/main/java/com/graphhopper/reader/dem/AbstractSRTMElevationProvider.java b/core/src/main/java/com/graphhopper/reader/dem/AbstractSRTMElevationProvider.java
index 81cbdbb8b..d9176ab35 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/AbstractSRTMElevationProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/AbstractSRTMElevationProvider.java
@@ -127,7 +127,7 @@ public abstract class AbstractSRTMElevationProvider extends AbstractElevationPro
 
             demProvider = new HeightTile(minLat, minLon, width, width, precision, DEGREE, DEGREE);
             cacheData.put(intKey, demProvider);
-            demProvider.setCalcMean(calcMean);
+            demProvider.setInterpolate(interpolate);
             demProvider.setHeights(heights);
         }
 
diff --git a/core/src/main/java/com/graphhopper/reader/dem/AbstractTiffElevationProvider.java b/core/src/main/java/com/graphhopper/reader/dem/AbstractTiffElevationProvider.java
index 7c32b6ad2..ebf1800a5 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/AbstractTiffElevationProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/AbstractTiffElevationProvider.java
@@ -104,7 +104,7 @@ public abstract class AbstractTiffElevationProvider extends AbstractElevationPro
             int minLon = getMinLonForTile(lon);
             // less restrictive against boundary checking
             demProvider = new HeightTile(minLat, minLon, WIDTH, HEIGHT, LON_DEGREE * precision, LON_DEGREE, LAT_DEGREE);
-            demProvider.setCalcMean(calcMean);
+            demProvider.setInterpolate(interpolate);
 
             cacheData.put(name, demProvider);
             DataAccess heights = getDirectory().find(name + ".gh");
diff --git a/core/src/main/java/com/graphhopper/reader/dem/EdgeElevationInterpolator.java b/core/src/main/java/com/graphhopper/reader/dem/EdgeElevationInterpolator.java
index feb760886..3bcb5881f 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/EdgeElevationInterpolator.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/EdgeElevationInterpolator.java
@@ -157,7 +157,7 @@ public class EdgeElevationInterpolator {
                 }
                 if (count > 2)
                     edge.setWayGeometry(pointList.shallowCopy(1, count - 1, false));
-                edge.setDistance(pointList.calcDistance(Helper.DIST_3D));
+                edge.setDistance(pointList.calcDistance(Helper.DIST_EARTH));
             }
         }
     }
diff --git a/core/src/main/java/com/graphhopper/reader/dem/ElevationProvider.java b/core/src/main/java/com/graphhopper/reader/dem/ElevationProvider.java
index 1e1cafaf0..7fc3fc7f0 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/ElevationProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/ElevationProvider.java
@@ -48,7 +48,7 @@ public interface ElevationProvider {
         }
 
         @Override
-        public void setCalcMean(boolean eleCalcMean) {
+        public void setInterpolate(boolean interpolate) {
         }
     };
 
@@ -69,10 +69,11 @@ public interface ElevationProvider {
     ElevationProvider setDAType(DAType daType);
 
     /**
-     * Configuration option to include surrounding elevation points when fetching the elevation. Has
-     * only an effect if called before the first getEle call. Turned off by default.
+     * Configuration option to use bilinear interpolation to find the elevation at a point from the
+     * surrounding elevation points. Has only an effect if called before the first getEle call.
+     * Turned off by default.
      */
-    void setCalcMean(boolean calcMean);
+    void setInterpolate(boolean interpolate);
 
     /**
      * Release resources.
diff --git a/core/src/main/java/com/graphhopper/reader/dem/GMTEDProvider.java b/core/src/main/java/com/graphhopper/reader/dem/GMTEDProvider.java
index 1faab6054..37cc11031 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/GMTEDProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/GMTEDProvider.java
@@ -132,7 +132,7 @@ public class GMTEDProvider extends AbstractTiffElevationProvider {
         System.out.println(provider.getEle(48.469123, 9.576393));
 
         // 1113 vs new: (1115)
-        provider.setCalcMean(true);
+        provider.setInterpolate(true);
         System.out.println(provider.getEle(47.467753, 14.573911));
 
         // 0
diff --git a/core/src/main/java/com/graphhopper/reader/dem/HeightTile.java b/core/src/main/java/com/graphhopper/reader/dem/HeightTile.java
index b8f902eab..114469d93 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/HeightTile.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/HeightTile.java
@@ -43,7 +43,9 @@ public class HeightTile {
     private final double lonHigherBound;
     private final double latHigherBound;
     private DataAccess heights;
-    private boolean calcMean;
+    private boolean interpolate;
+    private final double MIN_ELEVATION_METERS = -12_000;
+    private final double MAX_ELEVATION_METERS = 9_000;
 
     public HeightTile(int minLat, int minLon, int width, int height, double precision, int horizontalDegree, int verticalDegree) {
         this.minLat = minLat;
@@ -59,8 +61,8 @@ public class HeightTile {
         this.verticalDegree = verticalDegree;
     }
 
-    public HeightTile setCalcMean(boolean b) {
-        this.calcMean = b;
+    public HeightTile setInterpolate(boolean interpolate) {
+        this.interpolate = interpolate;
         return this;
     }
 
@@ -77,6 +79,20 @@ public class HeightTile {
         this.heights = da;
     }
 
+    private short getHeightSample(int x, int y) {
+        // always keep in mind factor 2 because of short value
+        return heights.getShort(2 * (y * width + x));
+    }
+
+    private boolean isValidElevation(double elevation) {
+        return elevation > MIN_ELEVATION_METERS && elevation < MAX_ELEVATION_METERS;
+    }
+
+    private double linearInterpolate(double a, double b, double f) {
+        // interpolate between a and b but if either are invalid, return the other
+        return !isValidElevation(a) ? b : !isValidElevation(b) ? a : (a + (b - a) * f);
+    }
+
     public double getHeight(double lat, double lon) {
         double deltaLat = Math.abs(lat - minLat);
         double deltaLon = Math.abs(lon - minLon);
@@ -85,47 +101,37 @@ public class HeightTile {
         if (deltaLon > lonHigherBound || deltaLon < lowerBound)
             throw new IllegalStateException("longitude not in boundary of this file:" + lat + "," + lon + ", this:" + this.toString());
 
-        // first row in the file is the northernmost one
-        // http://gis.stackexchange.com/a/43756/9006
-        int lonSimilar = (int) (width / horizontalDegree * deltaLon);
-        // different fallback methods for lat and lon as we have different rounding (lon -> positive, lat -> negative)
-        if (lonSimilar >= width)
-            lonSimilar = width - 1;
-        int latSimilar = height - 1 - (int) (height / verticalDegree * deltaLat);
-        if (latSimilar < 0)
-            latSimilar = 0;
-
-        // always keep in mind factor 2 because of short value
-        int daPointer = 2 * (latSimilar * width + lonSimilar);
-        int value = heights.getShort(daPointer);
-        AtomicInteger counter = new AtomicInteger(1);
-        if (value == Short.MIN_VALUE)
-            return Double.NaN;
-
-        if (calcMean) {
-            if (lonSimilar > 0)
-                value += includePoint(daPointer - 2, counter);
-
-            if (lonSimilar < width - 1)
-                value += includePoint(daPointer + 2, counter);
-
-            if (latSimilar > 0)
-                value += includePoint(daPointer - 2 * width, counter);
-
-            if (latSimilar < height - 1)
-                value += includePoint(daPointer + 2 * width, counter);
+        double elevation;
+        if (interpolate) {
+            double x = (width - 1) * deltaLon / horizontalDegree;
+            double y = (height - 1) * (1 - deltaLat / verticalDegree);
+            int left = (int) x;
+            int top = (int) y;
+            int right = left + 1;
+            int bottom = top + 1;
+
+            double w00 = getHeightSample(left, top);
+            double w01 = getHeightSample(left, bottom);
+            double w10 = getHeightSample(right, top);
+            double w11 = getHeightSample(right, bottom);
+
+            double topEle = linearInterpolate(w00, w10, x - left);
+            double bottomEle = linearInterpolate(w01, w11, x - left);
+            elevation = linearInterpolate(topEle, bottomEle, y - top);
+        } else {
+            // first row in the file is the northernmost one
+            // http://gis.stackexchange.com/a/43756/9006
+            int x = (int) (width / horizontalDegree * deltaLon);
+            // different fallback methods for lat and lon as we have different rounding (lon -> positive, lat -> negative)
+            if (x >= width)
+                x = width - 1;
+            int y = height - 1 - (int) (height / verticalDegree * deltaLat);
+            if (y < 0)
+                y = 0;
+
+            elevation = getHeightSample(x, y);
         }
-
-        return (double) value / counter.get();
-    }
-
-    private double includePoint(int pointer, AtomicInteger counter) {
-        short value = heights.getShort(pointer);
-        if (value == Short.MIN_VALUE)
-            return 0;
-
-        counter.incrementAndGet();
-        return value;
+        return isValidElevation(elevation) ? elevation : Double.NaN;
     }
 
     public void toImage(String imageFile) throws IOException {
diff --git a/core/src/main/java/com/graphhopper/reader/dem/MultiSourceElevationProvider.java b/core/src/main/java/com/graphhopper/reader/dem/MultiSourceElevationProvider.java
index 570c10105..451a7d4ff 100644
--- a/core/src/main/java/com/graphhopper/reader/dem/MultiSourceElevationProvider.java
+++ b/core/src/main/java/com/graphhopper/reader/dem/MultiSourceElevationProvider.java
@@ -77,9 +77,9 @@ public class MultiSourceElevationProvider implements ElevationProvider {
     }
 
     @Override
-    public void setCalcMean(boolean calcMean) {
-        srtmProvider.setCalcMean(calcMean);
-        globalProvider.setCalcMean(calcMean);
+    public void setInterpolate(boolean interpolate) {
+        srtmProvider.setInterpolate(interpolate);
+        globalProvider.setInterpolate(interpolate);
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/util/DistanceCalc2D.java b/core/src/main/java/com/graphhopper/util/DistanceCalcEuclidean.java
similarity index 66%
rename from core/src/main/java/com/graphhopper/util/DistanceCalc2D.java
rename to core/src/main/java/com/graphhopper/util/DistanceCalcEuclidean.java
index c34596ee5..276ab1f02 100644
--- a/core/src/main/java/com/graphhopper/util/DistanceCalc2D.java
+++ b/core/src/main/java/com/graphhopper/util/DistanceCalcEuclidean.java
@@ -28,12 +28,17 @@ import com.graphhopper.util.shapes.GHPoint;
  *
  * @author Peter Karich
  */
-public class DistanceCalc2D extends DistanceCalcEarth {
+public class DistanceCalcEuclidean extends DistanceCalcEarth {
     @Override
     public double calcDist(double fromY, double fromX, double toY, double toX) {
         return sqrt(calcNormalizedDist(fromY, fromX, toY, toX));
     }
 
+    @Override
+    public double calcDist3D(double fromY, double fromX, double fromHeight, double toY, double toX, double toHeight) {
+        return sqrt(calcNormalizedDist(fromY, fromX, toY, toX) + calcNormalizedDist(toHeight - fromHeight));
+    }
+
     @Override
     public double calcDenormalizedDist(double normedDist) {
         return sqrt(normedDist);
@@ -91,4 +96,38 @@ public class DistanceCalc2D extends DistanceCalcEarth {
     public boolean isCrossBoundary(double lon1, double lon2) {
         throw new UnsupportedOperationException("Not supported for the 2D Euclidean space");
     }
+
+    @Override
+    public double calcNormalizedEdgeDistance(double ry, double rx,
+                                             double ay, double ax,
+                                             double by, double bx) {
+        return calcNormalizedEdgeDistance3D(
+            ry, rx, 0,
+            ay, ax, 0,
+            by, bx, 0
+        );
+    }
+
+    @Override
+    public double calcNormalizedEdgeDistance3D(double ry, double rx, double rz,
+                                               double ay, double ax, double az,
+                                               double by, double bx, double bz) {
+        double dx = bx - ax;
+        double dy = by - ay;
+        double dz = bz - az;
+
+        double norm = dx * dx + dy * dy + dz * dz;
+        double factor = ((rx - ax) * dx + (ry - ay) * dy + (rz - az) * dz) / norm;
+
+        // x,y,z is projection of r onto segment a-b
+        double cx = ax + factor * dx;
+        double cy = ay + factor * dy;
+        double cz = az + factor * dz;
+
+        double rdx = cx - ax;
+        double rdy = cy - ay;
+        double rdz = cz - az;
+
+        return rdx * rdx + rdy * rdy + rdz * rdz;
+    }
 }
diff --git a/reader-osm/src/main/java/com/graphhopper/reader/osm/OSMReader.java b/reader-osm/src/main/java/com/graphhopper/reader/osm/OSMReader.java
index 8035d89a6..dee4bdff3 100644
--- a/reader-osm/src/main/java/com/graphhopper/reader/osm/OSMReader.java
+++ b/reader-osm/src/main/java/com/graphhopper/reader/osm/OSMReader.java
@@ -75,7 +75,6 @@ public class OSMReader implements DataReader, TurnCostParser.ExternalInternalMap
     private final NodeAccess nodeAccess;
     private final LongIndexedContainer barrierNodeIds = new LongArrayList();
     private final DistanceCalc distCalc = Helper.DIST_EARTH;
-    private final DistanceCalc3D distCalc3D = Helper.DIST_3D;
     private final DouglasPeucker simplifyAlgo = new DouglasPeucker();
     private boolean smoothElevation = false;
     private final boolean exitOnlyPillarNodeException = true;
diff --git a/tools/pom.xml b/tools/pom.xml
index 3e69cc52f..aa73df5be 100644
--- a/tools/pom.xml
+++ b/tools/pom.xml
@@ -3,9 +3,7 @@
          xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
     <modelVersion>4.0.0</modelVersion>
 
-    <groupId>com.graphhopper</groupId>
     <artifactId>graphhopper-tools</artifactId>
-    <version>1.0-SNAPSHOT</version>
     <packaging>jar</packaging>
     <name>GraphHopper Tools</name>
 
@@ -38,6 +36,10 @@
             <groupId>com.fasterxml.jackson.core</groupId>
             <artifactId>jackson-databind</artifactId>
         </dependency>
+        <dependency>
+            <groupId>com.graphhopper.external</groupId>
+            <artifactId>jackson-datatype-jts</artifactId>
+        </dependency>
         <dependency>
             <groupId>org.slf4j</groupId>
             <artifactId>slf4j-api</artifactId>
diff --git a/tools/src/main/java/com/graphhopper/tools/Measurement.java b/tools/src/main/java/com/graphhopper/tools/Measurement.java
index 45761be32..e41e90806 100644
--- a/tools/src/main/java/com/graphhopper/tools/Measurement.java
+++ b/tools/src/main/java/com/graphhopper/tools/Measurement.java
@@ -17,6 +17,8 @@
  */
 package com.graphhopper.tools;
 
+import com.bedatadriven.jackson.datatype.jts.JtsModule;
+import com.fasterxml.jackson.annotation.JsonInclude;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import com.graphhopper.*;
 import com.graphhopper.coll.GHBitSet;
@@ -24,9 +26,12 @@ import com.graphhopper.coll.GHBitSetImpl;
 import com.graphhopper.config.CHProfileConfig;
 import com.graphhopper.config.LMProfileConfig;
 import com.graphhopper.config.ProfileConfig;
+import com.graphhopper.json.geo.JsonFeatureCollection;
 import com.graphhopper.reader.DataReader;
 import com.graphhopper.reader.osm.GraphHopperOSM;
 import com.graphhopper.routing.util.*;
+import com.graphhopper.routing.util.spatialrules.*;
+import com.graphhopper.routing.util.spatialrules.SpatialRuleLookupBuilder.SpatialRuleFactory;
 import com.graphhopper.storage.*;
 import com.graphhopper.storage.index.LocationIndex;
 import com.graphhopper.util.*;
@@ -34,13 +39,20 @@ import com.graphhopper.util.Parameters.Algorithms;
 import com.graphhopper.util.Parameters.CH;
 import com.graphhopper.util.Parameters.Landmark;
 import com.graphhopper.util.shapes.BBox;
+import com.graphhopper.util.shapes.GHPoint;
+
+import org.locationtech.jts.geom.Polygon;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
+import java.io.BufferedReader;
 import java.io.File;
 import java.io.FileWriter;
 import java.io.IOException;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.DirectoryStream;
 import java.nio.file.Files;
+import java.nio.file.Path;
 import java.nio.file.Paths;
 import java.text.SimpleDateFormat;
 import java.util.*;
@@ -74,6 +86,7 @@ public class Measurement {
     // Every value is one y-value in a separate diagram with an identical x-value for every Measurement.start call
     void start(PMap args) throws IOException {
         final String graphLocation = args.get("graph.location", "");
+        final String countryBordersDirectory = args.get("spatial_rules.borders_directory", "");
         final boolean useJson = args.getBool("measurement.json", false);
         boolean cleanGraph = args.getBool("measurement.clean", false);
         String summaryLocation = args.get("measurement.summaryfile", "");
@@ -232,6 +245,10 @@ public class Measurement {
                             edgeBased().withInstructions().withPointHints().simplify());
                 }
             }
+            if (!isEmpty(countryBordersDirectory)) {
+                printSpatialRuleLookupTest(countryBordersDirectory, count * 100);
+            }
+            
         } catch (Exception ex) {
             logger.error("Problem while measuring " + graphLocation, ex);
             put("error", ex.toString());
@@ -456,6 +473,57 @@ public class Measurement {
         }.setIterations(count).start();
         print("unit_tests" + description + ".get_edge_state", miniPerf);
     }
+    
+    private void printSpatialRuleLookupTest(String countryBordersDirectory, int count) {
+        ObjectMapper objectMapper = new ObjectMapper();
+        objectMapper.registerModule(new JtsModule());
+        objectMapper.setSerializationInclusion(JsonInclude.Include.NON_NULL);
+        
+        List<JsonFeatureCollection> jsonFeatureCollections = new ArrayList<>();
+        try (DirectoryStream<Path> stream = Files.newDirectoryStream(Paths.get(countryBordersDirectory), "*.{geojson,json}")) {
+            for (Path borderFile : stream) {
+                try (BufferedReader reader = Files.newBufferedReader(borderFile, StandardCharsets.UTF_8)) {
+                    JsonFeatureCollection jsonFeatureCollection = objectMapper.readValue(reader, JsonFeatureCollection.class);
+                    jsonFeatureCollections.add(jsonFeatureCollection);
+                }
+            }
+        } catch (IOException e) {
+            logger.error("Failed to load borders.", e);
+            return;
+        }
+        
+        SpatialRuleFactory rulePerCountryFactory = new SpatialRuleFactory() {
+            
+            @Override
+            public SpatialRule createSpatialRule(final String id, final List<Polygon> borders) {
+                return new AbstractSpatialRule(borders) {
+                    @Override
+                    public String getId() {
+                        return id;
+                    }
+                };
+            }
+        };
+        
+        final SpatialRuleLookup spatialRuleLookup = SpatialRuleLookupBuilder.buildIndex(jsonFeatureCollections, "ISO_A3", rulePerCountryFactory);
+    
+        // generate random points in central Europe
+        final List<GHPoint> randomPoints = new ArrayList<>(count);
+        for (int i = 0; i < count; i++) {
+            double lat = 46d + Math.random() * 7d;
+            double lon = 6d + Math.random() * 21d;
+            randomPoints.add(new GHPoint(lat, lon));
+        }
+        
+        MiniPerfTest lookupPerfTest = new MiniPerfTest() {
+            @Override
+            public int doCalc(boolean warmup, int run) {
+                return spatialRuleLookup.lookupRule(randomPoints.get(run)).hashCode();
+            }
+        }.setIterations(count).start();
+        
+        print("spatialrulelookup", lookupPerfTest);
+    }
 
     private void compareRouting(final GraphHopper hopper, String vehicle, int count) {
         logger.info("Comparing " + count + " routes. Differences will be printed to stderr.");


Test patch:
diff --git a/api/src/test/java/com/graphhopper/util/DistanceCalcEarthTest.java b/api/src/test/java/com/graphhopper/util/DistanceCalcEarthTest.java
index 5fec97fa8..921ad97d1 100644
--- a/api/src/test/java/com/graphhopper/util/DistanceCalcEarthTest.java
+++ b/api/src/test/java/com/graphhopper/util/DistanceCalcEarthTest.java
@@ -99,6 +99,49 @@ public class DistanceCalcEarthTest {
         assertEquals(expectedDist, dist, 1e-4);
     }
 
+    @Test
+    public void testEdgeDistance3d() {
+        double dist = dc.calcNormalizedEdgeDistance3D(49.94241, 11.544356, 0,
+                49.937964, 11.541824, 0,
+                49.942272, 11.555643, 0);
+        double expectedDist = dc.calcNormalizedDist(49.94241, 11.544356,
+                49.9394, 11.54681);
+        assertEquals(expectedDist, dist, 1e-4);
+
+        // test identical lats
+        dist = dc.calcNormalizedEdgeDistance3D(49.936299, 11.543992, 0,
+                49.9357, 11.543047, 0,
+                49.9357, 11.549227, 0);
+        expectedDist = dc.calcNormalizedDist(49.936299, 11.543992,
+                49.9357, 11.543992);
+        assertEquals(expectedDist, dist, 1e-4);
+    }
+
+    @Test
+    public void testEdgeDistance3dEarth() {
+        double dist = dc.calcNormalizedEdgeDistance3D(0, 0.5, 10,
+                0, 0, 0,
+                0, 1, 0);
+        assertEquals(10, dc.calcDenormalizedDist(dist), 1e-4);
+    }
+
+    @Test
+    public void testEdgeDistance3dEarthNaN() {
+        double dist = dc.calcNormalizedEdgeDistance3D(0, 0.5, Double.NaN,
+                0, 0, 0,
+                0, 1, 0);
+        assertEquals(0, dc.calcDenormalizedDist(dist), 1e-4);
+    }
+
+    @Test
+    public void testEdgeDistance3dPlane() {
+        DistanceCalc calc = new DistancePlaneProjection();
+        double dist = calc.calcNormalizedEdgeDistance3D(0, 0.5, 10,
+                0, 0, 0,
+                0, 1, 0);
+        assertEquals(10, calc.calcDenormalizedDist(dist), 1e-4);
+    }
+
     @Test
     public void testValidEdgeDistance() {
         assertTrue(dc.validEdgeDistance(49.94241, 11.544356, 49.937964, 11.541824, 49.942272, 11.555643));
@@ -158,4 +201,60 @@ public class DistanceCalcEarthTest {
         assertEquals(new GHPoint(55.81863, 37.594626), distCalc.calcCrossingPointToEdge(queryLat, queryLon,
                 tmpLat, tmpLon, wayLat, wayLon));
     }
+
+    @Test
+    public void testDistance3dEarth() {
+        DistanceCalc distCalc = new DistanceCalcEarth();
+        assertEquals(1, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, 1
+        ), 1e-6);
+    }
+
+    @Test
+    public void testDistance3dEarthNaN() {
+        DistanceCalc distCalc = new DistanceCalcEarth();
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, Double.NaN
+        ), 1e-6);
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, Double.NaN,
+                0, 0, 10
+        ), 1e-6);
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, Double.NaN,
+                0, 0, Double.NaN
+        ), 1e-6);
+    }
+
+    @Test
+    public void testDistance3dPlane() {
+        DistancePlaneProjection distCalc = new DistancePlaneProjection();
+        assertEquals(1, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, 1
+        ), 1e-6);
+        assertEquals(10, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, 10
+        ), 1e-6);
+    }
+
+    @Test
+    public void testDistance3dPlaneNaN() {
+        DistancePlaneProjection distCalc = new DistancePlaneProjection();
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, Double.NaN
+        ), 1e-6);
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, Double.NaN,
+                0, 0, 10
+        ), 1e-6);
+        assertEquals(0, distCalc.calcDist3D(
+                0, 0, Double.NaN,
+                0, 0, Double.NaN
+        ), 1e-6);
+    }
 }
diff --git a/core/src/main/java/com/graphhopper/util/MiniPerfTest.java b/core/src/main/java/com/graphhopper/util/MiniPerfTest.java
index ffe3dcfc4..2cda3fec5 100644
--- a/core/src/main/java/com/graphhopper/util/MiniPerfTest.java
+++ b/core/src/main/java/com/graphhopper/util/MiniPerfTest.java
@@ -28,11 +28,16 @@ import java.util.Locale;
  * @author Peter Karich
  */
 public abstract class MiniPerfTest {
+
+    private static final double NS_PER_S  = 1e9;
+    private static final double NS_PER_MS = 1e6;
+    private static final double NS_PER_US = 1e3;
+    
     protected Logger logger = LoggerFactory.getLogger(getClass());
     private int counts = 100;
-    private double fullTime = 0;
-    private double max;
-    private double min = Double.MAX_VALUE;
+    private long fullTime = 0;
+    private long max;
+    private long min = Long.MAX_VALUE;
     private int dummySum;
 
     public MiniPerfTest start() {
@@ -65,21 +70,21 @@ public abstract class MiniPerfTest {
      * @return minimum time of every call, in ms
      */
     public double getMin() {
-        return min / 1e6;
+        return min / NS_PER_MS;
     }
 
     /**
      * @return maximum time of every calls, in ms
      */
     public double getMax() {
-        return max / 1e6;
+        return max / NS_PER_MS;
     }
 
     /**
      * @return time for all calls accumulated, in ms
      */
     public double getSum() {
-        return fullTime / 1e6;
+        return fullTime / NS_PER_MS;
     }
 
     /**
@@ -88,16 +93,33 @@ public abstract class MiniPerfTest {
     public double getMean() {
         return getSum() / counts;
     }
+    
+    private String formatDuration(double durationNs) {
+        double divisor;
+        String unit;
+        if (durationNs > 1e7d) {
+            divisor = NS_PER_S;
+            unit = "s";
+        } else if (durationNs > 1e4d) {
+            divisor = NS_PER_MS;
+            unit = "ms";
+        } else {
+            divisor = NS_PER_US;
+            unit = "us";
+        }
+        return nf(durationNs / divisor) + unit;
+    }
 
     public String getReport() {
-        return "sum:" + nf(getSum() / 1000f) + "s, time/call:" + nf(getMean() / 1000f) + "s";
+        double meanNs = ((double) fullTime) / counts;
+        return "sum:" + formatDuration(fullTime) + ", time/call:" + formatDuration(meanNs);
     }
 
     public int getDummySum() {
         return dummySum;
     }
 
-    public String nf(Number num) {
+    private String nf(Number num) {
         return new DecimalFormat("#.###", DecimalFormatSymbols.getInstance(Locale.ROOT)).format(num);
     }
 
diff --git a/core/src/test/java/com/graphhopper/reader/dem/HeightTileTest.java b/core/src/test/java/com/graphhopper/reader/dem/HeightTileTest.java
index 9d624115c..1722c92d6 100644
--- a/core/src/test/java/com/graphhopper/reader/dem/HeightTileTest.java
+++ b/core/src/test/java/com/graphhopper/reader/dem/HeightTileTest.java
@@ -96,37 +96,63 @@ public class HeightTileTest {
     }
 
     @Test
-    public void testCalcMean() {
-        int width = 10;
-        HeightTile instance = new HeightTile(0, 0, width, width, 1e-6, 10, 10).setCalcMean(true);
+    public void testInterpolate() {
+        HeightTile instance = new HeightTile(0, 0, 2, 2, 1e-6, 10, 10).setInterpolate(true);
         DataAccess heights = new RAMDirectory().find("tmp");
-        heights.create(2 * 10 * 10);
+        heights.create(2 * 2 * 2);
         instance.setHeights(heights);
-        init(heights, width, width, 1);
-
-        // x,y=0,9
-        heights.setShort(2 * (9 * width + 0), (short) 10);
-
-        // x,y=1,7
-        heights.setShort(2 * (7 * width + 1), (short) 70);
-
-        // x,y=2,8
-        heights.setShort(2 * (8 * width + 2), (short) 90);
-
-        assertEquals((70 + 4) / 5d, instance.getHeight(2, 1), 1e-3);
-
-        assertEquals((70 + 90 + 3) / 5d, instance.getHeight(2.5, 2.5), 1e-3);
-
-        assertEquals((90 + 3) / 4d, instance.getHeight(-0.5, 2.5), 1e-3);
-
-        assertEquals((10 + 2) / 3d, instance.getHeight(-0.5, -0.5), 1e-3);
+        double topLeft = 0;
+        double topRight = 1;
+        double bottomLeft = 2;
+        double bottomRight = 3;
+        set(heights, 2, 0, 0, (short) topLeft);
+        set(heights, 2, 1, 0, (short) topRight);
+        set(heights, 2, 0, 1, (short) bottomLeft);
+        set(heights, 2, 1, 1, (short) bottomRight);
+
+        // corners
+        assertEquals(bottomLeft, instance.getHeight(0, 0), 1e-3);
+        assertEquals(topLeft, instance.getHeight(10, 0), 1e-3);
+        assertEquals(bottomRight, instance.getHeight(0, 10), 1e-3);
+        assertEquals(topRight, instance.getHeight(10, 10), 1e-3);
+
+        // midpoints
+        assertEquals(avg(topLeft, topRight), instance.getHeight(10, 5), 1e-3);
+        assertEquals(avg(bottomLeft, bottomRight), instance.getHeight(0, 5), 1e-3);
+        assertEquals(avg(topLeft, bottomLeft), instance.getHeight(5, 0), 1e-3);
+        assertEquals(avg(topRight, bottomRight, topLeft, bottomLeft), instance.getHeight(5, 5), 1e-3);
+
+        // missing data uses whatever remains
+        set(heights, 2, 1, 0, Short.MIN_VALUE);
+        set(heights, 2, 0, 1, Short.MIN_VALUE);
+        set(heights, 2, 1, 1, Short.MIN_VALUE);
+        assertEquals(topLeft, instance.getHeight(0, 0), 1e-3);
+        assertEquals(topLeft, instance.getHeight(10, 0), 1e-3);
+        assertEquals(topLeft, instance.getHeight(0, 10), 1e-3);
+        assertEquals(topLeft, instance.getHeight(10, 10), 1e-3);
+
+        // when all data missing, returns NaN
+        set(heights, 2, 0, 0, Short.MIN_VALUE);
+        assertEquals(Double.NaN, instance.getHeight(5, 5), 1e-3);
     }
 
     private void init(DataAccess da, int width, int height, int i) {
         for (int x = 0; x < width; x++) {
             for (int y = 0; y < height; y++) {
-                da.setShort(2 * (y * width + x), (short) 1);
+                set(da, width, x, y, (short) 1);
             }
         }
     }
+
+    private void set(DataAccess da, int width, int x, int y, short height) {
+        da.setShort(2 * (y * width + x), height);
+    }
+
+    private double avg(double... ns) {
+        double sum = 0;
+        for (double n : ns) {
+            sum += n;
+        }
+        return sum / ns.length;
+    }
 }
\ No newline at end of file
diff --git a/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java b/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
index 9a66d0978..62d07bb6d 100644
--- a/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
+++ b/core/src/test/java/com/graphhopper/routing/ch/PrepareContractionHierarchiesTest.java
@@ -468,7 +468,7 @@ public class PrepareContractionHierarchiesTest {
         qr.setSnappedPosition(QueryResult.Position.EDGE);
         qr.setClosestNode(8);
         qr.setWayIndex(0);
-        qr.calcSnappedPoint(new DistanceCalc2D());
+        qr.calcSnappedPoint(new DistanceCalcEuclidean());
         QueryGraph queryGraph = QueryGraph.lookup(lg, qr);
 
         // we make sure our weight fine tunings do what they are supposed to
diff --git a/core/src/test/java/com/graphhopper/routing/querygraph/QueryGraphTest.java b/core/src/test/java/com/graphhopper/routing/querygraph/QueryGraphTest.java
index 5ca674bbb..d1ffee626 100644
--- a/core/src/test/java/com/graphhopper/routing/querygraph/QueryGraphTest.java
+++ b/core/src/test/java/com/graphhopper/routing/querygraph/QueryGraphTest.java
@@ -274,7 +274,7 @@ public class QueryGraphTest {
         EdgeIteratorState edge = g.edge(0, 1);
         EdgeIteratorState edgeReverse = edge.detach(true);
 
-        DistanceCalc2D distCalc = new DistanceCalc2D();
+        DistanceCalcEuclidean distCalc = new DistanceCalcEuclidean();
         QueryResult qr = new QueryResult(0, 0.00005);
         qr.setClosestEdge(edge);
         qr.setWayIndex(0);
@@ -310,7 +310,7 @@ public class QueryGraphTest {
         QueryResult qr = new QueryResult(-0.0005, 0.001);
         qr.setClosestEdge(edge);
         qr.setWayIndex(1);
-        qr.calcSnappedPoint(new DistanceCalc2D());
+        qr.calcSnappedPoint(new DistanceCalcEuclidean());
 
         QueryGraph qg = lookup(qr);
         EdgeExplorer ee = qg.createEdgeExplorer();
@@ -342,7 +342,7 @@ public class QueryGraphTest {
         QueryResult qr = new QueryResult(0.0011, 0.0009);
         qr.setClosestEdge(edge);
         qr.setWayIndex(1);
-        qr.calcSnappedPoint(new DistanceCalc2D());
+        qr.calcSnappedPoint(new DistanceCalcEuclidean());
 
         QueryGraph qg = lookup(qr);
         EdgeExplorer ee = qg.createEdgeExplorer();
@@ -545,7 +545,7 @@ public class QueryGraphTest {
         qr.setClosestEdge(edge);
         qr.setWayIndex(wayIndex);
         qr.setSnappedPosition(EDGE);
-        qr.calcSnappedPoint(new DistanceCalc2D());
+        qr.calcSnappedPoint(new DistanceCalcEuclidean());
         return qr;
     }
 
diff --git a/core/src/test/java/com/graphhopper/routing/util/spatialrules/SpatialRuleLookupBuilderTest.java b/core/src/test/java/com/graphhopper/routing/util/spatialrules/SpatialRuleLookupBuilderTest.java
index 38e17493a..28a81b809 100644
--- a/core/src/test/java/com/graphhopper/routing/util/spatialrules/SpatialRuleLookupBuilderTest.java
+++ b/core/src/test/java/com/graphhopper/routing/util/spatialrules/SpatialRuleLookupBuilderTest.java
@@ -24,7 +24,6 @@ import com.graphhopper.routing.profiles.*;
 import com.graphhopper.routing.util.CarFlagEncoder;
 import com.graphhopper.routing.util.EncodingManager;
 import com.graphhopper.routing.util.parsers.SpatialRuleParser;
-import com.graphhopper.routing.util.spatialrules.SpatialRuleLookupBuilder.SpatialRuleFactory;
 import com.graphhopper.routing.util.spatialrules.countries.GermanySpatialRule;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.GraphBuilder;
@@ -41,8 +40,6 @@ import org.locationtech.jts.geom.Polygon;
 import java.io.FileReader;
 import java.io.IOException;
 import java.util.Collections;
-import java.util.List;
-import java.util.concurrent.TimeUnit;
 
 import static com.graphhopper.util.GHUtility.updateDistancesFor;
 import static junit.framework.TestCase.assertFalse;
@@ -210,50 +207,4 @@ public class SpatialRuleLookupBuilderTest {
         e4.setFlags(em.handleWayTags(livingStreet2, map, relFlags));
         assertEquals(MaxSpeed.UNSET_SPEED, e4.get(tmpCarMaxSpeedEnc), .1);
     }
-
-    @Test
-    public void testSpeed() throws IOException {
-        final FileReader reader = new FileReader(COUNTRIES_FILE);
-        SpatialRuleFactory rulePerCountryFactory = new SpatialRuleFactory() {
-
-            @Override
-            public SpatialRule createSpatialRule(final String id, final List<Polygon> borders) {
-                return new AbstractSpatialRule(borders) {
-                    
-                    @Override
-                    public double getMaxSpeed(RoadClass roadClass, TransportationMode transport, double currentMaxSpeed) {
-                        return 100;
-                    }
-
-                    @Override
-                    public RoadAccess getAccess(RoadClass roadClass, TransportationMode transport, RoadAccess currentRoadAccess) {
-                        return RoadAccess.YES;
-                    }
-
-                    @Override
-                    public String getId() {
-                        return id;
-                    }
-                };
-            }
-        };
-        SpatialRuleLookup spatialRuleLookup = SpatialRuleLookupBuilder.buildIndex(Collections.singletonList(
-                Jackson.newObjectMapper().readValue(reader, JsonFeatureCollection.class)), "ISO_A3", rulePerCountryFactory);
-
-        // generate random points in central Europe
-        int randomPoints = 250_000;
-        long start = System.nanoTime();
-        for (int i = 0; i < randomPoints; i++) {
-            double lat = 46d + Math.random() * 7d;
-            double lon = 6d + Math.random() * 21d;
-            spatialRuleLookup.lookupRule(new GHPoint(lat, lon));
-        }
-
-        long duration = TimeUnit.NANOSECONDS.toMillis(System.nanoTime() - start);
-        // System.out.println("Lookup of " + randomPoints + " points took " + duration + "ms");
-        // System.out.println("Average lookup duration: " + ((double) duration) / randomPoints + "ms");
-        long maxBenchmarkRuntimeMs = 5_000L;
-        assertTrue("Benchmark must be finished in less than " + maxBenchmarkRuntimeMs + "ms",
-                duration < maxBenchmarkRuntimeMs);
-    }
 }
diff --git a/core/src/test/java/com/graphhopper/util/DistanceCalc2DTest.java b/core/src/test/java/com/graphhopper/util/DistanceCalcEuclideanTest.java
similarity index 73%
rename from core/src/test/java/com/graphhopper/util/DistanceCalc2DTest.java
rename to core/src/test/java/com/graphhopper/util/DistanceCalcEuclideanTest.java
index 6ce67951c..fca9d7cbe 100644
--- a/core/src/test/java/com/graphhopper/util/DistanceCalc2DTest.java
+++ b/core/src/test/java/com/graphhopper/util/DistanceCalcEuclideanTest.java
@@ -23,11 +23,11 @@ import org.junit.Test;
 
 import com.graphhopper.util.shapes.GHPoint;
 
-public class DistanceCalc2DTest {
+public class DistanceCalcEuclideanTest {
 
     @Test
     public void testCrossingPointToEdge() {
-        DistanceCalc2D distanceCalc = new DistanceCalc2D();
+        DistanceCalcEuclidean distanceCalc = new DistanceCalcEuclidean();
         GHPoint point = distanceCalc.calcCrossingPointToEdge(0, 10, 0, 0, 10, 10);
         Assert.assertEquals(5, point.getLat(), 0);
         Assert.assertEquals(5, point.getLon(), 0);
@@ -35,17 +35,30 @@ public class DistanceCalc2DTest {
 
     @Test
     public void testCalcNormalizedEdgeDistance() {
-        DistanceCalc2D distanceCalc = new DistanceCalc2D();
+        DistanceCalcEuclidean distanceCalc = new DistanceCalcEuclidean();
         double distance = distanceCalc.calcNormalizedEdgeDistance(0, 10, 0, 0, 10, 10);
         Assert.assertEquals(50, distance, 0);
     }
 
     @Test
     public void testValidEdgeDistance() {
-        DistanceCalc2D distanceCalc = new DistanceCalc2D();
+        DistanceCalcEuclidean distanceCalc = new DistanceCalcEuclidean();
         boolean validEdgeDistance = distanceCalc.validEdgeDistance(5, 15, 0, 0, 10, 10);
         Assert.assertEquals(false, validEdgeDistance);
         validEdgeDistance = distanceCalc.validEdgeDistance(15, 5, 0, 0, 10, 10);
         Assert.assertEquals(false, validEdgeDistance);
     }
+
+    @Test
+    public void testDistance3dEuclidean() {
+        DistanceCalcEuclidean distCalc = new DistanceCalcEuclidean();
+        Assert.assertEquals(1, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, 1
+        ), 1e-6);
+        Assert.assertEquals(10, distCalc.calcDist3D(
+                0, 0, 0,
+                0, 0, 10
+        ), 1e-6);
+    }
 }
diff --git a/core/src/test/java/com/graphhopper/util/GHUtilityTest.java b/core/src/test/java/com/graphhopper/util/GHUtilityTest.java
index 77e5b7a89..971658c03 100644
--- a/core/src/test/java/com/graphhopper/util/GHUtilityTest.java
+++ b/core/src/test/java/com/graphhopper/util/GHUtilityTest.java
@@ -70,7 +70,7 @@ public class GHUtilityTest {
 
     double getLengthOfAllEdges(Graph graph) {
         double distance = 0;
-        DistanceCalc calc = new DistanceCalc2D();
+        DistanceCalc calc = new DistanceCalcEuclidean();
         AllEdgesIterator iter = graph.getAllEdges();
         while (iter.next()) {
             // This is meant to verify that all of the same edges (including tower nodes)
diff --git a/reader-gtfs/src/test/java/com/graphhopper/reader/gtfs/GraphExplorerTest.java b/reader-gtfs/src/test/java/com/graphhopper/reader/gtfs/GraphExplorerTest.java
index 440701ae4..9a8ae4bb6 100644
--- a/reader-gtfs/src/test/java/com/graphhopper/reader/gtfs/GraphExplorerTest.java
+++ b/reader-gtfs/src/test/java/com/graphhopper/reader/gtfs/GraphExplorerTest.java
@@ -26,7 +26,7 @@ import com.graphhopper.routing.weighting.FastestWeighting;
 import com.graphhopper.storage.GraphHopperStorage;
 import com.graphhopper.storage.RAMDirectory;
 import com.graphhopper.storage.index.QueryResult;
-import com.graphhopper.util.DistanceCalc2D;
+import com.graphhopper.util.DistanceCalcEuclidean;
 import com.graphhopper.util.EdgeIteratorState;
 import com.graphhopper.util.PointList;
 import org.junit.Test;
@@ -184,12 +184,12 @@ public class GraphExplorerTest {
         point1.setQueryDistance(0.0);
         point1.setWayIndex(0);
         point1.setSnappedPosition(QueryResult.Position.EDGE);
-        point1.calcSnappedPoint(new DistanceCalc2D());
+        point1.calcSnappedPoint(new DistanceCalcEuclidean());
         point2.setClosestEdge(d);
         point2.setQueryDistance(0.0);
         point2.setWayIndex(0);
         point2.setSnappedPosition(QueryResult.Position.EDGE);
-        point2.calcSnappedPoint(new DistanceCalc2D());
+        point2.calcSnappedPoint(new DistanceCalcEuclidean());
         QueryGraph queryGraph = QueryGraph.lookup(wrapperGraph, point1, point2);
 
         GraphExplorer testee = new GraphExplorer(queryGraph, new FastestWeighting(foot), pt, gtfsStorage, realtimeFeed, false, false, 5.0, false);