core: pathfinding: filter out paths with repeated tracks during explo…

…ration

Signed-off-by: Eloi Charpentier <[email protected]>

core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/AppendOnlySet.kt

@@ -0,0 +1,36 @@
+package fr.sncf.osrd.utils
+
+/**
+ * Append-only set implementation. The internal structure is a linked list. Used to store data on
+ * diverging paths while minimizing copies. See also `AppendOnlyLinkedList`.
+ */
+class AppendOnlySet<T>(
+    private val list: AppendOnlyLinkedList<T>,
+    private val keyFilter: BloomFilter<T>,
+) {
+
+    /** Add the given value to the set. O(1). */
+    fun add(k: T) {
+        keyFilter.add(k)
+        list.add(k)
+    }
+
+    /** Returns a copy of the set. The underlying structure is *not* copied. O(1). */
+    fun shallowCopy(): AppendOnlySet<T> {
+        return AppendOnlySet(list.shallowCopy(), keyFilter.copy())
+    }
+
+    /**
+     * Returns true if the value is in the set. O(n) in worst case, O(1) if the value is near the
+     * end. Pre-filtered using a bloom filter.
+     */
+    fun contains(key: T): Boolean {
+        if (!keyFilter.mayContain(key)) return false
+        return list.findLast { it == key } != null
+    }
+}
+
+/** Returns a new empty set */
+fun <T> appendOnlySetOf(): AppendOnlySet<T> {
+    return AppendOnlySet(appendOnlyLinkedListOf(), emptyBloomFilter())
+}

core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorer.kt

@@ -14,8 +14,10 @@ import fr.sncf.osrd.sim_infra.utils.getRouteBlocks
 import fr.sncf.osrd.sim_infra.utils.routesOnBlock
 import fr.sncf.osrd.utils.AppendOnlyLinkedList
 import fr.sncf.osrd.utils.AppendOnlyMap
+import fr.sncf.osrd.utils.AppendOnlySet
 import fr.sncf.osrd.utils.appendOnlyLinkedListOf
 import fr.sncf.osrd.utils.appendOnlyMapOf
+import fr.sncf.osrd.utils.appendOnlySetOf
 import fr.sncf.osrd.utils.indexing.StaticIdx
 import fr.sncf.osrd.utils.indexing.StaticIdxList
 import fr.sncf.osrd.utils.indexing.mutableStaticIdxArrayListOf
@@ -131,6 +133,8 @@ fun initInfraExplorer(
                 appendOnlyLinkedListOf(),
                 appendOnlyLinkedListOf(),
                 appendOnlyMapOf(),
+                appendOnlySetOf(),
+                null,
                 incrementalPath,
                 blockToPathProperties,
                 stops = stops,
@@ -148,6 +152,8 @@ private class InfraExplorerImpl(
     private var blocks: AppendOnlyLinkedList<BlockId>,
     private var routes: AppendOnlyLinkedList<RouteId>,
     private var blockRoutes: AppendOnlyMap<BlockId, RouteId>,
+    private var visitedTracks: AppendOnlySet<TrackSectionId>,
+    private var lastTrack: TrackSectionId?,
     private var incrementalPath: IncrementalPath,
     private var pathPropertiesCache: MutableMap<BlockId, PathProperties>,
     private var currentIndex: Int = 0,
@@ -254,6 +260,8 @@ private class InfraExplorerImpl(
             this.blocks.shallowCopy(),
             this.routes.shallowCopy(),
             this.blockRoutes.shallowCopy(),
+            this.visitedTracks.shallowCopy(),
+            this.lastTrack,
             this.incrementalPath.clone(),
             this.pathPropertiesCache,
             this.currentIndex,
@@ -281,7 +289,7 @@ private class InfraExplorerImpl(
         val addedBlocks = mutableListOf<BlockId>()
 
         for (block in routeBlocks) {
-            if (blockRoutes.containsKey(block)) return false // We already passed by this block
+            if (hasRepeatedElements(block)) return false
             addedBlocks.add(block)
             if (block == firstLocation?.edge) {
                 seenFirstBlock = true
@@ -345,6 +353,26 @@ private class InfraExplorerImpl(
         return true
     }
 
+    /**
+     * Return true if parts of the new block have already been visited, otherwise mark tracks as
+     * visited.
+     */
+    private fun hasRepeatedElements(block: StaticIdx<Block>): Boolean {
+        if (blockRoutes.containsKey(block)) return true
+        val tracks =
+            blockInfra
+                .getBlockPath(block)
+                .flatMap { rawInfra.getZonePathChunks(it) }
+                .map { rawInfra.getTrackFromChunk(it.value) }
+        for (track in tracks) {
+            if (track == lastTrack) continue
+            if (visitedTracks.contains(track)) return true
+            visitedTracks.add(track)
+            lastTrack = track
+        }
+        return false
+    }
+
     private fun findStopsInTravelledPathAndOnBlock(
         block: StaticIdx<Block>,
         travelledPathBeginBlockOffset: Offset<Block>,

core/src/test/kotlin/fr/sncf/osrd/utils/DummyInfra.kt

@@ -391,7 +391,7 @@ class DummyInfra : RawInfra, BlockInfra {
     }
 
     override fun getTrackFromChunk(trackChunk: TrackChunkId): TrackSectionId {
-        TODO("Not yet implemented")
+        return convertId(trackChunk)
     }
 
     override fun getTrackChunkSlope(trackChunk: DirTrackChunkId): DistanceRangeMap<Double> {