From 1fe7f7055741ab114fe0ac23f6ba56ca84f59305 Mon Sep 17 00:00:00 2001
From: Eloi Charpentier <eloi.charpentier.42@gmail.com>
Date: Thu, 23 Jan 2025 17:24:51 +0100
Subject: [PATCH] core: pathfinding: fix pathfinding class to handle
 overlapping points

Signed-off-by: Eloi Charpentier <eloi.charpentier.42@gmail.com>
---
 .../kotlin/fr/sncf/osrd/graph/Pathfinding.kt  | 12 +++++++++-
 .../sncf/osrd/utils/graph/PathfindingTests.kt | 23 +++++++++++++++++++
 2 files changed, 34 insertions(+), 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
index e45cd8efbf4..f0efeff0d43 100644
--- 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
index 8978e519ec1..03e055ca37a 100644
--- 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