fix for calculation of carpool trip duration when having more than 2 stops

Author: hakoncarlsen

application/src/ext/java/org/opentripplanner/ext/carpooling/routing/InsertionCandidate.java

@@ -22,14 +22,15 @@ public record InsertionCandidate(
   int pickupPosition,
   int dropoffPosition,
   List<GraphPath<State, Edge, Vertex>> routeSegments,
-  Duration baselineDuration,
+  Duration durationBetweenOriginAndDestination,
   Duration totalDuration
 ) {
   /**
-   * Calculates the additional duration caused by inserting this passenger.
+   * Calculates the difference between the total duration when inserting this passenger,
+   * and the duration when driving directly from the start to the end of the trip.
    */
   public Duration additionalDuration() {
-    return totalDuration.minus(baselineDuration);
+    return totalDuration.minus(durationBetweenOriginAndDestination);
   }
 
   /**

application/src/ext/java/org/opentripplanner/ext/carpooling/routing/InsertionEvaluator.java

@@ -1,6 +1,7 @@
 package org.opentripplanner.ext.carpooling.routing;
 
 import static org.opentripplanner.ext.carpooling.util.GraphPathUtils.calculateCumulativeDurations;
+import static org.opentripplanner.ext.carpooling.util.GraphPathUtils.calculateDuration;
 
 import java.time.Duration;
 import java.util.ArrayList;
@@ -57,24 +58,18 @@ public InsertionEvaluator(
   }
 
   /**
-   * Routes all baseline segments and caches the results.
+   * Routes all segments of routePoints.
    *
    * @return Array of routed segments, or null if any segment fails to route
    */
   @SuppressWarnings("unchecked")
-  private GraphPath<State, Edge, Vertex>[] routeBaselineSegments(List<WgsCoordinate> routePoints) {
+  private GraphPath<State, Edge, Vertex>[] routeSegments(List<WgsCoordinate> routePoints) {
     GraphPath<State, Edge, Vertex>[] segments = new GraphPath[routePoints.size() - 1];
 
     for (int i = 0; i < routePoints.size() - 1; i++) {
       var fromCoord = routePoints.get(i);
       var toCoord = routePoints.get(i + 1);
-      GenericLocation from = GenericLocation.fromCoordinate(
-        fromCoord.latitude(),
-        fromCoord.longitude()
-      );
-      GenericLocation to = GenericLocation.fromCoordinate(toCoord.latitude(), toCoord.longitude());
-
-      GraphPath<State, Edge, Vertex> segment = routingFunction.route(from, to, linkingContext);
+      GraphPath<State, Edge, Vertex> segment = routeSegment(fromCoord, toCoord);
       if (segment == null) {
         LOG.debug("Baseline routing failed for segment {} → {}", i, i + 1);
         return null;
@@ -86,6 +81,23 @@ private GraphPath<State, Edge, Vertex>[] routeBaselineSegments(List<WgsCoordinat
     return segments;
   }
 
+  /**
+   * Routes from start to stop
+   *
+   * @return route from coordinate from to coordinate to, or null if routing function fails
+   */
+  private GraphPath<State, Edge, Vertex> routeSegment(WgsCoordinate from, WgsCoordinate to) {
+    GenericLocation fromGenericLocation = GenericLocation.fromCoordinate(
+      from.latitude(),
+      from.longitude()
+    );
+    GenericLocation toGenericLocation = GenericLocation.fromCoordinate(
+      to.latitude(),
+      to.longitude()
+    );
+    return routingFunction.route(fromGenericLocation, toGenericLocation, linkingContext);
+  }
+
   /**
    * Evaluates pre-filtered insertion positions using A* routing.
    * <p>
@@ -107,17 +119,28 @@ public InsertionCandidate findBestInsertion(
     WgsCoordinate passengerPickup,
     WgsCoordinate passengerDropoff
   ) {
-    GraphPath<State, Edge, Vertex>[] baselineSegments = routeBaselineSegments(trip.routePoints());
+    GraphPath<State, Edge, Vertex>[] baselineSegments = routeSegments(trip.routePoints());
     if (baselineSegments == null) {
       LOG.warn("Could not route baseline for trip {}", trip.getId());
       return null;
     }
 
     Duration[] cumulativeDurations = calculateCumulativeDurations(baselineSegments);
 
+    GraphPath<State, Edge, Vertex> pathBetweenStartAndStop = routeSegment(
+      trip.stops().getFirst().getCoordinate(),
+      trip.stops().getLast().getCoordinate()
+    );
+
+    if (pathBetweenStartAndStop == null) {
+      LOG.warn("Could not create route between start and stop for trip {}", trip.getId());
+      return null;
+    }
+
+    Duration durationBetweenOriginAndDestination = calculateDuration(pathBetweenStartAndStop);
+
     InsertionCandidate bestCandidate = null;
     Duration minAdditionalDuration = INITIAL_ADDITIONAL_DURATION;
-    Duration baselineDuration = cumulativeDurations[cumulativeDurations.length - 1];
 
     for (InsertionPosition position : viablePositions) {
       InsertionCandidate candidate = evaluateInsertion(
@@ -128,7 +151,7 @@ public InsertionCandidate findBestInsertion(
         passengerDropoff,
         baselineSegments,
         cumulativeDurations,
-        baselineDuration
+        durationBetweenOriginAndDestination
       );
 
       if (candidate == null) {
@@ -168,7 +191,7 @@ private InsertionCandidate evaluateInsertion(
     WgsCoordinate passengerDropoff,
     GraphPath<State, Edge, Vertex>[] baselineSegments,
     Duration[] originalCumulativeDurations,
-    Duration baselineDuration
+    Duration durationBetweenOriginAndDestination
   ) {
     // Build modified route segments by reusing cached baseline segments
     List<GraphPath<State, Edge, Vertex>> modifiedSegments = buildModifiedSegments(
@@ -217,7 +240,7 @@ private InsertionCandidate evaluateInsertion(
       pickupPos,
       dropoffPos,
       modifiedSegments,
-      baselineDuration,
+      durationBetweenOriginAndDestination,
       totalDuration
     );
   }

application/src/ext/java/org/opentripplanner/ext/carpooling/util/GraphPathUtils.java

@@ -16,13 +16,20 @@ public static Duration[] calculateCumulativeDurations(GraphPath<State, Edge, Ver
     cumulativeDurations[0] = Duration.ZERO;
 
     for (int i = 0; i < segments.length; i++) {
-      Duration segmentDuration = Duration.between(
-        segments[i].states.getFirst().getTime(),
-        segments[i].states.getLast().getTime()
-      );
+      Duration segmentDuration = calculateDuration(segments[i]);
       cumulativeDurations[i + 1] = cumulativeDurations[i].plus(segmentDuration);
     }
 
     return cumulativeDurations;
   }
+
+  /**
+   * Calculates duration for a segment
+   */
+  public static Duration calculateDuration(GraphPath<State, Edge, Vertex> segment) {
+    return Duration.between(
+      segment.states.getFirst().getTime(),
+      segment.states.getLast().getTime()
+    );
+  }
 }

application/src/test/java/org/opentripplanner/ext/carpooling/routing/InsertionEvaluatorTest.java

@@ -70,6 +70,34 @@ private InsertionCandidate findOptimalInsertion(
     return evaluator.findBestInsertion(trip, viablePositions, passengerPickup, passengerDropoff);
   }
 
+  private WgsCoordinate getCoordinateBetween(WgsCoordinate coordinate1, WgsCoordinate coordinate2) {
+    return new WgsCoordinate(
+      (coordinate1.latitude() + coordinate2.latitude()) / 2,
+      (coordinate1.longitude() + coordinate2.longitude()) / 2
+    );
+  }
+
+  @Test
+  void findOptimalInsertion_onDeviationBudgetExceeded_returnsNull() {
+    var trip = createTripWithStops(
+      OSLO_SOUTH,
+      List.of(createStopAt(0, OSLO_CENTER), createStopAt(0, OSLO_NORTHEAST)),
+      OSLO_NORTH
+    );
+
+    var mockPath = createGraphPath(Duration.ofMinutes(4));
+    RoutingFunction routingFunction = (from, to, linkingContext) -> mockPath;
+
+    var result = findOptimalInsertion(
+      trip,
+      getCoordinateBetween(OSLO_SOUTH, OSLO_CENTER),
+      getCoordinateBetween(OSLO_NORTHEAST, OSLO_NORTH),
+      routingFunction
+    );
+
+    assertNull(result);
+  }
+
   @Test
   void findOptimalInsertion_noValidPositions_returnsNull() {
     var trip = createSimpleTrip(OSLO_CENTER, OSLO_NORTH);
@@ -103,20 +131,20 @@ void findOptimalInsertion_routingFails_skipsPosition() {
     var stop1 = createStopAt(0, OSLO_EAST);
     var trip = createTripWithStops(OSLO_CENTER, List.of(stop1), OSLO_NORTH);
 
-    var mockPath = createGraphPath(Duration.ofMinutes(5));
+    var mockPath = createGraphPath(Duration.ofMinutes(3));
 
     // Routing sequence:
     // 1. Baseline calculation (2 segments: OSLO_CENTER → OSLO_EAST → OSLO_NORTH) = mockPath x2
     // 2. First insertion attempt fails (null for first segment)
     // 3. Second insertion attempt succeeds (mockPath for all segments)
     @SuppressWarnings("ConstantConditions")
     RoutingFunction routingFunction = (from, to, linkingContext) -> {
-      if(
+      if (
         new WgsCoordinate(from.lat, from.lng).equals(OSLO_CENTER) &&
         new WgsCoordinate(to.lat, to.lng).equals(OSLO_MIDPOINT_NORTH)
       ) {
         return null;
-      }else{
+      } else {
         return mockPath;
       }
     };
@@ -181,16 +209,24 @@ void findOptimalInsertion_baselineDurationCalculationFails_returnsNull() {
   void findOptimalInsertion_selectsMinimumAdditionalDuration() {
     var trip = createTripWithDeviationBudget(Duration.ofMinutes(20), OSLO_CENTER, OSLO_NORTH);
 
-    final Map<Pair<WgsCoordinate>, GraphPath<State, Edge, Vertex>> pathsMap = new HashMap<>(Map.of(
-      new Pair<>(OSLO_CENTER, OSLO_NORTH), createGraphPath(Duration.ofMinutes(10)),
-      new Pair<>(OSLO_CENTER, OSLO_EAST), createGraphPath(Duration.ofMinutes(4)),
-      new Pair<>(OSLO_EAST, OSLO_WEST), createGraphPath(Duration.ofMinutes(5)),
-      new Pair<>(OSLO_WEST, OSLO_NORTH), createGraphPath(Duration.ofMinutes(6))
-    ));
+    final Map<Pair<WgsCoordinate>, GraphPath<State, Edge, Vertex>> pathsMap = new HashMap<>(
+      Map.of(
+        new Pair<>(OSLO_CENTER, OSLO_NORTH),
+        createGraphPath(Duration.ofMinutes(10)),
+        new Pair<>(OSLO_CENTER, OSLO_EAST),
+        createGraphPath(Duration.ofMinutes(4)),
+        new Pair<>(OSLO_EAST, OSLO_WEST),
+        createGraphPath(Duration.ofMinutes(5)),
+        new Pair<>(OSLO_WEST, OSLO_NORTH),
+        createGraphPath(Duration.ofMinutes(6))
+      )
+    );
 
     @SuppressWarnings("ConstantConditions")
     RoutingFunction routingFunction = (from, to, linkingContext) ->
-      pathsMap.get(new Pair<>(new WgsCoordinate(from.lat, from.lng), new WgsCoordinate(to.lat, to.lng)));
+      pathsMap.get(
+        new Pair<>(new WgsCoordinate(from.lat, from.lng), new WgsCoordinate(to.lat, to.lng))
+      );
 
     var result = findOptimalInsertion(trip, OSLO_EAST, OSLO_WEST, routingFunction);
 
@@ -241,18 +277,26 @@ void findOptimalInsertion_insertBetweenTwoPoints_routesAllSegments() {
     // MIDPOINT_NORTH → NORTH
     var segmentDB = createGraphPath(Duration.ofMinutes(4));
 
-    final Map<Pair<WgsCoordinate>, GraphPath<State, Edge, Vertex>> pathsMap = new HashMap<>(Map.of(
-      new Pair<>(OSLO_CENTER, OSLO_NORTH), baselinePath,
-      new Pair<>(OSLO_CENTER, OSLO_EAST), segmentAC,
-      new Pair<>(OSLO_EAST, OSLO_MIDPOINT_NORTH), segmentCD,
-      new Pair<>(OSLO_MIDPOINT_NORTH, OSLO_NORTH), segmentDB
-    ));
+    final Map<Pair<WgsCoordinate>, GraphPath<State, Edge, Vertex>> pathsMap = new HashMap<>(
+      Map.of(
+        new Pair<>(OSLO_CENTER, OSLO_NORTH),
+        baselinePath,
+        new Pair<>(OSLO_CENTER, OSLO_EAST),
+        segmentAC,
+        new Pair<>(OSLO_EAST, OSLO_MIDPOINT_NORTH),
+        segmentCD,
+        new Pair<>(OSLO_MIDPOINT_NORTH, OSLO_NORTH),
+        segmentDB
+      )
+    );
 
     final int[] callCount = { 0 };
     @SuppressWarnings("ConstantConditions")
     RoutingFunction routingFunction = (from, to, linkingContext) -> {
       callCount[0]++;
-      return pathsMap.get(new Pair<>(new WgsCoordinate(from.lat, from.lng), new WgsCoordinate(to.lat, to.lng)));
+      return pathsMap.get(
+        new Pair<>(new WgsCoordinate(from.lat, from.lng), new WgsCoordinate(to.lat, to.lng))
+      );
     };
 
     // Passenger pickup at OSLO_EAST, dropoff at OSLO_MIDPOINT_NORTH
@@ -268,8 +312,9 @@ void findOptimalInsertion_insertBetweenTwoPoints_routesAllSegments() {
     // (typically 1-2 seconds per edge due to millisecond rounding in StreetEdge.doTraverse())
     // The baseline should be approximately 10 minutes (within 10 seconds tolerance)
     assertTrue(
-      Math.abs(result.baselineDuration().toSeconds() - 600) < 10,
-      "Baseline should be approximately 10 min (within 10s), got " + result.baselineDuration()
+      Math.abs(result.durationBetweenOriginAndDestination().toSeconds() - 600) < 10,
+      "Baseline should be approximately 10 min (within 10s), got " +
+      result.durationBetweenOriginAndDestination()
     );
 
     // CRITICAL: Total duration should be sum of NEW segments, NOT baseline duration
@@ -302,7 +347,7 @@ void findOptimalInsertion_insertAtEnd_reusesMostSegments() {
     var trip = createTripWithStops(OSLO_CENTER, List.of(stop1), OSLO_NORTH);
 
     // Baseline has 2 segments: CENTER→EAST, EAST→NORTH
-    var mockPath = createGraphPath(Duration.ofMinutes(5));
+    var mockPath = createGraphPath(Duration.ofMinutes(3));
 
     final int[] callCount = { 0 };
     RoutingFunction routingFunction = (from, to, linkingContext) -> {
@@ -315,8 +360,12 @@ void findOptimalInsertion_insertAtEnd_reusesMostSegments() {
 
     assertNotNull(result, "Should find valid insertion");
 
-    // Baseline should be calculated correctly
-    assertEquals(Duration.ofMinutes(10), result.baselineDuration());
+    // Duration between start and stop should be calculated correctly
+    assertTrue(
+      Duration.ofMinutes(3).minus(result.durationBetweenOriginAndDestination()).toSeconds() < 10,
+      "Baseline should be approximately 3 min (within 10s), got " +
+      result.durationBetweenOriginAndDestination()
+    );
 
     // The modified route should have more segments than baseline
     assertTrue(
@@ -342,7 +391,7 @@ void findOptimalInsertion_pickupAtExistingPoint_handlesCorrectly() {
     var stop1 = createStopAt(0, OSLO_EAST);
     var trip = createTripWithStops(OSLO_CENTER, List.of(stop1), OSLO_NORTHEAST);
 
-    var mockPath = createGraphPath(Duration.ofMinutes(5));
+    var mockPath = createGraphPath(Duration.ofMinutes(3));
 
     final int[] callCount = { 0 };
     RoutingFunction routingFunction = (from, to, linkingContext) -> {
@@ -388,6 +437,6 @@ void findOptimalInsertion_singleSegmentTrip_routesAllNewSegments() {
 
     // Total duration should be positive
     assertTrue(result.totalDuration().compareTo(Duration.ZERO) > 0);
-    assertTrue(result.baselineDuration().compareTo(Duration.ZERO) > 0);
+    assertTrue(result.durationBetweenOriginAndDestination().compareTo(Duration.ZERO) > 0);
   }
 }