core: fix pathfinding waypoints at track transitions

core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt

@@ -255,14 +255,13 @@ fun buildChunkPath(
     var mutBeginOffset = pathBeginOffset
     var mutEndOffset = pathEndOffset
     for (dirChunkId in chunks) {
-        if (totalBlocksLength >= pathEndOffset)
+        if (totalBlocksLength > pathEndOffset)
             break
         val length = infra.getTrackChunkLength(dirChunkId.value)
         val blockEndOffset = totalBlocksLength + length.distance
 
         // if the block ends before the path starts, it can be safely skipped
-        // If a block ends where the path starts, it can be skipped too
-        if (pathBeginOffset >= blockEndOffset) {
+        if (pathBeginOffset > blockEndOffset) {
             mutBeginOffset -= length.distance
             mutEndOffset -= length.distance
         } else {

core/osrd-geom/src/main/java/fr/sncf/osrd/geom/LineString.java

@@ -206,7 +206,6 @@ public Point interpolateNormalized(double distance) {
     public LineString slice(double begin, double end) {
         assert begin >= 0 && begin <= 1;
         assert end >= 0 && end <= 1;
-        assert Double.compare(begin, end) != 0;
 
         if (begin > end)
             return slice(end, begin).reverse();

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt

@@ -96,7 +96,7 @@ private fun getRouteDirTracks(rawInfra: RawSignalingInfra, routeId: RouteId): Di
     return res
 }
 
-private fun validatePathfindingResult(
+fun validatePathfindingResult(
     res: PathfindingResult, reqWaypoints: Array<Array<PathfindingWaypoint>>,
     rawInfra: RawSignalingInfra
 ) {
@@ -112,7 +112,6 @@ private fun validatePathfindingResult(
         .flatMap { route: RJSRoutePath? -> route!!.trackSections.stream() }
         .toList()
     assertPathTracksAreComplete(tracksOnPath, rawInfra)
-    assertRequiredWaypointsOnPathTracks(reqWaypoints, tracksOnPath, res.pathWaypoints)
 }
 
 private fun assertPathRoutesAreAdjacent(routeTracks: DirStaticIdxList<TrackSection>, rawInfra: RawSignalingInfra) {
@@ -173,33 +172,6 @@ private fun assertPathTracksAreComplete(tracksOnPath: List<RJSDirectionalTrackRa
     }
 }
 
-private fun assertRequiredWaypointsOnPathTracks(
-    reqWaypoints: Array<Array<PathfindingWaypoint>>,
-    tracksOnPath: List<RJSDirectionalTrackRange>,
-    pathWaypoints: List<PathWaypointResult>
-) {
-    // Checks that at least one waypoint of each step is on the path
-    assert(Arrays.stream(reqWaypoints).allMatch { step: Array<PathfindingWaypoint>? ->
-        Arrays.stream(step)
-            .anyMatch { waypoint: PathfindingWaypoint ->
-                tracksOnPath.stream()
-                    .anyMatch { trackOnPath: RJSDirectionalTrackRange -> isWaypointOnTrack(waypoint, trackOnPath) }
-            }
-    }) { "The path does not contain one of the wanted steps" }
-    for (waypoint in pathWaypoints) {
-        val loc = waypoint.location
-        assert(tracksOnPath.stream()
-            .filter { range: RJSDirectionalTrackRange -> range.trackSectionID == loc.trackSection }
-            .anyMatch { range: RJSDirectionalTrackRange ->
-                ((loc.offset > range.begin || TrainPhysicsIntegrator.arePositionsEqual(loc.offset, range.begin))
-                        && (loc.offset < range.end || TrainPhysicsIntegrator.arePositionsEqual(
-                    loc.offset,
-                    range.end
-                )))
-            }) { "A waypoint isn't included in the track path" }
-    }
-}
-
 private fun isWaypointOnTrack(waypoint: PathfindingWaypoint, track: RJSDirectionalTrackRange): Boolean {
     return (track.trackSectionID == waypoint.trackSection && track.direction == waypoint.direction
             && (track.begin <= waypoint.offset || abs(track.begin - waypoint.offset) < 1e-3)

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingResultConverter.kt

@@ -81,7 +81,7 @@ private fun makeUserDefinedWaypoints(
         for (waypoint in userDefinedWaypointsPerBlock.getOrDefault(blockRange.edge, ArrayList())) {
             if (blockRange.start <= waypoint && waypoint <= blockRange.end) {
                 val pathOffset = lengthPrevBlocks + waypoint.distance - startFirstRange.distance
-                res.add(makePendingWaypoint(infra, path, false, pathOffset, null))
+                res.add(makePendingUserDefinedWaypoint(infra, path, pathOffset))
             }
         }
         lengthPrevBlocks += blockInfra.getBlockLength(blockRange.edge).distance
@@ -96,27 +96,43 @@ fun makeOperationalPoints(
 ): Collection<PathWaypointResult> {
     val res = ArrayList<PathWaypointResult>()
     for ((opId, offset) in path.getOperationalPointParts()) {
-        val opName = infra.getOperationalPointPartName(opId)
-        res.add(makePendingWaypoint(infra, path, true, offset, opName))
+        res.add(makePendingOPWaypoint(infra, offset, opId))
     }
     return res
 }
 
+/** Creates a pending waypoint from an operational point part  */
+private fun makePendingOPWaypoint(
+    infra: RawSignalingInfra,
+    pathOffset: Offset<Path>,
+    opId: OperationalPointPartId
+): PathWaypointResult {
+    val partChunk = infra.getOperationalPointPartChunk(opId)
+    val partChunkOffset = infra.getOperationalPointPartChunkOffset(opId)
+    val opName = infra.getOperationalPointPartName(opId)
+    val trackId = infra.getTrackFromChunk(partChunk)
+    val trackOffset = partChunkOffset + infra.getTrackChunkOffset(partChunk).distance
+    val trackName = infra.getTrackSectionName(trackId)
+    val location = PathWaypointLocation(
+        trackName,
+        trackOffset.distance.meters
+    )
+    return PathWaypointResult(location, pathOffset.distance.meters, true, opName)
+}
+
 /** Creates a pending waypoint from a path and its offset  */
-private fun makePendingWaypoint(
+private fun makePendingUserDefinedWaypoint(
     infra: RawSignalingInfra,
     path: PathProperties,
-    suggestion: Boolean,
-    pathOffset: Offset<Path>,
-    opName: String?
+    pathOffset: Offset<Path>
 ): PathWaypointResult {
     val (trackId, offset) = path.getTrackLocationAtOffset(pathOffset)
     val trackName = infra.getTrackSectionName(trackId)
     val location = PathWaypointLocation(
         trackName,
         offset.distance.meters
     )
-    return PathWaypointResult(location, pathOffset.distance.meters, suggestion, opName)
+    return PathWaypointResult(location, pathOffset.distance.meters, false, null)
 }
 
 /** Sorts the waypoints on the path. When waypoints overlap, the user-defined one is kept.  */
@@ -259,7 +275,7 @@ private fun makeRJSTrackRanges(
         val dirEndOfRouteRange = dirTrackChunkOffset + routeEndOffset.distance - chunkStartPathOffset.distance
         val dirRangeStartOnTrack = Offset.max(dirTrackChunkOffset, dirStartOfRouteRange)
         val dirRangeEndOnTrack = Offset.min(dirTrackChunkOffset + chunkLength.distance, dirEndOfRouteRange)
-        if (dirRangeStartOnTrack < dirRangeEndOnTrack) {
+        if (dirRangeStartOnTrack <= dirRangeEndOnTrack) {
             val trackName = infra.getTrackSectionName(trackId)
             val direction =
                 if (dirChunkId.direction === Direction.INCREASING)

core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/response/PathWaypointResult.kt

@@ -46,6 +46,7 @@ class PathWaypointResult(
             return
         pathOffset = other.pathOffset
         id = other.id
+        location = other.location
     }
 
     override fun equals(other: Any?): Boolean {

core/src/test/kotlin/fr/sncf/osrd/pathfinding/PathfindingTest.kt

@@ -2,6 +2,7 @@ package fr.sncf.osrd.pathfinding
 
 import fr.sncf.osrd.api.ApiTest
 import fr.sncf.osrd.api.pathfinding.PathfindingBlocksEndpoint
+import fr.sncf.osrd.api.pathfinding.convertPathfindingResult
 import fr.sncf.osrd.api.pathfinding.request.PathfindingRequest
 import fr.sncf.osrd.api.pathfinding.request.PathfindingWaypoint
 import fr.sncf.osrd.api.pathfinding.response.CurveChartPointResult
@@ -10,18 +11,18 @@ import fr.sncf.osrd.api.pathfinding.response.PathWaypointResult.PathWaypointLoca
 import fr.sncf.osrd.api.pathfinding.response.PathfindingResult
 import fr.sncf.osrd.api.pathfinding.response.SlopeChartPointResult
 import fr.sncf.osrd.api.pathfinding.runPathfinding
+import fr.sncf.osrd.api.pathfinding.validatePathfindingResult
 import fr.sncf.osrd.cli.StandaloneSimulationCommand
 import fr.sncf.osrd.railjson.schema.common.graph.ApplicableDirection
 import fr.sncf.osrd.railjson.schema.common.graph.EdgeDirection
+import fr.sncf.osrd.railjson.schema.infra.RJSOperationalPoint
 import fr.sncf.osrd.railjson.schema.infra.RJSRoutePath
 import fr.sncf.osrd.railjson.schema.infra.RJSTrackSection
-import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSApplicableDirectionsTrackRange
-import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSCatenary
-import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSDirectionalTrackRange
-import fr.sncf.osrd.railjson.schema.infra.trackranges.RJSLoadingGaugeLimit
+import fr.sncf.osrd.railjson.schema.infra.trackranges.*
 import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType
 import fr.sncf.osrd.reporting.exceptions.ErrorType
 import fr.sncf.osrd.reporting.exceptions.OSRDError
+import fr.sncf.osrd.reporting.warnings.DiagnosticRecorderImpl
 import fr.sncf.osrd.train.TestTrains
 import fr.sncf.osrd.utils.Helpers
 import fr.sncf.osrd.utils.moshi.MoshiUtils
@@ -43,6 +44,7 @@ import java.util.stream.Collectors
 import java.util.stream.Stream
 import kotlin.math.max
 import kotlin.math.min
+import kotlin.test.assertEquals
 
 @TestInstance(TestInstance.Lifecycle.PER_CLASS)
 class PathfindingTest : ApiTest() {
@@ -624,6 +626,60 @@ class PathfindingTest : ApiTest() {
         )
     }
 
+    @Test
+    fun pathStartingAtTrackEdge() {
+        /*
+        foo_a   foo_to_bar   bar_a
+        ------>|----------->|------>
+              ^             ^
+           new_op_1      new_op_2
+         */
+        val waypointStart = PathfindingWaypoint("ne.micro.foo_a", 200.0, EdgeDirection.START_TO_STOP)
+        val waypointEnd = PathfindingWaypoint("ne.micro.bar_a", 0.0, EdgeDirection.START_TO_STOP)
+        val waypoints = Array(2) { Array(1) { waypointStart } }
+        waypoints[1][0] = waypointEnd
+        val rjsInfra = Helpers.getExampleInfra("tiny_infra/infra.json")
+        rjsInfra.operationalPoints.add(
+            RJSOperationalPoint(
+                "new_op_1", listOf(
+                    RJSOperationalPointPart("ne.micro.foo_a", 200.0)
+                )
+            )
+        )
+        rjsInfra.operationalPoints.add(
+            RJSOperationalPoint(
+                "new_op_2", listOf(
+                    RJSOperationalPointPart("ne.micro.bar_a", 0.0)
+                )
+            )
+        )
+        val infra = Helpers.fullInfraFromRJS(rjsInfra)
+
+        val path = runPathfinding(
+            infra,
+            waypoints,
+            listOf(TestTrains.REALISTIC_FAST_TRAIN)
+        )
+        val res = convertPathfindingResult(
+            infra.blockInfra, infra.rawInfra,
+            path, DiagnosticRecorderImpl(true)
+        )
+        validatePathfindingResult(res, waypoints, infra.rawInfra)
+        assertEquals(
+            listOf(
+                PathWaypointResult(
+                    PathWaypointLocation("ne.micro.foo_a", 200.0),
+                    0.0, false, "new_op_1"
+                ),
+                PathWaypointResult(
+                    PathWaypointLocation("ne.micro.bar_a", 0.0),
+                    10_000.0, false, "new_op_2"
+                ),
+            ),
+            res.pathWaypoints
+        )
+    }
+
     companion object {
         private const val SIGNALING_TYPE = "BAL3"
         private fun makeBidirectionalEndPoint(point: PathfindingWaypoint): Array<PathfindingWaypoint> {