core: pathfinding: fix pathfinding class to handle overlapping points

Signed-off-by: Eloi Charpentier <[email protected]>
OpenRailAssociation · Jan 24, 2025 · 1fe7f70 · 1fe7f70
1 parent a90a394
commit 1fe7f70
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 1 deletion.
diff --git a/core/src/main/kotlin/fr/sncf/osrd/graph/Pathfinding.kt b/core/src/main/kotlin/fr/sncf/osrd/graph/Pathfinding.kt
@@ -230,11 +230,21 @@ class Pathfinding<NodeT : Any, EdgeT : Any, OffsetType>(
                         }
                         val stepTargets = ArrayList(step.targets)
                         stepTargets.add(target)
+
+                        // Handle overlapping consecutive waypoints
+                        var newNReachedTargets = step.nReachedTargets + 1
+                        while (
+                            newNReachedTargets < targetsOnEdges.size &&
+                                targetsOnEdges[newNReachedTargets]
+                                    .apply(step.range.edge)
+                                    .contains(EdgeLocation(step.range.edge, step.range.end))
+                        ) newNReachedTargets++
+
                         registerStep(
                             newRange,
                             step.prev,
                             step.totalDistance,
-                            step.nReachedTargets + 1,
+                            newNReachedTargets,
                             stepTargets
                         )
                     }

diff --git a/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt b/core/src/test/java/fr/sncf/osrd/utils/graph/PathfindingTests.kt
@@ -133,6 +133,29 @@ class PathfindingTests {
         Assertions.assertEquals(listOf("0-1", "1-3", "3-4"), resIDs)
     }
 
+    @Test
+    fun overlappingWaypoints() {
+        val builder = SimpleGraphBuilder()
+        builder.makeNodes(7)
+        builder.makeEdge(0, 1, 10.meters)
+        builder.makeEdge(1, 2, 10.meters)
+        builder.makeEdge(2, 3, 10.meters)
+        val g = builder.build()
+        val res =
+            Pathfinding(g)
+                .setEdgeToLength { edge -> edge.length }
+                .runPathfindingEdgesOnly(
+                    listOf(
+                        listOf(builder.getEdgeLocation("0-1", 10.meters)),
+                        listOf(builder.getEdgeLocation("1-2", 10.meters)),
+                        listOf(builder.getEdgeLocation("1-2", 10.meters)),
+                        listOf(builder.getEdgeLocation("2-3", 1.meters)),
+                    )
+                )
+        val resIDs = res!!.map { x -> x.label }
+        Assertions.assertEquals(listOf("0-1", "1-2", "2-3"), resIDs)
+    }
+
     @Test
     fun severalStartsTest() {
         /* Bottom path has more edges but is shorter