core: support arrival on stop signal for conflict detection

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/TrainPhysicsIntegrator.java

@@ -175,6 +175,10 @@ public static boolean areTimesEqual(double a, double b) {
         return areDoublesEqual(a, b, TIME_EPSILON);
     }
 
+    public static boolean isTimeStrictlyPositive(double time) {
+        return time > TIME_EPSILON;
+    }
+
     private static boolean areDoublesEqual(double a, double b, double delta) {
         return Math.abs(a - b) < delta;
     }

core/osrd-railjson/src/main/java/fr/sncf/osrd/railjson/schema/schedule/RJSTrainStop.java

@@ -1,5 +1,6 @@
 package fr.sncf.osrd.railjson.schema.schedule;
 
+import com.squareup.moshi.Json;
 import edu.umd.cs.findbugs.annotations.SuppressFBWarnings;
 import fr.sncf.osrd.railjson.schema.common.RJSTrackLocation;
 
@@ -18,17 +19,21 @@ public class RJSTrainStop {
     @SuppressFBWarnings("UWF_NULL_FIELD")
     public RJSTrackLocation location;
 
+    @Json(name = "on_stop_signal")
+    public boolean onStopSignal;
+
     /** Stop duration */
     public double duration;
 
     /** Constructor with position */
-    public RJSTrainStop(Double position, double duration) {
+    public RJSTrainStop(Double position, double duration, boolean onStopSignal) {
         this.position = position;
         this.location = null;
         this.duration = duration;
+        this.onStopSignal = onStopSignal;
     }
 
     public static RJSTrainStop lastStop(double duration) {
-        return new RJSTrainStop(-1., duration);
+        return new RJSTrainStop(-1., duration, true);
     }
 }

core/src/main/java/fr/sncf/osrd/conflicts/IncrementalPath.kt

@@ -1,14 +1,19 @@
 package fr.sncf.osrd.conflicts
 
 import fr.sncf.osrd.sim_infra.api.*
+import fr.sncf.osrd.sim_infra.utils.getBlockEntry
+import fr.sncf.osrd.sim_infra.utils.getBlockExit
 import fr.sncf.osrd.utils.AppendOnlyLinkedList
 import fr.sncf.osrd.utils.appendOnlyLinkedListOf
 import fr.sncf.osrd.utils.indexing.StaticIdxList
 import fr.sncf.osrd.utils.units.*
 
+data class PathStop(val pathOffset: Offset<Path>, val onStopSignal: Boolean)
+
 class PathFragment(
     val routes: StaticIdxList<Route>,
     val blocks: StaticIdxList<Block>,
+    val stops: List<PathStop>,
     val containsStart: Boolean,
     val containsEnd: Boolean,
 
@@ -43,9 +48,10 @@ interface IncrementalPath {
 
     fun clone(): IncrementalPath
 
-    val endZonePathIndex: Int
-    val endBlockIndex: Int
-    val endRouteIndex: Int
+    val zonePathCount: Int
+    val blockCount: Int
+    val routeCount: Int
+    val stopCount: Int
 
     val routes: AppendOnlyLinkedList<RouteId>
 
@@ -81,23 +87,31 @@ interface IncrementalPath {
 
     fun getBlockEndZone(blockIndex: Int): Int
 
+    fun getStopOffset(stopIndex: Int): Offset<Path>
+
+    fun isStopOnClosedSignal(stopIndex: Int): Boolean
+
     val pathStarted: Boolean
     /** can only be called if pathStarted */
     val travelledPathBegin: Offset<Path>
 
     val pathComplete: Boolean
     /** can only be called if pathComplete */
-    val travelledPathEnd: Offset<Path> //
+    val travelledPathEnd: Offset<Path>
 
     fun toTravelledPath(offset: Offset<Path>): Offset<TravelledPath>
 
     fun fromTravelledPath(offset: Offset<TravelledPath>): Offset<Path>
+
+    fun getBlockPathEnd(): Offset<Path>
 }
 
 fun incrementalPathOf(rawInfra: RawInfra, blockInfra: BlockInfra): IncrementalPath {
     return IncrementalPathImpl(rawInfra, blockInfra)
 }
 
+private class IncrementalStop(val offset: Offset<Path>, val onStopSignal: Boolean)
+
 private class IncrementalPathImpl(
     private val rawInfra: RawInfra,
     private val blockInfra: BlockInfra,
@@ -106,6 +120,7 @@ private class IncrementalPathImpl(
     private var zonePaths: AppendOnlyLinkedList<ZonePathId> = appendOnlyLinkedListOf(),
     override var routes: AppendOnlyLinkedList<RouteId> = appendOnlyLinkedListOf(),
     private var blocks: AppendOnlyLinkedList<BlockId> = appendOnlyLinkedListOf(),
+    private var stops: AppendOnlyLinkedList<IncrementalStop> = appendOnlyLinkedListOf(),
 
     // lookup tables from blocks and routes to zone path bounds
     private val blockZoneBounds: AppendOnlyLinkedList<Int> = appendOnlyLinkedListOf(),
@@ -124,20 +139,27 @@ private class IncrementalPathImpl(
     override val pathComplete
         get() = travelledPathEnd != Offset<Path>((-1).meters)
 
-    override val endZonePathIndex
+    override val zonePathCount
         get() = zonePaths.size
 
-    override val endBlockIndex
+    override val blockCount
         get() = blocks.size
 
-    override val endRouteIndex
+    override val routeCount
         get() = routes.size
 
+    override val stopCount: Int
+        get() = stops.size
+
     override fun extend(fragment: PathFragment) {
         assert(!pathComplete) { "extending a complete path" }
 
         // add zones and routes
         for (route in fragment.routes) {
+            assert(
+                routes.isEmpty() ||
+                    rawInfra.getRouteEntry(route) == rawInfra.getRouteExit(routes.last())
+            )
             for (zonePath in rawInfra.getRoutePath(route)) {
                 val zonePathLen = rawInfra.getZonePathLength(zonePath)
                 val curEndOffset = zonePathBounds.last()
@@ -182,15 +204,25 @@ private class IncrementalPathImpl(
 
         // find the index of the zone path at which this fragment's blocks start
         val fragmentBlocksStartZoneIndex = blockZoneBounds.last()
+        val fragmentStartOffset = zonePathBounds[fragmentBlocksStartZoneIndex]
 
         if (fragment.containsStart) {
             assert(!pathStarted)
-            val curBlockOffset = zonePathBounds[fragmentBlocksStartZoneIndex]
-            travelledPathBegin = curBlockOffset + fragment.travelledPathBegin
+            travelledPathBegin = fragmentStartOffset + fragment.travelledPathBegin
+        }
+
+        for (stop in fragment.stops) {
+            val offset = fragmentStartOffset + stop.pathOffset.distance
+            stops.add(IncrementalStop(offset, stop.onStopSignal))
         }
 
         var fragBlocksZoneCount = 0
         for (block in fragment.blocks) {
+            assert(
+                blocks.isEmpty() ||
+                    blockInfra.getBlockEntry(rawInfra, block) ==
+                        blockInfra.getBlockExit(rawInfra, blocks.last())
+            )
             val blockPath = blockInfra.getBlockPath(block)
             fragBlocksZoneCount += blockPath.size
             val blockEndZonePathIndex = fragmentBlocksStartZoneIndex + fragBlocksZoneCount
@@ -212,6 +244,7 @@ private class IncrementalPathImpl(
             this.zonePaths.shallowCopy(),
             this.routes.shallowCopy(),
             this.blocks.shallowCopy(),
+            this.stops.shallowCopy(),
             this.blockZoneBounds.shallowCopy(),
             this.routeZoneBounds.shallowCopy(),
             this.zonePathBounds.shallowCopy(),
@@ -284,11 +317,23 @@ private class IncrementalPathImpl(
         return blockZoneBounds[blockIndex + 1]
     }
 
+    override fun getStopOffset(stopIndex: Int): Offset<Path> {
+        return stops[stopIndex].offset
+    }
+
+    override fun isStopOnClosedSignal(stopIndex: Int): Boolean {
+        return stops[stopIndex].onStopSignal
+    }
+
     override fun toTravelledPath(offset: Offset<Path>): Offset<TravelledPath> {
         return Offset(offset.distance - travelledPathBegin.distance)
     }
 
     override fun fromTravelledPath(offset: Offset<TravelledPath>): Offset<Path> {
         return Offset(offset.distance + travelledPathBegin.distance)
     }
+
+    override fun getBlockPathEnd(): Offset<Path> {
+        return getBlockEndOffset(blocks.size - 1)
+    }
 }

core/src/main/java/fr/sncf/osrd/conflicts/IncrementalRequirementEnvelopeAdapter.kt

@@ -16,17 +16,36 @@ class IncrementalRequirementEnvelopeAdapter(
         pathBeginOff: Offset<TravelledPath>,
         pathEndOff: Offset<TravelledPath>
     ): Double {
-        if (envelopeWithStops == null) return Double.POSITIVE_INFINITY
+        if (envelopeWithStops == null) {
+            return Double.POSITIVE_INFINITY
+        }
         val begin = pathBeginOff.distance.meters
         val end = pathEndOff.distance.meters
-        if (max(0.0, begin) >= min(envelopeWithStops.endPos, end))
+        if (max(0.0, begin) >= min(envelopeWithStops.endPos, end)) {
             return Double.POSITIVE_INFINITY // no overlap
+        }
         return envelopeWithStops.maxSpeedInRange(
             max(begin, 0.0),
             min(end, envelopeWithStops.endPos)
         )
     }
 
+    override fun departureFromStop(stopOffset: Offset<TravelledPath>): Double {
+        if (envelopeWithStops == null) {
+            return Double.POSITIVE_INFINITY
+        }
+        val endPos = envelopeWithStops.endPos
+        if (stopOffset.distance.meters > endPos) {
+            return Double.POSITIVE_INFINITY
+        }
+        // stop duration is included in interpolateTotalTime()
+        var pastStop = (stopOffset.distance).meters
+        if (pastStop > endPos) {
+            pastStop = endPos
+        }
+        return envelopeWithStops.interpolateTotalTime(pastStop)
+    }
+
     override fun arrivalTimeInRange(
         pathBeginOff: Offset<TravelledPath>,
         pathEndOff: Offset<TravelledPath>

core/src/main/java/fr/sncf/osrd/conflicts/Resources.kt

@@ -31,6 +31,9 @@ interface IncrementalRequirementCallbacks {
         pathBeginOff: Offset<TravelledPath>,
         pathEndOff: Offset<TravelledPath>
     ): Double
+
+    // departure time from a given stop. if the train never gets to a stop, +inf is returned
+    fun departureFromStop(stopOffset: Offset<TravelledPath>): Double
 }
 
 data class PathSignal(