core: etcs: add begin position constraint to braking curve calculations

Unit test incoming

Signed-off-by: Erashin <[email protected]>

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/ETCSBrakingCurves.kt

@@ -7,6 +7,7 @@ import fr.sncf.osrd.envelope.part.EnvelopePart
 import fr.sncf.osrd.envelope.part.EnvelopePartBuilder
 import fr.sncf.osrd.envelope.part.constraints.EnvelopeConstraint
 import fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType
+import fr.sncf.osrd.envelope.part.constraints.PositionConstraint
 import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint
 import fr.sncf.osrd.envelope_sim.*
 import fr.sncf.osrd.envelope_sim.overlays.EnvelopeDeceleration
@@ -36,22 +37,25 @@ fun addBrakingCurvesAtEOAs(
     context: EnvelopeSimContext,
     endsOfAuthority: Collection<EndOfAuthority>
 ): Envelope {
+    val sortedEndsOfAuthority = endsOfAuthority.sortedBy { it.offsetEOA }
+    var beginPos = 0.0
     val builder = OverlayEnvelopeBuilder.backward(envelope)
-    for (endOfAuthority in endsOfAuthority) {
+    for (endOfAuthority in sortedEndsOfAuthority) {
         val targetPosition = endOfAuthority.offsetEOA.distance.meters
         val targetSpeed = 0.0
         val overhead =
             Envelope.make(
                 EnvelopePart.generateTimes(
                     listOf(EnvelopeProfile.CONSTANT_SPEED),
-                    doubleArrayOf(0.0, targetPosition),
+                    doubleArrayOf(beginPos, targetPosition),
                     doubleArrayOf(envelope.maxSpeed, envelope.maxSpeed)
                 )
             )
         val guiCurve =
             computeBrakingCurve(
                 context,
                 overhead,
+                beginPos,
                 targetPosition,
                 targetSpeed,
                 BrakingType.ETCS_GUI
@@ -60,14 +64,22 @@ fun addBrakingCurvesAtEOAs(
             computeBrakingCurve(
                 context,
                 overhead,
+                beginPos,
                 targetPosition,
                 targetSpeed,
                 BrakingType.ETCS_SBD
             )
         val fullIndicationCurve =
-            computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
-        val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope)
+            computeIndicationBrakingCurveFromRef(
+                context,
+                sbdCurve,
+                BrakingCurveType.SBD,
+                guiCurve,
+                beginPos
+            )
+        val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope, beginPos)
         builder.addPart(indicationCurve)
+        beginPos = targetPosition
     }
     return builder.build()
 }
@@ -76,15 +88,16 @@ fun addBrakingCurvesAtEOAs(
 private fun computeBrakingCurve(
     context: EnvelopeSimContext,
     envelope: Envelope,
+    beginPos: Double,
     targetPosition: Double,
     targetSpeed: Double,
     brakingType: BrakingType
 ): EnvelopePart {
-    // If the stopPosition is below zero, the input is invalid
+    // If the stopPosition is below begin position, the input is invalid
     // If the stopPosition is after the end of the path, the input is invalid except if it is an
     // SVL, i.e. the target speed is 0 and the curve to compute is an EBD
     if (
-        targetPosition <= 0.0 ||
+        targetPosition <= beginPos ||
             (targetPosition > context.path.length &&
                 targetSpeed == 0.0 &&
                 brakingType != BrakingType.ETCS_EBD)
@@ -100,6 +113,7 @@ private fun computeBrakingCurve(
     val overlayBuilder =
         ConstrainedEnvelopePartBuilder(
             partBuilder,
+            PositionConstraint(beginPos, targetPosition),
             SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR),
             EnvelopeConstraint(envelope, EnvelopePartConstraintType.CEILING)
         )
@@ -120,11 +134,13 @@ private fun computeIndicationBrakingCurveFromRef(
     context: EnvelopeSimContext,
     refCurve: EnvelopePart,
     refBrakingCurveType: BrakingCurveType,
-    guiCurve: EnvelopePart
+    guiCurve: EnvelopePart,
+    beginPos: Double
 ): EnvelopePart {
     assert(
         refBrakingCurveType == BrakingCurveType.EBI || refBrakingCurveType == BrakingCurveType.SBD
     )
+    assert(refCurve.endPos > beginPos)
     val rollingStock = context.rollingStock
     val tBs =
         if (refBrakingCurveType == BrakingCurveType.EBI) rollingStock.rjsEtcsBrakeParams.tBs2
@@ -140,16 +156,17 @@ private fun computeIndicationBrakingCurveFromRef(
         val permittedSpeedPosition =
             getPermittedSpeedPosition(sbiPosition, speed, guiCurve.interpolatePosition(speed))
         val indicationPosition = getIndicationPosition(permittedSpeedPosition, speed, tBs)
-        if (indicationPosition >= 0) {
+        if (indicationPosition >= beginPos) {
             reversedNewPos.add(indicationPosition)
             reversedNewSpeeds.add(speed)
-        } else if (i != refCurve.pointCount() - 1 && reversedNewPos[i + 1] > 0) {
+        } else if (i != refCurve.pointCount() - 1 && reversedNewPos[i + 1] > beginPos) {
             val prevPos = reversedNewPos[i + 1]
             val prevSpeed = reversedNewSpeeds[i + 1]
-            val speedAtZero =
-                prevSpeed - prevPos * (speed - prevSpeed) / (indicationPosition - prevPos)
-            reversedNewPos.add(0.0)
-            reversedNewSpeeds.add(speedAtZero)
+            val speedAtBeginPos =
+                prevSpeed +
+                    (beginPos - prevPos) * (speed - prevSpeed) / (indicationPosition - prevPos)
+            reversedNewPos.add(beginPos)
+            reversedNewSpeeds.add(speedAtBeginPos)
             break
         }
     }
@@ -169,13 +186,15 @@ private fun computeIndicationBrakingCurveFromRef(
 
 /**
  * Keep the part of the full braking curve which is located underneath the overlay and intersects
- * with it.
+ * with it or with begin position.
  */
 private fun keepBrakingCurveUnderOverlay(
     fullBrakingCurve: EnvelopePart,
-    overlay: Envelope
+    overlay: Envelope,
+    beginPos: Double
 ): EnvelopePart {
     assert(fullBrakingCurve.maxSpeed >= overlay.minSpeed)
+    assert(fullBrakingCurve.endPos > beginPos)
     val positions = fullBrakingCurve.clonePositions()
     val speeds = fullBrakingCurve.cloneSpeeds()
     val timeDeltas = fullBrakingCurve.cloneTimes()
@@ -185,6 +204,7 @@ private fun keepBrakingCurveUnderOverlay(
     val overlayBuilder =
         ConstrainedEnvelopePartBuilder(
             partBuilder,
+            PositionConstraint(beginPos, overlay.endPos),
             SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR),
             EnvelopeConstraint(overlay, EnvelopePartConstraintType.CEILING)
         )