core: compute min between 2 envelope parts

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

core/envelope-sim/build.gradle

@@ -41,6 +41,9 @@ dependencies {
     // Use JUnit Jupiter Engine for testing.
     testRuntimeOnly libs.junit.jupiter.engine
 
+    // Use AssertJ for testing
+    testImplementation libs.assertj
+
     // for linter annotations
     testFixturesCompileOnly libs.jcip.annotations
     testFixturesCompileOnly libs.spotbugs.annotations

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope/part/EnvelopePart.java

@@ -9,10 +9,7 @@
 import fr.sncf.osrd.envelope_sim.EnvelopeProfile;
 import fr.sncf.osrd.envelope_utils.ExcludeFromGeneratedCodeCoverage;
 import fr.sncf.osrd.utils.SelfTypeHolder;
-import java.util.Arrays;
-import java.util.Collections;
-import java.util.HashMap;
-import java.util.Map;
+import java.util.*;
 import java.util.stream.Collectors;
 
 /**
@@ -616,6 +613,65 @@ public EnvelopePart copyAndShift(double positionDelta, double minPosition, doubl
                 new HashMap<>(attrs), newPositions.toArray(), newSpeeds.toArray(), newTimeDeltas.toArray());
     }
 
+    /**
+     * Compute the minimum EnvelopePart between 2 envelope parts with an intersecting range [a, b].
+     * The resulting envelope part's starts at the envelope with the minimum speed at a and ends at b.
+     */
+    public static EnvelopePart min(
+            EnvelopePart envelopePartA, EnvelopePart envelopePartB, Iterable<SelfTypeHolder> attrs) {
+        var beginPosA = envelopePartA.getBeginPos();
+        var beginPosB = envelopePartB.getBeginPos();
+        var endPosA = envelopePartA.getEndPos();
+        var endPosB = envelopePartB.getEndPos();
+        assert (beginPosA < endPosB && endPosA > beginPosB);
+        var startIntersectingRange = Math.max(beginPosA, beginPosB);
+        var start = envelopePartA.interpolateSpeed(startIntersectingRange)
+                        <= envelopePartB.interpolateSpeed(startIntersectingRange)
+                ? beginPosA
+                : beginPosB;
+        var end = Math.min(endPosA, endPosB);
+
+        TreeSet<Double> keyPositions =
+                Arrays.stream(envelopePartA.positions).boxed().collect(Collectors.toCollection(TreeSet::new));
+        keyPositions.addAll(Arrays.stream(envelopePartB.positions).boxed().collect(Collectors.toSet()));
+        keyPositions = new TreeSet<>(keyPositions.subSet(start, true, end, true));
+        var keyPosList = new ArrayList<>(keyPositions);
+
+        var newPositions = new DoubleArrayList();
+        var newSpeeds = new DoubleArrayList();
+        for (int i = 0; i < keyPosList.size(); i++) {
+            var pos = keyPosList.get(i);
+            boolean inEnvelopePartA = pos >= beginPosA;
+            boolean inEnvelopePartB = pos >= beginPosB;
+
+            double speedA = inEnvelopePartA ? envelopePartA.interpolateSpeed(pos) : Double.POSITIVE_INFINITY;
+            double speedB = inEnvelopePartB ? envelopePartB.interpolateSpeed(pos) : Double.POSITIVE_INFINITY;
+            double minSpeedAtPos = Math.min(speedA, speedB);
+
+            if (i > 0) {
+                double prevPos = keyPosList.get(i - 1);
+                boolean prevInEnvelopePartA = prevPos >= beginPosA;
+                boolean prevInEnvelopePartB = prevPos >= beginPosB;
+
+                if (inEnvelopePartA && inEnvelopePartB && prevInEnvelopePartA && prevInEnvelopePartB) {
+                    double prevSpeedA = envelopePartA.interpolateSpeed(prevPos);
+                    double prevSpeedB = envelopePartB.interpolateSpeed(prevPos);
+
+                    if ((prevSpeedA - prevSpeedB) * (speedA - speedB) < 0) {
+                        var intersection = EnvelopePhysics.intersectSteps(
+                                prevPos, prevSpeedA, pos, speedA, prevPos, prevSpeedB, pos, speedB);
+                        // Add intersection point
+                        newPositions.add(intersection.position);
+                        newSpeeds.add(intersection.speed);
+                    }
+                }
+            }
+            newPositions.add(pos);
+            newSpeeds.add(minSpeedAtPos);
+        }
+        return generateTimes(attrs, newPositions.toArray(), newSpeeds.toArray());
+    }
+
     // endregion
 
     // region EQUALS

core/envelope-sim/src/test/java/fr/sncf/osrd/envelope/EnvelopePartTest.java

@@ -1,14 +1,19 @@
 package fr.sncf.osrd.envelope;
 
 import static fr.sncf.osrd.envelope.EnvelopePhysics.getPartMechanicalEnergyConsumed;
+import static org.assertj.core.api.Assertions.assertThat;
 import static org.junit.jupiter.api.Assertions.*;
 
 import fr.sncf.osrd.envelope.part.EnvelopePart;
 import fr.sncf.osrd.envelope_sim.*;
 import fr.sncf.osrd.envelope_sim.allowances.utils.AllowanceValue;
+import java.util.Collections;
 import java.util.List;
-import org.junit.jupiter.api.Assertions;
+import java.util.stream.Stream;
 import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
 
 class EnvelopePartTest {
     @Test
@@ -93,7 +98,7 @@ void testGetMechanicalEnergyConsumed() {
                             / 1_000_000;
                     break;
                 case 1:
-                    Assertions.assertEquals(envelopePart.getMinSpeed(), envelopePart.getMaxSpeed());
+                    assertEquals(envelopePart.getMinSpeed(), envelopePart.getMaxSpeed());
                     expectedEnvelopePartEnergy = testRollingStock.getRollingResistance(envelopePart.getBeginSpeed())
                             * envelopePart.getTotalDistance();
                     break;
@@ -109,4 +114,43 @@ void testGetMechanicalEnergyConsumed() {
             assertEquals(expectedEnvelopePartEnergy, envelopePartEnergy, 0.1 * expectedEnvelopePartEnergy + 1000);
         }
     }
+
+    @ParameterizedTest
+    @MethodSource("minEnvelopePartsArgs")
+    void testMinEnvelopeParts(
+            EnvelopePart envelopePartA,
+            EnvelopePart envelopePartB,
+            Stream<Integer> intersectionIndexes,
+            double[] expectedPositions,
+            double[] expectedSpeeds) {
+        var minEnvelope = EnvelopePart.min(
+                envelopePartA, envelopePartB, Collections.singleton(envelopePartA.getAttr(EnvelopeProfile.class)));
+        var resultingPositions = minEnvelope.clonePositions();
+        var resultingSpeeds = minEnvelope.cloneSpeeds();
+
+        intersectionIndexes.forEach(intersectionIndex -> {
+            var intersectionPosition = resultingPositions[intersectionIndex];
+            var intersectionSpeed = resultingSpeeds[intersectionIndex];
+            assertEquals(envelopePartA.interpolateSpeed(intersectionPosition), intersectionSpeed);
+            assertEquals(envelopePartB.interpolateSpeed(intersectionPosition), intersectionSpeed);
+        });
+        assertThat(expectedPositions).isEqualTo(resultingPositions);
+        assertThat(expectedSpeeds).isEqualTo(resultingSpeeds);
+    }
+
+    static Stream<Arguments> minEnvelopePartsArgs() {
+        return Stream.of(
+                Arguments.of(
+                        EnvelopeTestUtils.generateTimes(new double[] {0, 10, 20}, new double[] {100, 50, 0}),
+                        EnvelopeTestUtils.generateTimes(new double[] {5, 14, 16}, new double[] {150, 50, 0}),
+                        Stream.of(4),
+                        new double[] {0, 5, 10, 14, 15, 16},
+                        new double[] {100, 79.05694150420949, 50, 38.72983346207417, 35.35533905932738, 0}),
+                Arguments.of(
+                        EnvelopeTestUtils.generateTimes(new double[] {5, 14, 16}, new double[] {100, 50, 0}),
+                        EnvelopeTestUtils.generateTimes(new double[] {0, 10, 20}, new double[] {150, 50, 0}),
+                        Stream.of(1, 4),
+                        new double[] {5, 7.142857142857143, 10, 14, 15, 16},
+                        new double[] {100, 90.63269671749657, 50, 38.72983346207417, 35.35533905932738, 0}));
+    }
 }