Astar turn restriction eager blocking fix

application/src/main/java/org/opentripplanner/astar/model/ShortestPathTree.java

@@ -144,6 +144,14 @@ public boolean add(State newState) {
       State oldState = it.next();
       // order is important, because in the case of a tie
       // we want to reject the new state
+      // [ pass RequestVertex to dominance function (same for both states) ]
+      // The RequestVertex thing doesn't really work for turn restrictions which are
+      // not pointlike, which we want to support in the near future. Instead we should
+      // keep track of the pending (have passed the start of, have yet to pass the end of)
+      // turn restrictions, and compare the two sets of turn restrictions in the dominance
+      // functions, and never report dominance unless they are equal. This even keeps the
+      // change out of AStar, ShortestPathTree, and Edge, limiting it to State, StreetEdge,
+      // and DominanceFunctions
       if (dominanceFunction.betterOrEqualAndComparable(oldState, newState)) {
         return false;
       }

application/src/main/java/org/opentripplanner/street/search/state/State.java

@@ -5,6 +5,7 @@
 import java.time.Instant;
 import java.util.ArrayList;
 import java.util.Collection;
+import java.util.HashSet;
 import java.util.Objects;
 import java.util.Set;
 import java.util.stream.Stream;
@@ -15,6 +16,7 @@
 import org.opentripplanner.service.vehiclerental.street.VehicleRentalEdge;
 import org.opentripplanner.service.vehiclerental.street.VehicleRentalPlaceVertex;
 import org.opentripplanner.street.model.RentalFormFactor;
+import org.opentripplanner.street.model.TurnRestriction;
 import org.opentripplanner.street.model.edge.Edge;
 import org.opentripplanner.street.model.vertex.Vertex;
 import org.opentripplanner.street.search.TraverseMode;
@@ -43,6 +45,9 @@ public class State implements AStarState<State, Edge, Vertex>, Cloneable {
 
   public Edge backEdge;
 
+  public Set<TurnRestriction> pendingTurnRestrictions;
+  public Set<Edge> unusedOutgoingEdges;
+
   /* StateData contains data which is unlikely to change as often */
   public StateData stateData;
 

application/src/main/java/org/opentripplanner/street/search/state/StateEditor.java

@@ -1,9 +1,12 @@
 package org.opentripplanner.street.search.state;
 
+import java.util.HashSet;
 import java.util.Set;
 import javax.annotation.Nullable;
 import org.opentripplanner.street.model.RentalFormFactor;
+import org.opentripplanner.street.model.TurnRestriction;
 import org.opentripplanner.street.model.edge.Edge;
+import org.opentripplanner.street.model.edge.StreetEdge;
 import org.opentripplanner.street.model.vertex.Vertex;
 import org.opentripplanner.street.search.TraverseMode;
 import org.opentripplanner.street.search.request.StreetSearchRequest;
@@ -109,6 +112,27 @@ public State makeState() {
       return null;
     }
 
+    // record pending turn restrictions, which gets more complicated when we start
+    // supporting non-pointlike turn restrictions
+    // the isReverseOf check in StreetEdge.doTraverse _always_ prohibits u-turns, so
+    // there is an implicit u-turn restriction always in place!
+    if (
+      child.getBackMode() != null &&
+      child.getBackMode().isInCar() &&
+      child.backEdge instanceof StreetEdge streetEdge
+    ) {
+      child.pendingTurnRestrictions = new HashSet<>();
+      child.unusedOutgoingEdges = new HashSet<>();
+      for (TurnRestriction turnRestriction : streetEdge.getTurnRestrictions()) {
+        if (turnRestriction.from == streetEdge) {
+          child.pendingTurnRestrictions.add(turnRestriction);
+          child.unusedOutgoingEdges.add(turnRestriction.to);
+        }
+      }
+      child.unusedOutgoingEdges.add(streetEdge);
+      child.pendingTurnRestrictions.addAll(streetEdge.getTurnRestrictions());
+    }
+
     if (child.backState != null) {
       // make it impossible to use a state with lower weight than its
       // parent.

application/src/main/java/org/opentripplanner/street/search/strategy/DominanceFunctions.java

@@ -1,8 +1,11 @@
 package org.opentripplanner.street.search.strategy;
 
 import java.io.Serializable;
+import java.util.HashSet;
 import java.util.Objects;
+import java.util.Set;
 import org.opentripplanner.astar.spi.DominanceFunction;
+import org.opentripplanner.street.model.edge.Edge;
 import org.opentripplanner.street.model.edge.StreetEdge;
 import org.opentripplanner.street.search.state.State;
 
@@ -23,6 +26,43 @@
  */
 public abstract class DominanceFunctions implements Serializable, DominanceFunction<State> {
 
+  // Separate this part as its own method to make the various early exits feasible without gotos
+  // or other convoluted flow control. Avoid allocating new memory in the form of a new HashSet
+  // if at all possible. Returns true if the states were non-commensurable, i.e. the sets of
+  // unused outgoing edges were different. Afterwards the outgoing edge sets will be the same,
+  // not only same for equals() but also for object identity. smaller.unusedOutgoingEdges will
+  // be reused if it is correct (always for the empty set, if it is a subset of the larger set
+  // otherwise).
+  private boolean unifyUnusedOutgoingStates(State larger, State smaller, boolean sizesDifferent) {
+    if (smaller.unusedOutgoingEdges.isEmpty()) {
+      if (!sizesDifferent) {
+        return false;
+      }
+      larger.unusedOutgoingEdges = smaller.unusedOutgoingEdges;
+      return true;
+    }
+    boolean setsDifferent = false;
+    for (Edge edge : smaller.unusedOutgoingEdges) {
+      if (!larger.unusedOutgoingEdges.contains(edge)) {
+        setsDifferent = true;
+        break;
+      }
+    }
+    if (!setsDifferent) {
+      larger.unusedOutgoingEdges = smaller.unusedOutgoingEdges;
+      return sizesDifferent;
+    }
+    HashSet<Edge> unusedOutgoingEdges = new HashSet<>();
+    for (Edge edge : larger.unusedOutgoingEdges) {
+      if (smaller.unusedOutgoingEdges.contains(edge)) {
+        unusedOutgoingEdges.add(edge);
+      }
+    }
+    larger.unusedOutgoingEdges = unusedOutgoingEdges;
+    smaller.unusedOutgoingEdges = unusedOutgoingEdges;
+    return true;
+  }
+
   /**
    * For bike rental, parking, and approaching turn-restricted intersections states are
    * incomparable: they exist on separate planes. The core state dominance logic is wrapped in this
@@ -89,14 +129,57 @@ public boolean betterOrEqualAndComparable(State a, State b) {
      * including the loops should still result in a route. Often this will be preferable to
      * taking a detour due to turn restrictions anyway.
      */
+    // The backEdge difference test only applies when all turn restrictions are pointlike.
+    // This really should work without it, but it does yield a small performance benefit.
+    // When we start supporting non-pointlike turn restrictions, we'll have to return to
+    // this implementation and actually look at State.pendingTurnRestrictions as well.
     if (
       a.backEdge != b.getBackEdge() &&
       (a.backEdge instanceof StreetEdge) &&
       a.getBackMode() != null &&
-      a.getBackMode().isInCar() &&
-      a.getRequest().isCloseToStartOrEnd(a.getVertex())
+      a.getBackMode().isInCar()
     ) {
-      return false;
+      // Fortunately the number of edges from a vertex is usually very low. We can reimplement
+      // this as a bunch of integer bit operations. This will require passing through the number
+      // of the edge in the current context through a bunch of method calls:
+      //  1. AStar.iterate
+      //  2. Edge.traverse(State) -> (int, State)
+      //  2a. StreetEdge.traverse(State) -> (int, State)
+      //  2b. other edge types can probably ignore this change
+      //  3. StreetEdge.doTraverse(State, TraverseMode, boolean)
+      //      -> (int, State, TraverseMode, boolean)
+      //  4. BikeWalkableEdge.createEditor(State, Edge, TraverseMode, boolean)
+      //      -> (State, Edge, int, TraverseMode, boolean)
+      //  5. State.edit(Edge) -> (Edge, int)
+      //  6. new StateEditor(State, Edge) -> (State, Edge, int)
+
+      // This entire thing, including the helper method unifyUnusedOutgoingStates, will reduce,
+      // using integers, to:
+      //   boolean nonCommensurable = a.unusedOutgoingEdgesInt ^ b.unusedOutgoingEdgesInt != 0;
+      //   int unusedOutgoingEdgesInt = a.unusedOutgoingEdgesInt & b.unusedOutgoingEdgesInt;
+      //   a.unusedOutgoingEdgesInt = unusedOutgoingEdgesInt;
+      //   b.unusedOutgoingEdgesInt = unusedOutgoingEdgesInt;
+      //   if (nonCommensurable) {
+      //     return false;
+      //   }
+
+      // The unification might have made the unusedOutgoingEdges sets of two different States
+      // the same, test for that immediately, because that's a cheap early exit.
+      if (a.unusedOutgoingEdges != b.unusedOutgoingEdges) {
+        int aSize = a.unusedOutgoingEdges.size();
+        int bSize = b.unusedOutgoingEdges.size();
+        boolean nonCommensurable;
+        if (aSize > bSize) {
+          nonCommensurable = unifyUnusedOutgoingStates(a, b, true);
+        } else if (aSize < bSize) {
+          nonCommensurable = unifyUnusedOutgoingStates(b, a, true);
+        } else {
+          nonCommensurable = unifyUnusedOutgoingStates(a, b, false);
+        }
+        if (nonCommensurable) {
+          return false;
+        }
+      }
     }
 
     // These two states are comparable (they are on the same "plane" or "copy" of the graph).

application/src/test/java/org/opentripplanner/street/integration/CarRoutingTest.java

@@ -5,23 +5,40 @@
 import static org.opentripplanner.test.support.PolylineAssert.assertThatPolylinesAreEqual;
 
 import java.time.Instant;
+import java.util.List;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.DisplayName;
 import org.junit.jupiter.api.Test;
 import org.locationtech.jts.geom.Geometry;
 import org.opentripplanner.ConstantsForTests;
 import org.opentripplanner.TestOtpModel;
 import org.opentripplanner._support.time.ZoneIds;
+import org.opentripplanner.astar.model.GraphPath;
+import org.opentripplanner.astar.model.ShortestPathTree;
 import org.opentripplanner.framework.geometry.EncodedPolyline;
 import org.opentripplanner.model.GenericLocation;
 import org.opentripplanner.model.plan.StreetLeg;
 import org.opentripplanner.routing.algorithm.mapping.GraphPathToItineraryMapper;
 import org.opentripplanner.routing.api.request.RouteRequest;
 import org.opentripplanner.routing.api.request.StreetMode;
+import org.opentripplanner.routing.api.request.request.StreetRequest;
 import org.opentripplanner.routing.graph.Graph;
 import org.opentripplanner.routing.impl.GraphPathFinder;
+import org.opentripplanner.street.model.StreetTraversalPermission;
+import org.opentripplanner.street.model.TurnRestriction;
+import org.opentripplanner.street.model.TurnRestrictionType;
+import org.opentripplanner.street.model._data.StreetModelForTest;
+import org.opentripplanner.street.model.edge.Edge;
+import org.opentripplanner.street.model.edge.StreetEdge;
+import org.opentripplanner.street.model.edge.StreetEdgeBuilder;
+import org.opentripplanner.street.model.vertex.StreetVertex;
+import org.opentripplanner.street.model.vertex.Vertex;
+import org.opentripplanner.street.search.StreetSearchBuilder;
 import org.opentripplanner.street.search.TemporaryVerticesContainer;
 import org.opentripplanner.street.search.TraverseMode;
+import org.opentripplanner.street.search.TraverseModeSet;
+import org.opentripplanner.street.search.state.State;
+import org.opentripplanner.street.search.strategy.EuclideanRemainingWeightHeuristic;
 import org.opentripplanner.test.support.ResourceLoader;
 
 public class CarRoutingTest {
@@ -165,4 +182,82 @@ private static String computePolyline(Graph graph, GenericLocation from, Generic
     Geometry legGeometry = itineraries.get(0).legs().get(0).getLegGeometry();
     return EncodedPolyline.encode(legGeometry).points();
   }
+
+  private StreetVertex vertex(Graph graph, String label, double lat, double lon) {
+    var v = StreetModelForTest.intersectionVertex(label, lat, lon);
+    graph.addVertex(v);
+    return v;
+  }
+
+  private StreetEdge streetEdge(StreetVertex a, StreetVertex b, double length) {
+    return new StreetEdgeBuilder<>()
+      .withFromVertex(a)
+      .withToVertex(b)
+      .withMeterLength(length)
+      .withPermission(StreetTraversalPermission.ALL)
+      .buildAndConnect();
+  }
+
+  private StreetEdge[] edges(StreetVertex a, StreetVertex b, double length) {
+    return new StreetEdge[] { streetEdge(a, b, length), streetEdge(b, a, length) };
+  }
+
+  @Test
+  public void turnRestrictedVisitDoesNotBlockSearch() {
+    // The costs of the edges are set up so that the search first goes A->B->D before trying
+    // A->B->C->D. The test tests that the previous visit of D does not block the proper
+    // path A->B->C->D->F.
+    //      E
+    //      |
+    //  F - D -\
+    //      |   C
+    //      B -/
+    //      |
+    //      A
+    // B-D-F is forbidden by a turn restriction
+    var graph = new Graph();
+    var A = vertex(graph, "A", 0.0, 0.0);
+    var B = vertex(graph, "B", 1.0, 0.0);
+    var C = vertex(graph, "C", 1.5, 1.0);
+    var D = vertex(graph, "D", 2.0, 0.0);
+    var E = vertex(graph, "E", 3.0, 0.0);
+    var F = vertex(graph, "F", 2.0, -1.0);
+    edges(A, B, 1.0);
+    edges(B, C, 1.0);
+    var BD = edges(B, D, 1.0);
+    edges(C, D, 1.0);
+    edges(D, E, 1.0);
+    var DF = edges(D, F, 1.0);
+    BD[0].addTurnRestriction(
+        new TurnRestriction(
+          BD[0],
+          DF[0],
+          TurnRestrictionType.NO_TURN,
+          new TraverseModeSet(TraverseMode.CAR),
+          null
+        )
+      );
+
+    var request = new RouteRequest();
+    request.setDateTime(dateTime);
+    request.journey().direct().setMode(StreetMode.CAR);
+
+    var streetRequest = new StreetRequest(StreetMode.CAR);
+
+    ShortestPathTree<State, Edge, Vertex> spt = StreetSearchBuilder.of()
+      .setHeuristic(new EuclideanRemainingWeightHeuristic())
+      .setRequest(request)
+      .setStreetRequest(streetRequest)
+      .setFrom(A)
+      .setTo(F)
+      .getShortestPathTree();
+    GraphPath<State, Edge, Vertex> path = spt.getPath(F);
+    List<State> states = path.states;
+    assertEquals(5, states.size());
+    assertEquals(states.get(0).getVertex(), A);
+    assertEquals(states.get(1).getVertex(), B);
+    assertEquals(states.get(2).getVertex(), C);
+    assertEquals(states.get(3).getVertex(), D);
+    assertEquals(states.get(4).getVertex(), F);
+  }
 }