Skip to content

Commit

Permalink
Fix turn restriction handling at virtual via edges (graphhopper#3001)
Browse files Browse the repository at this point in the history
  • Loading branch information
@easbar
easbar authored May 5, 2024
1 parent 1341ded commit f6c7fbd
Show file tree
Hide file tree
Showing 5 changed files with 210 additions and 14 deletions.
2 changes: 1 addition & 1 deletion core/src/main/java/com/graphhopper/routing/querygraph/QueryGraph.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@ public TurnCostStorage getTurnCostStorage() {

@Override
public Weighting wrapWeighting(Weighting weighting) {
return new QueryGraphWeighting(weighting, baseGraph.getNodes(), baseGraph.getEdges(), queryOverlay.getClosestEdges());
return new QueryGraphWeighting(baseGraph, weighting, queryOverlay.getClosestEdges());
}

@Override
Expand Down
3 changes: 1 addition & 2 deletions core/src/main/java/com/graphhopper/routing/util/parsers/RestrictionSetter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,8 @@
import com.graphhopper.util.EdgeExplorer;
import com.graphhopper.util.EdgeIterator;
import com.graphhopper.util.EdgeIteratorState;
import com.graphhopper.util.FetchMode;

import java.util.List;
import java.util.*;

import static com.graphhopper.reader.osm.RestrictionType.NO;
import static com.graphhopper.reader.osm.RestrictionType.ONLY;
Expand Down
43 changes: 34 additions & 9 deletions core/src/main/java/com/graphhopper/routing/weighting/QueryGraphWeighting.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,24 +20,29 @@

import com.carrotsearch.hppc.IntArrayList;
import com.graphhopper.routing.querygraph.QueryGraph;
import com.graphhopper.storage.BaseGraph;
import com.graphhopper.util.EdgeIterator;
import com.graphhopper.util.EdgeIteratorState;

import java.util.concurrent.atomic.AtomicReference;

/**
* Whenever a {@link QueryGraph} is used for shortest path calculations including turn costs we need to wrap the
* {@link Weighting} we want to use with this class. Otherwise turn costs at virtual nodes and/or including virtual
* edges will not be calculated correctly.
*/
public class QueryGraphWeighting implements Weighting {
private final BaseGraph graph;
private final Weighting weighting;
private final int firstVirtualNodeId;
private final int firstVirtualEdgeId;
private final IntArrayList closestEdges;

public QueryGraphWeighting(Weighting weighting, int firstVirtualNodeId, int firstVirtualEdgeId, IntArrayList closestEdges) {
public QueryGraphWeighting(BaseGraph graph, Weighting weighting, IntArrayList closestEdges) {
this.graph = graph;
this.weighting = weighting;
this.firstVirtualNodeId = firstVirtualNodeId;
this.firstVirtualEdgeId = firstVirtualEdgeId;
this.firstVirtualNodeId = graph.getNodes();
this.firstVirtualEdgeId = graph.getEdges();
this.closestEdges = closestEdges;
}

Expand Down Expand Up @@ -69,13 +74,33 @@ public double calcTurnWeight(int inEdge, int viaNode, int outEdge) {
}
// to calculate the actual turn costs or detect u-turns we need to look at the original edge of each virtual
// edge, see #1593
if (isVirtualEdge(inEdge)) {
inEdge = getOriginalEdge(inEdge);
}
if (isVirtualEdge(outEdge)) {
outEdge = getOriginalEdge(outEdge);
if (isVirtualEdge(inEdge) && isVirtualEdge(outEdge)) {
EdgeIteratorState inEdgeState = graph.getEdgeIteratorState(getOriginalEdge(inEdge), Integer.MIN_VALUE);
EdgeIteratorState outEdgeState = graph.getEdgeIteratorState(getOriginalEdge(outEdge), Integer.MIN_VALUE);
AtomicReference<Double> minTurnWeight = new AtomicReference<>(Double.POSITIVE_INFINITY);
graph.forEdgeAndCopiesOfEdge(graph.createEdgeExplorer(), inEdgeState, p -> {
graph.forEdgeAndCopiesOfEdge(graph.createEdgeExplorer(), outEdgeState, q -> {
minTurnWeight.set(Math.min(minTurnWeight.get(), weighting.calcTurnWeight(p.getEdge(), viaNode, q.getEdge())));
});
});
return minTurnWeight.get();
} else if (isVirtualEdge(inEdge)) {
EdgeIteratorState inEdgeState = graph.getEdgeIteratorState(getOriginalEdge(inEdge), Integer.MIN_VALUE);
AtomicReference<Double> minTurnWeight = new AtomicReference<>(Double.POSITIVE_INFINITY);
graph.forEdgeAndCopiesOfEdge(graph.createEdgeExplorer(), inEdgeState, p -> {
minTurnWeight.set(Math.min(minTurnWeight.get(), weighting.calcTurnWeight(p.getEdge(), viaNode, outEdge)));
});
return minTurnWeight.get();
} else if (isVirtualEdge(outEdge)) {
EdgeIteratorState outEdgeState = graph.getEdgeIteratorState(getOriginalEdge(outEdge), Integer.MIN_VALUE);
AtomicReference<Double> minTurnWeight = new AtomicReference<>(Double.POSITIVE_INFINITY);
graph.forEdgeAndCopiesOfEdge(graph.createEdgeExplorer(), outEdgeState, p -> {
minTurnWeight.set(Math.min(minTurnWeight.get(), weighting.calcTurnWeight(inEdge, viaNode, p.getEdge())));
});
return minTurnWeight.get();
} else {
return weighting.calcTurnWeight(inEdge, viaNode, outEdge);
}
return weighting.calcTurnWeight(inEdge, viaNode, outEdge);
}

private boolean isUTurn(int inEdge, int outEdge) {
Expand Down
18 changes: 18 additions & 0 deletions core/src/test/java/com/graphhopper/GraphHopperTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2592,6 +2592,24 @@ public void testBarriers() {
}
}

@Test
public void turnRestrictionWithSnapToViaEdge_issue2996() {
final String profile = "profile";
GraphHopper hopper = new GraphHopper().
setGraphHopperLocation(GH_LOCATION).
setOSMFile("../map-matching/files/leipzig_germany.osm.pbf").
setEncodedValuesString("car_access, car_average_speed").
setProfiles(TestProfiles.accessAndSpeed(profile, "car").setTurnCostsConfig(TurnCostsConfig.car())).
setMinNetworkSize(200);
hopper.importOrLoad();
// doing a simple left-turn is allowed
GHResponse res = hopper.route(new GHRequest(51.34665, 12.391847, 51.346254, 12.39256).setProfile(profile));
assertEquals(81, res.getBest().getDistance(), 1);
// if we stop right after the left-turn on the via-edge the turn should still be allowed of course (there should be no detour that avoids the turn)
res = hopper.route(new GHRequest(51.34665, 12.391847, 51.346306, 12.392091).setProfile(profile));
assertEquals(48, res.getBest().getDistance(), 1);
}

@Test
public void germanyCountryRuleAvoidsTracks() {
final String profile = "profile";
Expand Down
158 changes: 156 additions & 2 deletions core/src/test/java/com/graphhopper/routing/util/parsers/RestrictionSetterTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,23 @@
import com.graphhopper.reader.osm.RestrictionType;
import com.graphhopper.routing.Dijkstra;
import com.graphhopper.routing.ev.*;
import com.graphhopper.routing.querygraph.QueryGraph;
import com.graphhopper.routing.util.EdgeFilter;
import com.graphhopper.routing.util.EncodingManager;
import com.graphhopper.routing.util.TraversalMode;
import com.graphhopper.routing.weighting.SpeedWeighting;
import com.graphhopper.routing.weighting.TurnCostProvider;
import com.graphhopper.storage.BaseGraph;
import com.graphhopper.storage.Graph;
import com.graphhopper.storage.NodeAccess;
import com.graphhopper.storage.index.LocationIndex;
import com.graphhopper.storage.index.LocationIndexTree;
import com.graphhopper.storage.index.Snap;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;

import java.util.Arrays;
import java.util.List;

import static org.junit.jupiter.api.Assertions.*;

Expand Down Expand Up @@ -330,14 +338,160 @@ void viaWay_only_twoRestrictionsSharingSameVia_different_directions() {
assertEquals(nodes(5, 1, 2, 4), calcPath(5, 4, turnRestrictionEnc));
}

@Test
void viaWayAndNode() {
// 4-0-1-2
// |
// 3
int e0_1 = edge(0, 1);
int e0_4 = edge(0, 4);
int e1_2 = edge(1, 2);
int e1_3 = edge(1, 3);
BooleanEncodedValue turnRestrictionEnc = createTurnRestrictionEnc("car");
assertEquals(nodes(0, 1, 3), calcPath(0, 3, turnRestrictionEnc));
assertEquals(nodes(4, 0, 1, 2), calcPath(4, 2, turnRestrictionEnc));
r.setRestrictions(List.of(
new Pair<>(GraphRestriction.way(e0_4, e0_1, e1_2, nodes(0, 1)), RestrictionType.NO),
new Pair<>(GraphRestriction.node(e0_1, 1, e1_3), RestrictionType.NO)
), turnRestrictionEnc);
assertEquals(NO_PATH, calcPath(4, 2, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(0, 3, turnRestrictionEnc));
}

@Test
void snapToViaWay() {
// 6 0
// | |
// 1-2-x-3-4
// | |
// 5 7
int e1_2 = edge(1, 2);
int e2_3 = edge(2, 3);
int e3_4 = edge(3, 4);
int e0_3 = edge(0, 3);
int e2_5 = edge(2, 5);
int e2_6 = edge(2, 6);
int e3_7 = edge(3, 7);
NodeAccess na = graph.getNodeAccess();
na.setNode(0, 40.03, 5.03);
na.setNode(1, 40.02, 5.01);
na.setNode(2, 40.02, 5.02);
na.setNode(3, 40.02, 5.03);
na.setNode(4, 40.02, 5.04);
na.setNode(5, 40.01, 5.02);
na.setNode(6, 40.03, 5.02);
na.setNode(7, 40.01, 5.03);
BooleanEncodedValue turnRestrictionEnc = createTurnRestrictionEnc("car");

assertEquals(nodes(1, 2, 3, 0), calcPath(1, 0, turnRestrictionEnc));
assertEquals(nodes(1, 2, 3, 4), calcPath(1, 4, turnRestrictionEnc));
assertEquals(nodes(5, 2, 3, 0), calcPath(5, 0, turnRestrictionEnc));
assertEquals(nodes(6, 2, 3), calcPath(6, 3, turnRestrictionEnc));
assertEquals(nodes(2, 3, 7), calcPath(2, 7, turnRestrictionEnc));
r.setRestrictions(List.of(
new Pair<>(GraphRestriction.way(e1_2, e2_3, e0_3, nodes(2, 3)), RestrictionType.NO),
new Pair<>(GraphRestriction.node(e2_6, 2, e2_3), RestrictionType.NO),
new Pair<>(GraphRestriction.node(e2_3, 3, e3_7), RestrictionType.NO)
), turnRestrictionEnc);
assertEquals(NO_PATH, calcPath(1, 0, turnRestrictionEnc));
assertEquals(nodes(1, 2, 3, 4), calcPath(1, 4, turnRestrictionEnc));
assertEquals(nodes(5, 2, 3, 0), calcPath(5, 0, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(6, 3, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(2, 7, turnRestrictionEnc));

// Now we try to route to and from a virtual node x. The problem here is that the 1-2-3-0
// restriction forces paths coming from 1 onto an artificial edge (2-3)' (denying turns onto
// 2-3 coming from 1), so if we just snapped to the original edge 2-3 we wouldn't find a path!
// But if we snapped to the artificial edge we wouldn't find a path if we came from node 5.
// If x was our starting point we wouldn't be able to go to 0 either.
LocationIndex locationIndex = new LocationIndexTree(graph, graph.getDirectory()).prepareIndex();
Snap snap = locationIndex.findClosest(40.02, 5.025, EdgeFilter.ALL_EDGES);
QueryGraph queryGraph = QueryGraph.create(graph, snap);
final int x = 8;
// due to the virtual node the 1-2-3-0 path is now possible
assertEquals(nodes(1, 2, x, 3, 0), calcPath(queryGraph, 1, 0, turnRestrictionEnc));
assertEquals(nodes(1, 2, 3, 4), calcPath(queryGraph, 1, 4, turnRestrictionEnc));
assertEquals(nodes(1, 2, x), calcPath(queryGraph, 1, x, turnRestrictionEnc));
assertEquals(nodes(5, 2, x), calcPath(queryGraph, 5, x, turnRestrictionEnc));
assertEquals(nodes(x, 3, 0), calcPath(queryGraph, x, 0, turnRestrictionEnc));
assertEquals(nodes(x, 3, 4), calcPath(queryGraph, x, 4, turnRestrictionEnc));
// the 6-2-3 and 2-3-7 restrictions are still enforced, despite the virtual node
assertEquals(NO_PATH, calcPath(queryGraph, 6, 3, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(queryGraph, 2, 7, turnRestrictionEnc));
}

@Test
void snapToViaWay_twoVirtualNodes() {
// 1-2-x-3-y-4-z-5-6
int e1_2 = edge(1, 2);
int e2_3 = edge(2, 3);
int e3_4 = edge(3, 4);
int e4_5 = edge(4, 5);
int e5_6 = edge(5, 6);
NodeAccess na = graph.getNodeAccess();
na.setNode(1, 40.02, 5.01);
na.setNode(2, 40.02, 5.02);
na.setNode(3, 40.02, 5.03);
na.setNode(4, 40.02, 5.04);
na.setNode(5, 40.02, 5.05);
na.setNode(6, 40.02, 5.06);
BooleanEncodedValue turnRestrictionEnc = createTurnRestrictionEnc("car");

assertEquals(nodes(1, 2, 3, 4), calcPath(1, 4, turnRestrictionEnc));
assertEquals(nodes(2, 3, 4), calcPath(2, 4, turnRestrictionEnc));
assertEquals(nodes(2, 3, 4, 5), calcPath(2, 5, turnRestrictionEnc));
assertEquals(nodes(3, 4, 5), calcPath(3, 5, turnRestrictionEnc));
assertEquals(nodes(3, 4, 5, 6), calcPath(3, 6, turnRestrictionEnc));
assertEquals(nodes(4, 5, 6), calcPath(4, 6, turnRestrictionEnc));
r.setRestrictions(List.of(
new Pair<>(GraphRestriction.way(e1_2, e2_3, e3_4, nodes(2, 3)), RestrictionType.NO),
new Pair<>(GraphRestriction.way(e2_3, e3_4, e4_5, nodes(3, 4)), RestrictionType.NO),
new Pair<>(GraphRestriction.way(e3_4, e4_5, e5_6, nodes(4, 5)), RestrictionType.NO)
), turnRestrictionEnc);
assertEquals(NO_PATH, calcPath(1, 4, turnRestrictionEnc));
assertEquals(nodes(2, 3, 4), calcPath(2, 4, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(2, 5, turnRestrictionEnc));
assertEquals(nodes(3, 4, 5), calcPath(3, 5, turnRestrictionEnc));
assertEquals(NO_PATH, calcPath(3, 6, turnRestrictionEnc));
assertEquals(nodes(4, 5, 6), calcPath(4, 6, turnRestrictionEnc));

// three virtual notes
LocationIndex locationIndex = new LocationIndexTree(graph, graph.getDirectory()).prepareIndex();
Snap snapX = locationIndex.findClosest(40.02, 5.025, EdgeFilter.ALL_EDGES);
Snap snapY = locationIndex.findClosest(40.02, 5.035, EdgeFilter.ALL_EDGES);
Snap snapZ = locationIndex.findClosest(40.02, 5.045, EdgeFilter.ALL_EDGES);
QueryGraph queryGraph = QueryGraph.create(graph, List.of(snapX, snapY, snapZ));
final int x = 8;
final int y = 7;
final int z = 9;
assertEquals(x, snapX.getClosestNode());
assertEquals(y, snapY.getClosestNode());
assertEquals(z, snapZ.getClosestNode());
assertEquals(nodes(1, 2, x, 3, 4), calcPath(queryGraph, 1, 4, turnRestrictionEnc));
assertEquals(nodes(2, x, 3, 4), calcPath(queryGraph, 2, 4, turnRestrictionEnc));
assertEquals(nodes(2, x, 3, y, 4, 5), calcPath(queryGraph, 2, 5, turnRestrictionEnc));
assertEquals(nodes(3, y, 4, 5), calcPath(queryGraph, 3, 5, turnRestrictionEnc));
assertEquals(nodes(3, y, 4, z, 5, 6), calcPath(queryGraph, 3, 6, turnRestrictionEnc));
assertEquals(nodes(4, z, 5, 6), calcPath(queryGraph, 4, 6, turnRestrictionEnc));
// turning between the virtual nodes is still possible
assertEquals(nodes(x, 3, y), calcPath(queryGraph, x, y, turnRestrictionEnc));
assertEquals(nodes(y, 3, x), calcPath(queryGraph, y, x, turnRestrictionEnc));
assertEquals(nodes(y, 4, z), calcPath(queryGraph, y, z, turnRestrictionEnc));
assertEquals(nodes(z, 4, y), calcPath(queryGraph, z, y, turnRestrictionEnc));
}

private static BooleanEncodedValue createTurnRestrictionEnc(String name) {
BooleanEncodedValue turnRestrictionEnc = TurnRestriction.create(name);
turnRestrictionEnc.init(new EncodedValue.InitializerConfig());
return turnRestrictionEnc;
}

private IntArrayList calcPath(int from, int to, BooleanEncodedValue turnRestrictionEnc) {
return new IntArrayList(new Dijkstra(graph, new SpeedWeighting(speedEnc, new TurnCostProvider() {
return calcPath(this.graph, from, to, turnRestrictionEnc);
}

private IntArrayList calcPath(Graph graph, int from, int to, BooleanEncodedValue turnRestrictionEnc) {
return new IntArrayList(new Dijkstra(graph, graph.wrapWeighting(new SpeedWeighting(speedEnc, new TurnCostProvider() {
@Override
public double calcTurnWeight(int inEdge, int viaNode, int outEdge) {
if (inEdge == outEdge) return Double.POSITIVE_INFINITY;
Expand All @@ -348,7 +502,7 @@ public double calcTurnWeight(int inEdge, int viaNode, int outEdge) {
public long calcTurnMillis(int inEdge, int viaNode, int outEdge) {
return Double.isInfinite(calcTurnWeight(inEdge, viaNode, outEdge)) ? Long.MAX_VALUE : 0L;
}
}), TraversalMode.EDGE_BASED).calcPath(from, to).calcNodes());
})), TraversalMode.EDGE_BASED).calcPath(from, to).calcNodes());
}

private IntArrayList nodes(int... nodes) {
Expand Down

0 comments on commit f6c7fbd

Please sign in to comment.