Repository: graphhopper/graphhopper (java)

Issue:
If heading is provided change EdgeFilter for LocationIndexTree
The issue can be reproduced with oneway streets like [this](https://graphhopper.com/maps/?point=51.211414%2C14.566305&point=51.211521%2C14.561906&ch.disable=true&heading=270), ie. the heading does not work. Expected is the snapping to the other side of the motorway but it is unclear if the heading should already be applied at the snapping step. (The ordinary case works like [here](https://graphhopper.com/maps/?point=51.192725%2C14.56094&point=51.192819%2C14.567678&ch.disable=true&heading=270))

Pull request description:
Consider heading when snapping
Fixes #1463 

Patch:
diff --git a/core/src/main/java/com/graphhopper/routing/HeadingResolver.java b/core/src/main/java/com/graphhopper/routing/HeadingResolver.java
index 1548ffabb..db22126a8 100644
--- a/core/src/main/java/com/graphhopper/routing/HeadingResolver.java
+++ b/core/src/main/java/com/graphhopper/routing/HeadingResolver.java
@@ -19,12 +19,14 @@
 package com.graphhopper.routing;
 
 import com.carrotsearch.hppc.IntArrayList;
+import com.graphhopper.routing.util.EdgeFilter;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.util.*;
+import com.graphhopper.util.shapes.GHPoint;
 
 public class HeadingResolver {
     private final EdgeExplorer edgeExplorer;
-    private double toleranceRad = deg2Rad(100);
+    private double toleranceRad = Math.toRadians(100);
 
     public HeadingResolver(Graph graph) {
         this.edgeExplorer = graph.createEdgeExplorer();
@@ -63,12 +65,7 @@ public class HeadingResolver {
      * Sets the tolerance for {@link #getEdgesWithDifferentHeading} in degrees.
      */
     public HeadingResolver setTolerance(double tolerance) {
-        this.toleranceRad = deg2Rad(tolerance);
+        this.toleranceRad = Math.toRadians(tolerance);
         return this;
     }
-
-    private static double deg2Rad(double deg) {
-        return Math.toRadians(deg);
-    }
-
 }
diff --git a/core/src/main/java/com/graphhopper/routing/Router.java b/core/src/main/java/com/graphhopper/routing/Router.java
index 26d9ce07b..26b23a746 100644
--- a/core/src/main/java/com/graphhopper/routing/Router.java
+++ b/core/src/main/java/com/graphhopper/routing/Router.java
@@ -24,6 +24,7 @@ import com.graphhopper.GHResponse;
 import com.graphhopper.ResponsePath;
 import com.graphhopper.config.Profile;
 import com.graphhopper.routing.ch.CHRoutingAlgorithmFactory;
+import com.graphhopper.routing.ev.BooleanEncodedValue;
 import com.graphhopper.routing.ev.EncodedValueLookup;
 import com.graphhopper.routing.ev.Subnetwork;
 import com.graphhopper.routing.lm.LMRoutingAlgorithmFactory;
@@ -165,12 +166,12 @@ public class Router {
     }
 
     private void checkPointHints(GHRequest request) {
-        if (request.getPointHints().size() > 0 && request.getPointHints().size() != request.getPoints().size())
+        if (!request.getPointHints().isEmpty() && request.getPointHints().size() != request.getPoints().size())
             throw new IllegalArgumentException("If you pass " + POINT_HINT + ", you need to pass exactly one hint for every point, empty hints will be ignored");
     }
 
     private void checkCurbsides(GHRequest request) {
-        if (request.getCurbsides().size() > 0 && request.getCurbsides().size() != request.getPoints().size())
+        if (!request.getCurbsides().isEmpty() && request.getCurbsides().size() != request.getPoints().size())
             throw new IllegalArgumentException("If you pass " + CURBSIDE + ", you need to pass exactly one curbside for every point, empty curbsides will be ignored");
     }
 
@@ -196,7 +197,7 @@ public class Router {
         StopWatch sw = new StopWatch().start();
         double startHeading = request.getHeadings().isEmpty() ? Double.NaN : request.getHeadings().get(0);
         RoundTripRouting.Params params = new RoundTripRouting.Params(request.getHints(), startHeading, routerConfig.getMaxRoundTripRetries());
-        List<Snap> snaps = RoundTripRouting.lookup(request.getPoints(), solver.getSnapFilter(), locationIndex, params);
+        List<Snap> snaps = RoundTripRouting.lookup(request.getPoints(), solver.createSnapFilter(), locationIndex, params);
         ghRsp.addDebugInfo("idLookup:" + sw.stop().getSeconds() + "s");
 
         QueryGraph queryGraph = QueryGraph.create(ghStorage, snaps);
@@ -216,7 +217,8 @@ public class Router {
             throw new IllegalArgumentException("Currently alternative routes work only with start and end point. You tried to use: " + request.getPoints().size() + " points");
         GHResponse ghRsp = new GHResponse();
         StopWatch sw = new StopWatch().start();
-        List<Snap> snaps = ViaRouting.lookup(encodingManager, request.getPoints(), solver.getSnapFilter(), locationIndex, request.getSnapPreventions(), request.getPointHints());
+        List<Snap> snaps = ViaRouting.lookup(encodingManager, request.getPoints(), solver.createSnapFilter(), locationIndex,
+                request.getSnapPreventions(), request.getPointHints(), solver.createDirectedSnapFilter(), request.getHeadings());
         ghRsp.addDebugInfo("idLookup:" + sw.stop().getSeconds() + "s");
         QueryGraph queryGraph = QueryGraph.create(ghStorage, snaps);
         PathCalculator pathCalculator = solver.createPathCalculator(queryGraph);
@@ -246,7 +248,8 @@ public class Router {
     protected GHResponse routeVia(GHRequest request, Solver solver) {
         GHResponse ghRsp = new GHResponse();
         StopWatch sw = new StopWatch().start();
-        List<Snap> snaps = ViaRouting.lookup(encodingManager, request.getPoints(), solver.getSnapFilter(), locationIndex, request.getSnapPreventions(), request.getPointHints());
+        List<Snap> snaps = ViaRouting.lookup(encodingManager, request.getPoints(), solver.createSnapFilter(), locationIndex,
+                request.getSnapPreventions(), request.getPointHints(), solver.createDirectedSnapFilter(), request.getHeadings());
         ghRsp.addDebugInfo("idLookup:" + sw.stop().getSeconds() + "s");
         // (base) query graph used to resolve headings, curbsides etc. this is not necessarily the same thing as
         // the (possibly implementation specific) query graph used by PathCalculator
@@ -254,7 +257,8 @@ public class Router {
         PathCalculator pathCalculator = solver.createPathCalculator(queryGraph);
         boolean passThrough = getPassThrough(request.getHints());
         boolean forceCurbsides = getForceCurbsides(request.getHints());
-        ViaRouting.Result result = ViaRouting.calcPaths(request.getPoints(), queryGraph, snaps, solver.weighting, pathCalculator, request.getCurbsides(), forceCurbsides, request.getHeadings(), passThrough);
+        ViaRouting.Result result = ViaRouting.calcPaths(request.getPoints(), queryGraph, snaps, solver.weighting,
+                pathCalculator, request.getCurbsides(), forceCurbsides, request.getHeadings(), passThrough);
 
         if (request.getPoints().size() != result.paths.size() + 1)
             throw new RuntimeException("There should be exactly one more point than paths. points:" + request.getPoints().size() + ", paths:" + result.paths.size());
@@ -377,10 +381,15 @@ public class Router {
 
         protected abstract Weighting createWeighting();
 
-        protected EdgeFilter getSnapFilter() {
+        protected EdgeFilter createSnapFilter() {
             return new DefaultSnapFilter(weighting, lookup.getBooleanEncodedValue(Subnetwork.key(profile.getName())));
         }
 
+        protected EdgeFilter createDirectedSnapFilter() {
+            BooleanEncodedValue inSubnetworkEnc = lookup.getBooleanEncodedValue(Subnetwork.key(profile.getName()));
+            return edgeState -> !edgeState.get(inSubnetworkEnc) && Double.isFinite(weighting.calcEdgeWeightWithAccess(edgeState, false));
+        }
+
         protected abstract PathCalculator createPathCalculator(QueryGraph queryGraph);
 
         private List<String> getTurnCostProfiles() {
diff --git a/core/src/main/java/com/graphhopper/routing/ViaRouting.java b/core/src/main/java/com/graphhopper/routing/ViaRouting.java
index 6966dd1e5..2cc67a77e 100644
--- a/core/src/main/java/com/graphhopper/routing/ViaRouting.java
+++ b/core/src/main/java/com/graphhopper/routing/ViaRouting.java
@@ -24,7 +24,7 @@ import com.graphhopper.routing.ev.RoadClass;
 import com.graphhopper.routing.ev.RoadEnvironment;
 import com.graphhopper.routing.querygraph.QueryGraph;
 import com.graphhopper.routing.util.EdgeFilter;
-import com.graphhopper.routing.util.FiniteWeightFilter;
+import com.graphhopper.routing.util.HeadingEdgeFilter;
 import com.graphhopper.routing.util.NameSimilarityEdgeFilter;
 import com.graphhopper.routing.util.SnapPreventionEdgeFilter;
 import com.graphhopper.routing.weighting.Weighting;
@@ -54,27 +54,36 @@ public class ViaRouting {
     /**
      * @throws MultiplePointsNotFoundException in case one or more points could not be resolved
      */
-    public static List<Snap> lookup(EncodedValueLookup lookup, List<GHPoint> points, EdgeFilter edgeFilter, LocationIndex locationIndex, List<String> snapPreventions, List<String> pointHints) {
+    public static List<Snap> lookup(EncodedValueLookup lookup, List<GHPoint> points, EdgeFilter snapFilter,
+                                    LocationIndex locationIndex, List<String> snapPreventions, List<String> pointHints,
+                                    EdgeFilter directedSnapFilter, List<Double> headings) {
         if (points.size() < 2)
             throw new IllegalArgumentException("At least 2 points have to be specified, but was:" + points.size());
 
         final EnumEncodedValue<RoadClass> roadClassEnc = lookup.getEnumEncodedValue(RoadClass.KEY, RoadClass.class);
         final EnumEncodedValue<RoadEnvironment> roadEnvEnc = lookup.getEnumEncodedValue(RoadEnvironment.KEY, RoadEnvironment.class);
         EdgeFilter strictEdgeFilter = snapPreventions.isEmpty()
-                ? edgeFilter
-                : new SnapPreventionEdgeFilter(edgeFilter, roadClassEnc, roadEnvEnc, snapPreventions);
+                ? snapFilter
+                : new SnapPreventionEdgeFilter(snapFilter, roadClassEnc, roadEnvEnc, snapPreventions);
         List<Snap> snaps = new ArrayList<>(points.size());
         IntArrayList pointsNotFound = new IntArrayList();
         for (int placeIndex = 0; placeIndex < points.size(); placeIndex++) {
             GHPoint point = points.get(placeIndex);
             Snap snap = null;
-            if (!pointHints.isEmpty())
+            if (placeIndex < headings.size() && !Double.isNaN(headings.get(placeIndex))) {
+                if (!pointHints.isEmpty() && !Helper.isEmpty(pointHints.get(placeIndex)))
+                    throw new IllegalArgumentException("Cannot specify heading and point_hint at the same time. " +
+                            "Make sure you specify either an empty point_hint (String) or a NaN heading (double) for point " + placeIndex);
+                snap = locationIndex.findClosest(point.lat, point.lon, new HeadingEdgeFilter(directedSnapFilter, headings.get(placeIndex), point));
+            } else if (!pointHints.isEmpty()) {
                 snap = locationIndex.findClosest(point.lat, point.lon, new NameSimilarityEdgeFilter(strictEdgeFilter,
                         pointHints.get(placeIndex), point, 100));
-            else if (!snapPreventions.isEmpty())
+            } else if (!snapPreventions.isEmpty()) {
                 snap = locationIndex.findClosest(point.lat, point.lon, strictEdgeFilter);
+            }
+
             if (snap == null || !snap.isValid())
-                snap = locationIndex.findClosest(point.lat, point.lon, edgeFilter);
+                snap = locationIndex.findClosest(point.lat, point.lon, snapFilter);
             if (!snap.isValid())
                 pointsNotFound.add(placeIndex);
 
diff --git a/core/src/main/java/com/graphhopper/routing/util/HeadingEdgeFilter.java b/core/src/main/java/com/graphhopper/routing/util/HeadingEdgeFilter.java
new file mode 100644
index 000000000..a6b986fe5
--- /dev/null
+++ b/core/src/main/java/com/graphhopper/routing/util/HeadingEdgeFilter.java
@@ -0,0 +1,68 @@
+package com.graphhopper.routing.util;
+
+import com.graphhopper.util.*;
+import com.graphhopper.util.shapes.GHPoint;
+
+public class HeadingEdgeFilter implements EdgeFilter {
+
+    private final double heading;
+    private final EdgeFilter directedEdgeFilter;
+    private final GHPoint pointNearHeading;
+
+    public HeadingEdgeFilter(EdgeFilter directedEdgeFilter, double heading, GHPoint pointNearHeading) {
+        this.directedEdgeFilter = directedEdgeFilter;
+        this.heading = heading;
+        this.pointNearHeading = pointNearHeading;
+    }
+
+    @Override
+    public boolean accept(EdgeIteratorState edgeState) {
+        final double tolerance = 30;
+        // we only accept edges that are not too far away. It might happen that only far away edges match the heading
+        // in which case we rather rely on the fallback snapping than return a match here.
+        final double maxDistance = 20;
+        double headingOfEdge = getHeadingOfGeometryNearPoint(edgeState, pointNearHeading, maxDistance);
+        if (Double.isNaN(headingOfEdge))
+            // this edge is too far away. we do not accept it.
+            return false;
+        // we accept the edge if either of the two directions roughly has the right heading
+        return Math.abs(headingOfEdge - heading) < tolerance && directedEdgeFilter.accept(edgeState) ||
+                Math.abs((headingOfEdge + 180) % 360 - heading) < tolerance && directedEdgeFilter.accept(edgeState.detach(true));
+    }
+
+    /**
+     * Calculates the heading (in degrees) of the given edge in fwd direction near the given point. If the point is
+     * too far away from the edge (according to the maxDistance parameter) it returns Double.NaN.
+     */
+    static double getHeadingOfGeometryNearPoint(EdgeIteratorState edgeState, GHPoint point, double maxDistance) {
+        final DistanceCalc calcDist = DistanceCalcEarth.DIST_EARTH;
+        double closestDistance = Double.POSITIVE_INFINITY;
+        PointList points = edgeState.fetchWayGeometry(FetchMode.ALL);
+        int closestPoint = -1;
+        for (int i = 1; i < points.size(); i++) {
+            double fromLat = points.getLat(i - 1), fromLon = points.getLon(i - 1);
+            double toLat = points.getLat(i), toLon = points.getLon(i);
+            // the 'distance' between the point and an edge segment is either the vertical distance to the segment or
+            // the distance to the closer one of the two endpoints. here we save one call to calcDist per segment,
+            // because each endpoint appears in two segments (except the first and last).
+            double distance = calcDist.validEdgeDistance(point.lat, point.lon, fromLat, fromLon, toLat, toLon)
+                    ? calcDist.calcDenormalizedDist(calcDist.calcNormalizedEdgeDistance(point.lat, point.lon, fromLat, fromLon, toLat, toLon))
+                    : calcDist.calcDist(fromLat, fromLon, point.lat, point.lon);
+            if (i == points.size() - 1)
+                distance = Math.min(distance, calcDist.calcDist(toLat, toLon, point.lat, point.lon));
+            if (distance > maxDistance)
+                continue;
+            if (distance < closestDistance) {
+                closestDistance = distance;
+                closestPoint = i;
+            }
+        }
+        if (closestPoint < 0)
+            return Double.NaN;
+
+        double fromLat = points.getLat(closestPoint - 1), fromLon = points.getLon(closestPoint - 1);
+        double toLat = points.getLat(closestPoint), toLon = points.getLon(closestPoint);
+        // calcOrientation returns value relative to East, but heading is relative to North
+        return (Math.toDegrees(AngleCalc.ANGLE_CALC.calcOrientation(fromLat, fromLon, toLat, toLon)) + 90) % 360;
+    }
+}


Test patch:
diff --git a/core/src/test/java/com/graphhopper/GraphHopperTest.java b/core/src/test/java/com/graphhopper/GraphHopperTest.java
index 6bcc47597..f68fe4cc2 100644
--- a/core/src/test/java/com/graphhopper/GraphHopperTest.java
+++ b/core/src/test/java/com/graphhopper/GraphHopperTest.java
@@ -230,7 +230,7 @@ public class GraphHopperTest {
         request.addPoint(new GHPoint(43.743887, 7.431151));
         request.addPoint(new GHPoint(43.744007, 7.431076));
         //Force initial U-Turn
-        request.setHeadings(Arrays.asList(200., Double.NaN));
+        request.setHeadings(Arrays.asList(200.));
 
         request.setAlgorithm(ASTAR).setProfile(profile);
         GHResponse rsp = hopper.route(request);
diff --git a/core/src/test/java/com/graphhopper/routing/HeadingRoutingTest.java b/core/src/test/java/com/graphhopper/routing/HeadingRoutingTest.java
index 37b037a10..23d911f10 100644
--- a/core/src/test/java/com/graphhopper/routing/HeadingRoutingTest.java
+++ b/core/src/test/java/com/graphhopper/routing/HeadingRoutingTest.java
@@ -155,7 +155,7 @@ class HeadingRoutingTest {
         GHPoint via = new GHPoint(0.000, 0.0015);
         GHRequest req = new GHRequest().
                 setPoints(Arrays.asList(start, via, end)).
-                setHeadings(Arrays.asList(0., 3.14 / 2, Double.NaN)).
+                setHeadings(Arrays.asList(0., 90., Double.NaN)).
                 setProfile("profile").
                 setPathDetails(Collections.singletonList("edge_key"));
         req.putHint(Parameters.Routing.PASS_THROUGH, true);
@@ -164,6 +164,96 @@ class HeadingRoutingTest {
         assertArrayEquals(new int[]{5, 4, 3, 8, 7, 6, 5, 4, 3, 2}, calcNodes(graph, response.getAll().get(0)));
     }
 
+    @Test
+    public void testHeadingWithSnapFilter() {
+        GraphHopperStorage graph = createSquareGraphWithTunnel();
+        Router router = createRouter(graph);
+        // Start at 8 (slightly north to make it independent on some edge ordering and always use 8-3 or 3-8 as fallback)
+        GHPoint start = new GHPoint(0.0011, 0.001);
+        // End at middle of edge 2-3
+        GHPoint end = new GHPoint(0.002, 0.0005);
+
+        // no heading
+        GHRequest req = new GHRequest().
+                setPoints(Arrays.asList(start, end)).
+                setProfile("profile").
+                setPathDetails(Collections.singletonList("edge_key"));
+        req.putHint(Parameters.Routing.PASS_THROUGH, true);
+        GHResponse response = router.route(req);
+        assertFalse(response.hasErrors());
+        assertArrayEquals(new int[]{8, 3, 2}, calcNodes(graph, response.getAll().get(0)));
+
+        // same start + end but heading=0, parallel to 3-8-7
+        req = new GHRequest().
+                setPoints(Arrays.asList(start, end)).
+                setHeadings(Arrays.asList(0.)).
+                setProfile("profile").
+                setPathDetails(Collections.singletonList("edge_key"));
+        req.putHint(Parameters.Routing.PASS_THROUGH, true);
+        response = router.route(req);
+        assertFalse(response.hasErrors());
+        assertArrayEquals(new int[]{8, 3, 2}, calcNodes(graph, response.getAll().get(0)));
+
+        // heading=90 parallel to 1->5
+        req = new GHRequest().
+                setPoints(Arrays.asList(start, end)).
+                setHeadings(Arrays.asList(90., Double.NaN)).
+                setProfile("profile").
+                setPathDetails(Collections.singletonList("edge_key"));
+        req.putHint(Parameters.Routing.PASS_THROUGH, true);
+        response = router.route(req);
+        assertFalse(response.hasErrors());
+        assertArrayEquals(new int[]{1, 5, 4, 3, 2}, calcNodes(graph, response.getAll().get(0)));
+
+        for (double angle = 0; angle < 360; angle += 10) {
+            // Ignore angles nearly parallel to 1->5. I.e. it should fallback to results with 8-3.. or 3-8..
+            if (angle >= 60 && angle <= 120) continue;
+
+            req = new GHRequest().
+                    setPoints(Arrays.asList(start, end)).
+                    setHeadings(Arrays.asList(angle, Double.NaN)).
+                    setProfile("profile").
+                    setPathDetails(Collections.singletonList("edge_key"));
+            req.putHint(Parameters.Routing.PASS_THROUGH, true);
+            response = router.route(req);
+            assertFalse(response.hasErrors());
+
+            int[] expectedNodes = (angle >= 130 && angle <= 250) ? new int[]{3, 8, 7, 0, 1, 2, 3} : new int[]{8, 3, 2};
+            // System.out.println(Arrays.toString(calcNodes(graph, response.getAll().get(0))) + " angle:" + angle);
+            assertArrayEquals(expectedNodes, calcNodes(graph, response.getAll().get(0)), "angle: " + angle);
+        }
+    }
+
+    @Test
+    public void testHeadingWithSnapFilter2() {
+        GraphHopperStorage graph = createSquareGraphWithTunnel();
+        Router router = createRouter(graph);
+        // Start at 8 (slightly east to snap to edge 1->5 per default)
+        GHPoint start = new GHPoint(0.001, 0.0011);
+        // End at middle of edge 2-3
+        GHPoint end = new GHPoint(0.002, 0.0005);
+
+        GHRequest req = new GHRequest().
+                setPoints(Arrays.asList(start, end)).
+                setProfile("profile").
+                setHeadings(Arrays.asList(0.)).
+                setPathDetails(Collections.singletonList("edge_key"));
+        req.putHint(Parameters.Routing.PASS_THROUGH, true);
+        GHResponse response = router.route(req);
+        assertFalse(response.hasErrors());
+        assertArrayEquals(new int[]{8, 3, 2}, calcNodes(graph, response.getAll().get(0)));
+
+        req = new GHRequest().
+                setPoints(Arrays.asList(start, end)).
+                setProfile("profile").
+                setHeadings(Arrays.asList(180.)).
+                setPathDetails(Collections.singletonList("edge_key"));
+        req.putHint(Parameters.Routing.PASS_THROUGH, true);
+        response = router.route(req);
+        assertFalse(response.hasErrors());
+        assertArrayEquals(new int[]{8, 3, 2}, calcNodes(graph, response.getAll().get(0)));
+    }
+
     @Test
     public void headingTest6() {
         // Test if snaps at tower nodes are ignored
@@ -199,11 +289,11 @@ class HeadingRoutingTest {
         EncodingManager encodingManager = new EncodingManager.Builder().add(carEncoder).add(Subnetwork.create("profile")).build();
         GraphHopperStorage g = new GraphBuilder(encodingManager).create();
 
-        //   2---3---4
-        //  /    |    \
-        //  1----8----5
-        //  /    |    /
-        //  0----7---6
+        // 2---3---4
+        // |   |   |
+        // 1---8---5
+        // |   |   |
+        // 0---7---6
         NodeAccess na = g.getNodeAccess();
         na.setNode(0, 0.000, 0.000);
         na.setNode(1, 0.001, 0.000);
@@ -232,6 +322,43 @@ class HeadingRoutingTest {
         return g;
     }
 
+    private GraphHopperStorage createSquareGraphWithTunnel() {
+        CarFlagEncoder carEncoder = new CarFlagEncoder();
+        EncodingManager encodingManager = new EncodingManager.Builder().add(carEncoder).add(Subnetwork.create("profile")).build();
+        GraphHopperStorage g = new GraphBuilder(encodingManager).create();
+
+        // 2----3---4
+        // |    |   |
+        // 1->- 8 >-5 (edge 1->5 is not connected to 8)
+        // |    |   |
+        // 0----7---6
+        NodeAccess na = g.getNodeAccess();
+        na.setNode(0, 0.000, 0.000);
+        na.setNode(1, 0.001, 0.000);
+        na.setNode(2, 0.002, 0.000);
+        na.setNode(3, 0.002, 0.001);
+        na.setNode(4, 0.002, 0.002);
+        na.setNode(5, 0.001, 0.002);
+        na.setNode(6, 0.000, 0.002);
+        na.setNode(7, 0.000, 0.001);
+        na.setNode(8, 0.001, 0.001);
+
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(0, 1).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(1, 2).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(2, 3).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(3, 4).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(4, 5).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(5, 6).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(6, 7).setDistance(100));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(7, 0).setDistance(100));
+
+        GHUtility.setSpeed(60, true, false, carEncoder, g.edge(1, 5).setDistance(110));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(3, 8).setDistance(110));
+        GHUtility.setSpeed(60, true, true, carEncoder, g.edge(7, 8).setDistance(110));
+
+        return g;
+    }
+
     private int[] calcNodes(Graph graph, ResponsePath responsePath) {
         List<PathDetail> edgeKeys = responsePath.getPathDetails().get("edge_key");
         int[] result = new int[edgeKeys.size() + 1];