From d1788fd4c10cf91d88508b7fd3188329923dba5f Mon Sep 17 00:00:00 2001 From: "Victor \"multun\" Collod" Date: Wed, 6 Sep 2023 15:52:49 +0200 Subject: [PATCH] core: incremental spacing resource generation Switch the spacing resource generation to an incremental generator, in preparation for the STDCM integration. Currently, it is only called in one go by the standalone simulation. Co-Authored-By: Victor "multun" Collod Co-Authored-By: Younes "khoyo" Khoudli --- core/build.gradle | 1 + .../src/main/kotlin/collections/RingBuffer.kt | 21 +- .../src/test/kotlin/TestRingBuffer.kt | 16 + .../osrd/sim_infra/api/InterlockingInfra.kt | 4 + .../osrd/sim_infra/impl/BlockInfraBuilder.kt | 15 +- .../osrd/sim_infra/impl/BlockInfraImpl.kt | 7 +- .../osrd/sim_infra/impl/PathPropertiesImpl.kt | 1 - .../fr/sncf/osrd/utils/indexing/StaticIdx.kt | 4 +- .../fr/sncf/osrd/utils/units/Distance.kt | 4 +- .../fr/sncf/osrd/conflicts/IncrementalPath.kt | 273 +++++++++++++++++ .../IncrementalRequirementEnvelopeAdapter.kt | 51 ++++ .../java/fr/sncf/osrd/conflicts/Resources.kt | 25 ++ .../conflicts/SpacingResourceGenerator.kt | 278 ++++++++++++++++++ .../ScheduleMetadataExtractor.kt | 224 ++------------ .../standalone_sim/result/ResultTrain.java | 2 + .../sim_infra_adapter/PathPropertiesTests.kt | 2 +- 16 files changed, 706 insertions(+), 222 deletions(-) create mode 100644 core/src/main/java/fr/sncf/osrd/conflicts/IncrementalPath.kt create mode 100644 core/src/main/java/fr/sncf/osrd/conflicts/IncrementalRequirementEnvelopeAdapter.kt create mode 100644 core/src/main/java/fr/sncf/osrd/conflicts/Resources.kt create mode 100644 core/src/main/java/fr/sncf/osrd/conflicts/SpacingResourceGenerator.kt diff --git a/core/build.gradle b/core/build.gradle index 3cff5676bc9..8a504538ad2 100644 --- a/core/build.gradle +++ b/core/build.gradle @@ -34,6 +34,7 @@ dependencies { testImplementation libs.kotlinx.coroutines.test implementation project(':kt-osrd-utils') + implementation project(':kt-fast-collections') implementation project(':kt-osrd-signaling') implementation project(":kt-osrd-sim-interlocking") implementation project(":kt-osrd-sim-infra") diff --git a/core/kt-fast-collections-generator/src/main/kotlin/collections/RingBuffer.kt b/core/kt-fast-collections-generator/src/main/kotlin/collections/RingBuffer.kt index adc078ae911..6a8c7ed35fe 100644 --- a/core/kt-fast-collections-generator/src/main/kotlin/collections/RingBuffer.kt +++ b/core/kt-fast-collections-generator/src/main/kotlin/collections/RingBuffer.kt @@ -12,13 +12,10 @@ private fun CollectionItemType.generateRingBuffer(context: GeneratorContext, cur val simpleName = type.simpleName val paramsDecl = type.paramsDecl val paramsUse = type.paramsUse - val paramsStar = type.paramsStar val fileName = "${simpleName}RingBuffer" val bufferType = "Mutable${simpleName}Array${paramsUse}" - val itemListType = "${simpleName}List${paramsUse}" val storageType = storageType!! val primitiveZero = storageType.primitiveZero() - val toPrimitive = storageType.toPrimitive val wrapperZero = storageType.fromPrimitive(primitiveZero) val file = context.codeGenerator.createNewFile(Dependencies(true, currentFile), generatedPackage, fileName) file.appendText(""" @@ -45,6 +42,12 @@ private fun CollectionItemType.generateRingBuffer(context: GeneratorContext, cur val size get() = _size + fun isEmpty() = size == 0 + fun isNotEmpty() = size != 0 + + val beginIndex get() = if (isEmpty()) -1 else startIndex + val endIndex get() = if (isEmpty()) -1 else startIndex + size + /** GENERATED CODE */ override fun iterator(): Iterator<$type> { return object : Iterator<$type> { @@ -167,6 +170,18 @@ private fun CollectionItemType.generateRingBuffer(context: GeneratorContext, cur return oldValue } + /** GENERATED CODE */ + fun removeFrontUntil(cutoffIndex: Int) { + val removedCount = cutoffIndex - startIndex + if (removedCount == 0) + return + assert(removedCount > 0) + assert(removedCount <= size) + _size -= removedCount + startIndex += removedCount + offset = (offset + removedCount).mod(capacity) + } + /** GENERATED CODE */ fun clone() : Mutable${simpleName}RingBuffer${paramsUse} { return Mutable${simpleName}RingBuffer${paramsUse}(size, buffer.copyOf()) diff --git a/core/kt-fast-collections/src/test/kotlin/TestRingBuffer.kt b/core/kt-fast-collections/src/test/kotlin/TestRingBuffer.kt index 6ad57ff6811..e27c69aa504 100644 --- a/core/kt-fast-collections/src/test/kotlin/TestRingBuffer.kt +++ b/core/kt-fast-collections/src/test/kotlin/TestRingBuffer.kt @@ -113,4 +113,20 @@ class RingBufferTest { assertEquals(-2, deque.addFront(5)) assertEquals(listOf(5, 3, 1, 2, 4), deque.toList()) } + + @Test + fun `test removeFrontUntil`() { + val deque = MutableIntRingBuffer() + assertEquals(0, deque.addFront(1)) + assertEquals(1, deque.addBack(2)) + assertEquals(-1, deque.addFront(3)) + assertEquals(2, deque.addBack(4)) + assertEquals(-2, deque.addFront(5)) + assertEquals(listOf(5, 3, 1, 2, 4), deque.toList()) + deque.removeFrontUntil(1) // 1 is the index of "2" + assertEquals(2, deque[1]) + assertEquals(listOf(2, 4), deque.toList()) + deque.removeFrontUntil(3) // 3 is the index of "2" + assertEquals(listOf(), deque.toList()) + } } diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/InterlockingInfra.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/InterlockingInfra.kt index f085fcff8e0..ed400ea6858 100644 --- a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/InterlockingInfra.kt +++ b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/api/InterlockingInfra.kt @@ -54,6 +54,10 @@ interface ReservationInfra : LocationInfra { fun getZonePathChunks(zonePath: ZonePathId): DirStaticIdxList } +fun ReservationInfra.getZonePathZone(zonePath: ZonePathId): ZoneId { + return getNextZone(getZonePathEntry(zonePath))!! +} + /** A zone path is a path inside a zone */ sealed interface ZonePath typealias ZonePathId = StaticIdx diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraBuilder.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraBuilder.kt index 0a0c1328366..dc5ce80b00d 100644 --- a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraBuilder.kt +++ b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraBuilder.kt @@ -16,7 +16,7 @@ interface BlockInfraBuilder { } -class BlockInfraBuilderImpl : BlockInfraBuilder { +class BlockInfraBuilderImpl(val loadedSignalInfra: LoadedSignalInfra, val rawInfra: RawInfra) : BlockInfraBuilder { private val blockSet = mutableMapOf() private val blockPool = StaticPool() override fun block( @@ -27,11 +27,16 @@ class BlockInfraBuilderImpl : BlockInfraBuilder { signalsDistances: OffsetList, ): BlockId { assert(path.size != 0) - val newBlock = BlockDescriptor(startAtBufferStop, stopsAtBufferStop, path, signals, signalsDistances) + + var length = Length(0.meters) + for (zonePath in path) + length += rawInfra.getZonePathLength(zonePath).distance + + val newBlock = BlockDescriptor(length, startAtBufferStop, stopsAtBufferStop, path, signals, signalsDistances) return blockSet.getOrPut(newBlock) { blockPool.add(newBlock) } } - fun build(loadedSignalInfra: LoadedSignalInfra, rawInfra: RawInfra): BlockInfra { + fun build(): BlockInfra { return BlockInfraImpl(blockPool, loadedSignalInfra, rawInfra) } } @@ -42,7 +47,7 @@ fun blockInfraBuilder( rawInfra: RawInfra, init: BlockInfraBuilder.() -> Unit, ): BlockInfra { - val builder = BlockInfraBuilderImpl() + val builder = BlockInfraBuilderImpl(loadedSignalInfra, rawInfra) builder.init() - return builder.build(loadedSignalInfra, rawInfra) + return builder.build() } diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraImpl.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraImpl.kt index 12653aaae0a..c0d724467f9 100644 --- a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraImpl.kt +++ b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/BlockInfraImpl.kt @@ -6,6 +6,7 @@ import fr.sncf.osrd.utils.indexing.* import fr.sncf.osrd.utils.units.* class BlockDescriptor( + val length: Length, val startAtBufferStop: Boolean, val stopsAtBufferStop: Boolean, val path: StaticIdxList, @@ -117,10 +118,6 @@ class BlockInfraImpl( } override fun getBlockLength(block: BlockId): Length { - var length = Length(0.meters) - for (path in blockPool[block].path) { - length += rawInfra.getZonePathLength(path).distance - } - return length + return blockPool[block].length } } diff --git a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt index f6bfa3739f5..236f2b8f078 100644 --- a/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt +++ b/core/kt-osrd-sim-infra/src/main/kotlin/fr/sncf/osrd/sim_infra/impl/PathPropertiesImpl.kt @@ -7,7 +7,6 @@ import fr.sncf.osrd.utils.DistanceRangeMap import fr.sncf.osrd.utils.distanceRangeMapOf import fr.sncf.osrd.utils.indexing.DirStaticIdxList import fr.sncf.osrd.utils.units.Distance -import fr.sncf.osrd.utils.units.Offset import fr.sncf.osrd.utils.units.meters import java.lang.RuntimeException diff --git a/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/indexing/StaticIdx.kt b/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/indexing/StaticIdx.kt index 5cfa560c300..16b523ed232 100644 --- a/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/indexing/StaticIdx.kt +++ b/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/indexing/StaticIdx.kt @@ -3,7 +3,7 @@ primitive = UInt::class, fromPrimitive = "StaticIdx(%s)", toPrimitive = "%s.index", - collections = ["Array", "ArrayList", "ArraySortedSet"], + collections = ["Array", "ArrayList", "ArraySortedSet", "RingBuffer"], ) package fr.sncf.osrd.utils.indexing @@ -187,4 +187,4 @@ class VirtualStaticPool(private var _size: UInt) : StaticIdxIterable { return StaticIdxSpace(size) } -} \ No newline at end of file +} diff --git a/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/units/Distance.kt b/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/units/Distance.kt index fa356b4aa29..ae54c43eeec 100644 --- a/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/units/Distance.kt +++ b/core/kt-osrd-utils/src/main/kotlin/fr/sncf/osrd/utils/units/Distance.kt @@ -3,7 +3,7 @@ primitive = Long::class, fromPrimitive = "Distance(%s)", toPrimitive = "%s.millimeters", - collections = ["Array", "ArrayList"], + collections = ["Array", "ArrayList", "RingBuffer"], ) @file:PrimitiveWrapperCollections( @@ -11,7 +11,7 @@ primitive = Long::class, fromPrimitive = "Offset(Distance(%s))", toPrimitive = "%s.distance.millimeters", - collections = ["Array", "ArrayList"], + collections = ["Array", "ArrayList", "RingBuffer"], ) package fr.sncf.osrd.utils.units diff --git a/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalPath.kt b/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalPath.kt new file mode 100644 index 00000000000..a0c84b9a4bf --- /dev/null +++ b/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalPath.kt @@ -0,0 +1,273 @@ +package fr.sncf.osrd.conflicts + +import fr.sncf.osrd.fast_collections.MutableIntRingBuffer +import fr.sncf.osrd.fast_collections.mutableIntRingBufferOf +import fr.sncf.osrd.sim_infra.api.* +import fr.sncf.osrd.utils.indexing.MutableStaticIdxRingBuffer +import fr.sncf.osrd.utils.indexing.StaticIdxList +import fr.sncf.osrd.utils.units.* + + +class PathFragment( + val routes: StaticIdxList, + val blocks: StaticIdxList, + + val containsStart: Boolean, + val containsEnd: Boolean, + + // distance from the start of this fragment's first block + travelledPathBegin: Distance, + // distance from the end of this fragment's last block + travelledPathEnd: Distance, +) { + init { + assert(containsStart || travelledPathBegin == Distance.ZERO) + assert(containsEnd || travelledPathEnd == Distance.ZERO) + } + + val travelledPathBegin = travelledPathBegin + get() { + assert(containsStart) + return field + } + + val travelledPathEnd = travelledPathEnd + get() { + assert(containsEnd) + return field + } +} + +/** A marker type for Offset */ +sealed interface Path + +/** A marker type for Offset */ +sealed interface TravelledPath + +interface IncrementalPath { + fun extend(fragment: PathFragment) + + // TODO: implement trimming + fun setLastRequiredBlock(blockIndex: Int) + fun setLastRequiredRoute(routeIndex: Int) + fun trim() + + val beginZonePathIndex: Int + val beginBlockIndex: Int + val beginRouteIndex: Int + + val endZonePathIndex: Int + val endBlockIndex: Int + val endRouteIndex: Int + + fun getBlock(blockIndex: Int): BlockId + fun getRoute(routeIndex: Int): RouteId + fun getZonePath(zonePathIndex: Int): ZonePathId + + fun getZonePathStartOffset(zonePathIndex: Int): Offset + fun getBlockStartOffset(blockIndex: Int): Offset + fun getRouteStartOffset(routeIndex: Int): Offset + + fun getZonePathEndOffset(zonePathIndex: Int): Offset + fun getBlockEndOffset(blockIndex: Int): Offset + fun getRouteEndOffset(routeIndex: Int): Offset + + fun convertZonePathOffset(zonePathIndex: Int, offset: Offset): Offset + fun convertBlockOffset(blockIndex: Int, offset: Offset): Offset + fun convertRouteOffset(routeIndex: Int, offset: Offset): Offset + + fun getRouteStartZone(routeIndex: Int): Int + fun getRouteEndZone(routeIndex: Int): Int + fun getBlockStartZone(blockIndex: Int): Int + fun getBlockEndZone(blockIndex: Int): Int + + val pathStarted: Boolean + /** can only be called if pathStarted */ + val travelledPathBegin: Offset + + val pathComplete: Boolean + /** can only be called if pathComplete */ + val travelledPathEnd: Offset // + + fun toTravelledPath(offset: Offset): Offset +} + +fun incrementalPathOf(rawInfra: RawInfra, blockInfra: BlockInfra): IncrementalPath { + return IncrementalPathImpl(rawInfra, blockInfra) +} + +private class IncrementalPathImpl(private val rawInfra: RawInfra, private val blockInfra: BlockInfra) : IncrementalPath { + // objects + private var zonePaths = MutableStaticIdxRingBuffer() + private var routes = MutableStaticIdxRingBuffer() + private var blocks = MutableStaticIdxRingBuffer() + + // lookup tables from blocks and routes to zone path bounds + private val blockZoneBounds = MutableIntRingBuffer() + private val routeZoneBounds = mutableIntRingBufferOf(0) + + // a lookup table from zone index to zone start path offset + private var zonePathBounds = mutableOffsetRingBufferOf(Offset(0.meters)) + + override var travelledPathBegin = Offset((-1).meters) + override var travelledPathEnd = Offset((-1).meters) + + override val pathStarted get() = travelledPathBegin != Offset((-1).meters) + override val pathComplete get() = travelledPathEnd != Offset((-1).meters) + + override fun extend(fragment: PathFragment) { + assert(!pathComplete) { "extending a complete path" } + + // add zones and routes + for (route in fragment.routes) { + for (zonePath in rawInfra.getRoutePath(route)) { + val zonePathLen = rawInfra.getZonePathLength(zonePath) + val curEndOffset = zonePathBounds[zonePathBounds.endIndex - 1] + val newEndOffset = curEndOffset + zonePathLen.distance + zonePaths.addBack(zonePath) + zonePathBounds.addBack(newEndOffset) + } + routeZoneBounds.addBack(zonePaths.endIndex) + routes.addBack(route) + } + + assert(routes.isNotEmpty()) + assert(routeZoneBounds.endIndex == routes.endIndex + 1) + + if (fragment.blocks.size == 0) { + assert(!fragment.containsStart) + assert(!fragment.containsEnd) + return + } + + // if we're starting the path, locate the start of the first block relative to the first route + if (blockZoneBounds.isEmpty()) { + val firstBlock = fragment.blocks[0] + val firstBlockZonePath = blockInfra.getBlockPath(firstBlock)[0] + var firstBlockZonePathIndex = -1 + for (zonePathIndex in zonePaths.beginIndex until zonePaths.endIndex) { + val zonePath = zonePaths[zonePathIndex] + if (zonePath == firstBlockZonePath) { + firstBlockZonePathIndex = zonePathIndex + break + } + } + assert(firstBlockZonePathIndex != -1) { "block does not have any common points with routes" } + + // initialize block zone bounds + blockZoneBounds.addBack(firstBlockZonePathIndex) + } + + // find the index of the zone path at which this fragment's blocks start + val fragmentBlocksStartZoneIndex = blockZoneBounds[blockZoneBounds.endIndex - 1] + + if (fragment.containsStart) { + assert(!pathStarted) + val curBlockOffset = zonePathBounds[fragmentBlocksStartZoneIndex] + travelledPathBegin = curBlockOffset + fragment.travelledPathBegin + } + + var fragBlocksZoneCount = 0 + for (block in fragment.blocks) { + val blockPath = blockInfra.getBlockPath(block) + fragBlocksZoneCount += blockPath.size + val blockEndZonePathIndex = fragmentBlocksStartZoneIndex + fragBlocksZoneCount + assert(blockEndZonePathIndex <= zonePaths.endIndex) + blocks.addBack(block) + blockZoneBounds.addBack(blockEndZonePathIndex) + } + + if (fragment.containsEnd) { + val blockPathEnd = zonePathBounds[blockZoneBounds[blockZoneBounds.endIndex - 1]] + travelledPathEnd = blockPathEnd - fragment.travelledPathEnd + } + } + + override fun setLastRequiredBlock(blockIndex: Int) { + TODO("Not yet implemented") + } + + override fun setLastRequiredRoute(routeIndex: Int) { + TODO("Not yet implemented") + } + + override fun trim() { + TODO("Not yet implemented") + } + + override val beginZonePathIndex get() = zonePaths.beginIndex + override val beginBlockIndex get() = blocks.beginIndex + override val beginRouteIndex get() = routes.beginIndex + + override val endZonePathIndex get() = zonePaths.endIndex + override val endBlockIndex get() = blocks.endIndex + override val endRouteIndex get() = routes.endIndex + + override fun getBlock(blockIndex: Int): BlockId { + return blocks[blockIndex] + } + + override fun getRoute(routeIndex: Int): RouteId { + return routes[routeIndex] + } + + override fun getZonePath(zonePathIndex: Int): ZonePathId { + return zonePaths[zonePathIndex] + } + + override fun getZonePathStartOffset(zonePathIndex: Int): Offset { + return zonePathBounds[zonePathIndex] + } + + override fun getBlockStartOffset(blockIndex: Int): Offset { + return getZonePathStartOffset(getBlockStartZone(blockIndex)) + } + + override fun getRouteStartOffset(routeIndex: Int): Offset { + return getZonePathStartOffset(getRouteStartZone(routeIndex)) + } + + override fun getZonePathEndOffset(zonePathIndex: Int): Offset { + return zonePathBounds[zonePathIndex + 1] + } + + override fun getBlockEndOffset(blockIndex: Int): Offset { + return getZonePathEndOffset(getBlockEndZone(blockIndex) - 1) + } + + override fun getRouteEndOffset(routeIndex: Int): Offset { + return getZonePathEndOffset(getRouteEndZone(routeIndex) - 1) + } + + override fun convertZonePathOffset(zonePathIndex: Int, offset: Offset): Offset { + return getZonePathStartOffset(zonePathIndex) + offset.distance + } + + override fun convertBlockOffset(blockIndex: Int, offset: Offset): Offset { + return getBlockStartOffset(blockIndex) + offset.distance + } + + override fun convertRouteOffset(routeIndex: Int, offset: Offset): Offset { + return getRouteStartOffset(routeIndex) + offset.distance + } + + override fun getRouteStartZone(routeIndex: Int): Int { + return routeZoneBounds[routeIndex] + } + + override fun getRouteEndZone(routeIndex: Int): Int { + return routeZoneBounds[routeIndex + 1] + } + + override fun getBlockStartZone(blockIndex: Int): Int { + return blockZoneBounds[blockIndex] + } + + override fun getBlockEndZone(blockIndex: Int): Int { + return blockZoneBounds[blockIndex + 1] + } + + override fun toTravelledPath(offset: Offset): Offset { + return Offset(offset.distance - travelledPathBegin.distance) + } +} diff --git a/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalRequirementEnvelopeAdapter.kt b/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalRequirementEnvelopeAdapter.kt new file mode 100644 index 00000000000..0267772bfa4 --- /dev/null +++ b/core/src/main/java/fr/sncf/osrd/conflicts/IncrementalRequirementEnvelopeAdapter.kt @@ -0,0 +1,51 @@ +package fr.sncf.osrd.conflicts + +import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper +import fr.sncf.osrd.train.RollingStock +import fr.sncf.osrd.utils.units.Offset +import kotlin.math.absoluteValue +import kotlin.math.max +import kotlin.math.min + +class IncrementalRequirementEnvelopeAdapter( + private val incrementalPath: IncrementalPath, + private val rollingStock: RollingStock, + private val envelopeWithStops: EnvelopeStopWrapper +) : IncrementalRequirementCallbacks { + override fun arrivalTimeInRange(pathBeginOff: Offset, pathEndOff: Offset): Double { + // if the head of the train enters the zone at some point, use that + val travelledPathBegin = incrementalPath.toTravelledPath(pathBeginOff) + val begin = travelledPathBegin.distance.meters + if (begin >= 0.0 && begin <= envelopeWithStops.endPos) + return envelopeWithStops.interpolateTotalTime(begin) + + val travelledPathEnd = incrementalPath.toTravelledPath(pathEndOff) + val end = travelledPathEnd.distance.meters + + val trainBegin = -rollingStock.length + val trainEnd = 0.0 + + if (max(trainBegin, begin) < min(trainEnd, end)) + return 0.0 + + return Double.POSITIVE_INFINITY + } + + override fun departureTimeFromRange(pathBeginOff: Offset, pathEndOff: Offset): Double { + val travelledPathEnd = incrementalPath.toTravelledPath(pathEndOff) + val end = travelledPathEnd.distance.meters + + val criticalPoint = end + rollingStock.length + if (criticalPoint >= 0.0 && criticalPoint <= envelopeWithStops.endPos) + return envelopeWithStops.interpolateTotalTime(criticalPoint) + + if (arrivalTimeInRange(pathBeginOff, pathEndOff).isFinite()) + return envelopeWithStops.totalTime + + return Double.POSITIVE_INFINITY + } + + override fun endTime(): Double { + return envelopeWithStops.totalTime + } +} diff --git a/core/src/main/java/fr/sncf/osrd/conflicts/Resources.kt b/core/src/main/java/fr/sncf/osrd/conflicts/Resources.kt new file mode 100644 index 00000000000..4ec18bc1f55 --- /dev/null +++ b/core/src/main/java/fr/sncf/osrd/conflicts/Resources.kt @@ -0,0 +1,25 @@ +package fr.sncf.osrd.conflicts + +import fr.sncf.osrd.sim_infra.api.LogicalSignalId +import fr.sncf.osrd.utils.units.Offset + +interface IncrementalRequirementCallbacks { + // if the train never arrives in range, +inf is returned + // a range is used rather than a point to properly handle the train appearing and disappearing + fun arrivalTimeInRange(pathBeginOff: Offset, pathEndOff: Offset): Double + + // the departure time from a given location, which has to take into account train length. + // this is used to compute zone occupancy. if the train never leaves a location, +inf is returned + fun departureTimeFromRange(pathBeginOff: Offset, pathEndOff: Offset): Double + + // the end time of the train + fun endTime(): Double +} + + +data class PathSignal( + val signal: LogicalSignalId, + val pathOffset: Offset, + // when a signal is between blocks, prefer the index of the first block + val minBlockPathIndex: Int, +) diff --git a/core/src/main/java/fr/sncf/osrd/conflicts/SpacingResourceGenerator.kt b/core/src/main/java/fr/sncf/osrd/conflicts/SpacingResourceGenerator.kt new file mode 100644 index 00000000000..5c2ef499a4b --- /dev/null +++ b/core/src/main/java/fr/sncf/osrd/conflicts/SpacingResourceGenerator.kt @@ -0,0 +1,278 @@ +package fr.sncf.osrd.conflicts + +import fr.sncf.osrd.signaling.SignalingSimulator +import fr.sncf.osrd.signaling.ZoneStatus +import fr.sncf.osrd.sim_infra.api.* +import fr.sncf.osrd.sim_infra_adapter.SimInfraAdapter +import fr.sncf.osrd.standalone_sim.result.ResultTrain.SpacingRequirement +import fr.sncf.osrd.utils.indexing.mutableStaticIdxArrayListOf +import fr.sncf.osrd.utils.units.meters +import mu.KotlinLogging + +private val logger = KotlinLogging.logger {} + + +/** + * ``` + * zone occupied Y Y + * explicit requirement Y Y + * needs requirement Y Y Y + * signals ┎o ┎o ┎o ┎o ┎o ┎o + * zones +---------|----------|----------|----------|----------|----------| + * train path ============= + * phase headroom begin main end tailroom + * ``` + */ +enum class SpacingRequirementPhase { + HeadRoom, + Begin, + Main, + End, + TailRoom; + + /** Checks whether the current state accepts this zone configuration */ + fun check(occupied: Boolean, hasRequirement: Boolean): Boolean { + return when (this) { + HeadRoom -> !occupied && !hasRequirement + Begin -> occupied && !hasRequirement + Main -> occupied && hasRequirement + End -> !occupied && hasRequirement + TailRoom -> !occupied && !hasRequirement + } + } + + fun react(occupied: Boolean, hasRequirement: Boolean): SpacingRequirementPhase { + // no state change + if (check(occupied, hasRequirement)) + return this + + when (this) { + HeadRoom -> { + if (occupied) + return Begin.react(true, hasRequirement) + } + Begin -> { + if (hasRequirement) + return Main.react(occupied, true) + if (!occupied) + return TailRoom + } + Main -> { + if (!occupied) + return End.react(false, hasRequirement) + } + End -> { + if (!hasRequirement) + return TailRoom + } + TailRoom -> return TailRoom + } + return this + } +} + + +class SpacingRequirementAutomaton( + // context + val rawInfra: SimInfraAdapter, + val loadedSignalInfra: LoadedSignalInfra, + val blockInfra: BlockInfra, + val simulator: SignalingSimulator, + val callbacks: IncrementalRequirementCallbacks, + val incrementalPath: IncrementalPath, +) { + private var nextProcessedBlock = 0 + + private var phase = SpacingRequirementPhase.HeadRoom + + // last zone for which a requirement was emitted + private var lastEmittedZone = -1 + + // last zone whose occupancy was tested with the current pending signal + // when a new signal starts processing, this value is initialized to the zone immediately after it + private var nextProbedZoneForSignal = -1 + + // the queue of signals awaiting processing + private val pendingSignals = ArrayDeque() + + private fun registerPathExtension() { + // if the path has not yet started, skip signal processing + if (!incrementalPath.pathStarted) { + nextProcessedBlock = incrementalPath.endBlockIndex + return + } + + // queue signals + for (blockIndex in nextProcessedBlock until incrementalPath.endBlockIndex) { + val block = incrementalPath.getBlock(blockIndex) + val signals = blockInfra.getBlockSignals(block) + val signalBlockPositions = blockInfra.getSignalsPositions(block) + for (signalBlockIndex in 0 until signals.size) { + val signal = signals[signalBlockIndex] + val signalBlockPosition = signalBlockPositions[signalBlockIndex] + // skip block transition signals + if (signalBlockIndex == 0 && pendingSignals.isNotEmpty() && pendingSignals.last().signal == signal) + continue + + val signalPathOffset = incrementalPath.convertBlockOffset(blockIndex, signalBlockPosition) + // skip signals outside the path + if (signalPathOffset < incrementalPath.travelledPathBegin) + continue + if (incrementalPath.pathComplete && signalPathOffset >= incrementalPath.travelledPathEnd) + continue + pendingSignals.addLast(PathSignal(signal, signalPathOffset, blockIndex)) + } + } + nextProcessedBlock = incrementalPath.endBlockIndex + } + + /** + * For all zones which either occupied by the train or required at some point, emit a zone requirement. + * Some zones do not have requirements: those before the train's starting position, and those far enough from + * the end of the train path. + */ + private fun emitZoneRequirement( + zoneIndex: Int, + zoneRequirementTime: Double + ): SpacingRequirement? { + val zonePath = incrementalPath.getZonePath(zoneIndex) + val zone = rawInfra.getZonePathZone(zonePath) + + val zoneEntryOffset = incrementalPath.getZonePathStartOffset(zoneIndex) + val zoneExitOffset = incrementalPath.getZonePathEndOffset(zoneIndex) + val zoneEntryTime = callbacks.arrivalTimeInRange(zoneEntryOffset, zoneExitOffset) + val zoneExitTime = callbacks.departureTimeFromRange(zoneEntryOffset, zoneExitOffset) + val enters = zoneEntryTime.isFinite() + val exits = zoneExitTime.isFinite() + assert(enters == exits) + val occupied = enters + + val explicitRequirement = zoneRequirementTime.isFinite() + + phase = phase.react(occupied, explicitRequirement) + val correctPhase = phase.check(occupied, explicitRequirement) + if (!correctPhase) + logger.error { "incorrect phase for zone $zone" } + + if (phase == SpacingRequirementPhase.HeadRoom || phase == SpacingRequirementPhase.TailRoom) + return null + + val beginTime: Double + val endTime: Double + + val zoneName = rawInfra.getZoneName(zone) + + when (phase) { + SpacingRequirementPhase.Begin -> { + beginTime = 0.0 + endTime = zoneExitTime + } + SpacingRequirementPhase.Main -> { + beginTime = if (zoneRequirementTime.isFinite()) { + zoneRequirementTime + } else { + // zones may not be required due to faulty signaling. + // in this case, fall back to the time at which the zone was first occupied + logger.error { "missing main phase zone requirement on zone $zoneName" } + zoneEntryTime + } + endTime = zoneExitTime + } + else -> /* SpacingRequirementPhase.End */ { + assert(zoneRequirementTime.isFinite()) + beginTime = zoneRequirementTime + // the time at which this requirement ends is the one at which the train + // exits the simulation + endTime = callbacks.endTime() + } + } + + return SpacingRequirement(zoneName, beginTime, endTime) + } + + private fun getSignalProtectedZone(signal: PathSignal): Int { + // the signal protects all zone inside the block + // if a signal is at a block boundary, + return incrementalPath.getBlockEndZone(signal.minBlockPathIndex) + } + + fun processPathUpdate(): List { + registerPathExtension() + + val res = mutableListOf() + fun emitRequirementsUntil(zoneIndex: Int, sightTime: Double) { + if (zoneIndex <= lastEmittedZone) + return + + for (skippedZone in lastEmittedZone + 1 until zoneIndex) { + val req = emitZoneRequirement(skippedZone, Double.POSITIVE_INFINITY) + if (req != null) + res.add(req) + } + val req = emitZoneRequirement(zoneIndex, sightTime) + if (req != null) + res.add(req) + lastEmittedZone = zoneIndex + } + + + val blocks = mutableStaticIdxArrayListOf() + for (blockIndex in incrementalPath.beginBlockIndex until incrementalPath.endBlockIndex) + blocks.add(incrementalPath.getBlock(blockIndex)) + + // there may be more zone states than what's contained in the path's blocks, which shouldn't matter + val zoneStartIndex = incrementalPath.getBlockStartZone(incrementalPath.beginBlockIndex) + val zoneCount = incrementalPath.getBlockEndZone(incrementalPath.endBlockIndex - 1) - zoneStartIndex + val zoneStates = MutableList(zoneCount) { ZoneStatus.CLEAR } + + // for all signals, update zone requirement times until a signal is found for which + // more path is needed + signalLoop@ while (pendingSignals.isNotEmpty()) { + val pathSignal = pendingSignals.first() + if (nextProbedZoneForSignal == -1) + nextProbedZoneForSignal = getSignalProtectedZone(pathSignal) + val physicalSignal = loadedSignalInfra.getPhysicalSignal(pathSignal.signal) + + // figure out when the signal is first seen + val sightOffset = pathSignal.pathOffset - rawInfra.getSignalSightDistance(physicalSignal) + val sightTime = callbacks.arrivalTimeInRange(sightOffset, pathSignal.pathOffset) + + // find the first zone after the signal which can be occupied without disturbing the train + var lastConstrainingZone = -1 + zoneProbingLoop@ while (true) { + // if we reached the last zone, just quit + if (nextProbedZoneForSignal == incrementalPath.endZonePathIndex) { + if (incrementalPath.pathComplete) + break@zoneProbingLoop + break@signalLoop + } + val zoneIndex = nextProbedZoneForSignal++ + + // find the index of this signal's block in the block array + val currentBlockOffset = pathSignal.minBlockPathIndex - incrementalPath.beginBlockIndex + assert(blocks[currentBlockOffset] == incrementalPath.getBlock(pathSignal.minBlockPathIndex)) + zoneStates[zoneIndex - zoneStartIndex] = ZoneStatus.OCCUPIED + val simulatedSignalStates = simulator.evaluate( + rawInfra, loadedSignalInfra, blockInfra, + blocks, 0, blocks.size, + zoneStates, ZoneStatus.CLEAR + ) + zoneStates[zoneIndex - zoneStartIndex] = ZoneStatus.CLEAR + val signalState = simulatedSignalStates[pathSignal.signal]!! + + // FIXME: Have a better way to check if the signal is constraining + if (signalState.getEnum("aspect") == "VL") + break + emitRequirementsUntil(zoneIndex, sightTime) + lastConstrainingZone = zoneIndex + } + + if (lastConstrainingZone == -1) { + logger.error { "signal ${rawInfra.getLogicalSignalName(pathSignal.signal)} does not react to zone occupation" } + } + pendingSignals.removeFirst() + nextProbedZoneForSignal = -1 + } + return res + } +} diff --git a/core/src/main/java/fr/sncf/osrd/standalone_sim/ScheduleMetadataExtractor.kt b/core/src/main/java/fr/sncf/osrd/standalone_sim/ScheduleMetadataExtractor.kt index 7bdd1b877e5..9603b9ca09b 100644 --- a/core/src/main/java/fr/sncf/osrd/standalone_sim/ScheduleMetadataExtractor.kt +++ b/core/src/main/java/fr/sncf/osrd/standalone_sim/ScheduleMetadataExtractor.kt @@ -2,7 +2,9 @@ package fr.sncf.osrd.standalone_sim + import fr.sncf.osrd.api.FullInfra +import fr.sncf.osrd.conflicts.* import fr.sncf.osrd.envelope.Envelope import fr.sncf.osrd.envelope.EnvelopePhysics import fr.sncf.osrd.envelope.EnvelopeTimeInterpolate @@ -27,9 +29,11 @@ import fr.sncf.osrd.utils.indexing.* import kotlin.collections.* import fr.sncf.osrd.utils.units.Distance import fr.sncf.osrd.utils.units.MutableDistanceArray +import fr.sncf.osrd.utils.units.Offset import fr.sncf.osrd.utils.units.meters import mu.KotlinLogging import kotlin.math.abs +import kotlin.math.absoluteValue import kotlin.math.max import kotlin.math.min @@ -104,6 +108,11 @@ fun run( // Compute signal updates val startOffset = trainPathBlockOffset(trainPath) + var blockPathLength = 0.meters + for (block in blockPath) + blockPathLength += blockInfra.getBlockLength(block).distance + val endOffset = blockPathLength - startOffset - (envelope.endPos - envelope.beginPos).meters + val pathSignals = pathSignalsInEnvelope(startOffset, blockPath, blockInfra, envelopeWithStops) val zoneOccupationChangeEvents = zoneOccupationChangeEvents(startOffset, blockPath, blockInfra, envelopeWithStops, rawInfra, trainLength) @@ -136,16 +145,15 @@ fun run( val mechanicalEnergyConsumed = EnvelopePhysics.getMechanicalEnergyConsumed(envelope, envelopePath, schedule.rollingStock) - val spacingRequirements = spacingRequirements( - simulator, - blockPath, - loadedSignalInfra, - blockInfra, - envelopeWithStops, - rawInfra, - pathSignals, - zoneOccupationChangeEvents - ) + val incrementalPath = incrementalPathOf(rawInfra, blockInfra) + val envelopeAdapter = IncrementalRequirementEnvelopeAdapter(incrementalPath, schedule.rollingStock, envelopeWithStops) + val spacingGenerator = SpacingRequirementAutomaton(rawInfra, loadedSignalInfra, blockInfra, simulator, envelopeAdapter, incrementalPath) + incrementalPath.extend(PathFragment( + routePath, blockPath, + containsStart = true, containsEnd = true, + startOffset, endOffset + )) + val spacingRequirements = spacingGenerator.processPathUpdate() val routingRequirements = routingRequirements( startOffset, @@ -173,55 +181,6 @@ fun run( ) } -enum class SpacingRequirementPhase { - HeadRoom, - Begin, - Main, - End, - TailRoom; - - /** Checks whether the current state accepts this zone configuration */ - fun check(occupied: Boolean, hasRequirement: Boolean): Boolean { - return when (this) { - HeadRoom -> !occupied && !hasRequirement - Begin -> occupied && !hasRequirement - Main -> occupied && hasRequirement - End -> !occupied && hasRequirement - TailRoom -> !occupied && !hasRequirement - } - } - - fun react(occupied: Boolean, hasRequirement: Boolean): SpacingRequirementPhase { - // no state change - if (check(occupied, hasRequirement)) - return this - - when (this) { - HeadRoom -> { - if (occupied) - return Begin.react(true, hasRequirement) - } - Begin -> { - if (hasRequirement) - return Main.react(occupied, true) - if (!occupied) - return TailRoom - } - Main -> { - if (!occupied) - return End.react(false, hasRequirement) - } - End -> { - if (!hasRequirement) - return TailRoom - - } - TailRoom -> return TailRoom - } - return this - } -} - private fun routingRequirements( // the start offset is the distance from the start of the first route to the start location startOffset: Distance, @@ -406,141 +365,6 @@ fun EnvelopeTimeInterpolate.clampInterpolate(position: Distance): Double { return interpolateTotalTime(criticalPos) } -private fun spacingRequirements( - simulator: SignalingSimulator, - blockPath: StaticIdxList, - loadedSignalInfra: LoadedSignalInfra, - blockInfra: BlockInfra, - envelope: EnvelopeTimeInterpolate, - rawInfra: SimInfraAdapter, - pathSignals: List, - zoneOccupationChangeEvents: MutableList -): List { - val res = mutableListOf() - data class ZoneOccupation(val entry: Double, val exit: Double) - val zoneOccupancies = zoneOccupationChangeEvents.groupBy { it.zone }.mapValues { (_, events) -> - assert(events.size <= 2) - assert(events[0].isEntry) - assert(events.size == 1 || !events[1].isEntry) - val entryTime = events.first().time / 1000.0 - val exitTime = if (events.size == 1) envelope.totalTime else events.last().time / 1000.0 - ZoneOccupation(entryTime, exitTime) - } - val zoneMap = arrayListOf() - var zoneCount = 0 - for (block in blockPath) { - for (zonePath in blockInfra.getBlockPath(block)) { - val zone = rawInfra.getNextZone(rawInfra.getZonePathEntry(zonePath))!! - zoneMap.add(zone) - zoneCount++ - } - } - - - val zoneRequirementTimes = DoubleArray(zoneCount) { Double.POSITIVE_INFINITY } - - for (pathSignal in pathSignals) { - val physicalSignal = loadedSignalInfra.getPhysicalSignal(pathSignal.signal) - val sightOffset = max(0.0, (pathSignal.pathOffset - rawInfra.getSignalSightDistance(physicalSignal)).meters) - val sightTime = envelope.interpolateTotalTime(sightOffset) - - val signalZoneOffset = - blockPath.take(pathSignal.minBlockPathIndex + 1).sumOf { blockInfra.getBlockPath(it).size } - - val zoneStates = ArrayList(zoneCount) - for (i in 0 until zoneCount) zoneStates.add(ZoneStatus.CLEAR) - - var lastConstrainingZone = -1 - for (i in signalZoneOffset until zoneCount) { - zoneStates[i] = ZoneStatus.OCCUPIED - val simulatedSignalStates = simulator.evaluate( - rawInfra, loadedSignalInfra, blockInfra, - blockPath, 0, blockPath.size, - zoneStates, ZoneStatus.CLEAR - ) - zoneStates[i] = ZoneStatus.CLEAR - val signalState = simulatedSignalStates[pathSignal.signal]!! - - // FIXME: Have a better way to check if the signal is constraining - if (signalState.getEnum("aspect") == "VL") - break - lastConstrainingZone = i - } - - if (lastConstrainingZone == -1) { - logger.error { "signal ${rawInfra.getLogicalSignalName(pathSignal.signal)} does not react to zone occupation" } - continue - } - - for (zoneIndex in signalZoneOffset .. lastConstrainingZone) { - val prevRequiredTime = zoneRequirementTimes[zoneIndex] - zoneRequirementTimes[zoneIndex] = min(sightTime, prevRequiredTime) - } - } - - /* - For all zones which either occupied by the train or required at some point, emit a zone requirement. - Some zones do not have requirements: those before the train's starting position, and those far enough from - the end of the train path. - - zone occupied Y Y - explicit zone requirement Y Y - zone needs requirement Y Y Y - signals ┎o ┎o ┎o ┎o ┎o ┎o - zones +----------|----------|----------|----------|----------|----------| - train path ============= - phase headroom begin main end tailroom - */ - - var phase = SpacingRequirementPhase.HeadRoom - for (zoneIndex in 0 until zoneCount) { - val zoneRequirementTime = zoneRequirementTimes[zoneIndex] - val zone = zoneMap[zoneIndex] - val zoneOccupancy = zoneOccupancies[zone] - - val explicitRequirement = zoneRequirementTime.isFinite() - val occupied = zoneOccupancy != null - - phase = phase.react(occupied, explicitRequirement) - val correctPhase = phase.check(occupied, explicitRequirement) - if (!correctPhase) - logger.error { "incorrect phase for zone $zoneIndex" } - - if (phase == SpacingRequirementPhase.HeadRoom || phase == SpacingRequirementPhase.TailRoom) - continue - - var beginTime: Double - var endTime: Double - - val zoneName = rawInfra.getZoneName(zone) - - when (phase) { - SpacingRequirementPhase.Begin -> { - beginTime = 0.0 - endTime = zoneOccupancy!!.exit - } - SpacingRequirementPhase.Main -> { - beginTime = if (zoneRequirementTime.isFinite()) { - zoneRequirementTime - } else { - // zones may not be required due to faulty signaling. - // in this case, fall back to the time at which the zone was first occupied - logger.error { "missing main phase zone requirement on zone $zoneName" } - zoneOccupancy!!.entry - } - endTime = zoneOccupancy!!.exit - } - else -> /* SpacingRequirementPhase.End */ { - assert(zoneRequirementTime.isFinite()) - beginTime = zoneRequirementTime - endTime = envelope.totalTime - } - } - - res.add(SpacingRequirement(zoneName, beginTime, endTime)) - } - return res -} private fun routeOccupancies( zoneOccupationChangeEvents: MutableList, @@ -632,10 +456,8 @@ private fun zoneOccupationChangeEvents( data class PathSignal( val signal: LogicalSignalId, val pathOffset: Distance, - // when a signal is between blocks, these two values will be different. - // for distant signals, minBlockPath index will be one less than maxBlockPathIndex + // when a signal is between blocks, prefer the index of the first block val minBlockPathIndex: Int, - val maxBlockPathIndex: Int, ) @@ -648,7 +470,6 @@ fun pathSignals( val pathSignals = mutableListOf() var currentOffset = -startOffset for ((blockIdx, block) in blockPath.withIndex()) { - val blockSize = blockInfra.getBlockLength(block).distance val blockSignals = blockInfra.getBlockSignals(block) val blockSignalPositions = blockInfra.getSignalsPositions(block) for (signalIndex in 0 until blockSignals.size) { @@ -658,13 +479,10 @@ fun pathSignals( continue val signal = blockSignals[signalIndex] val position = blockSignalPositions[signalIndex].distance - val dedupedSignal = blockIdx != blockPath.size - 1 && signalIndex == blockSignals.size - 1 - val maxBlockPathIndex = if (dedupedSignal) blockIdx + 1 else blockIdx - pathSignals.add(PathSignal(signal, currentOffset + position, blockIdx, maxBlockPathIndex)) + pathSignals.add(PathSignal(signal, currentOffset + position, blockIdx)) } - currentOffset += blockSize + currentOffset += blockInfra.getBlockLength(block).distance } - return pathSignals } diff --git a/core/src/main/java/fr/sncf/osrd/standalone_sim/result/ResultTrain.java b/core/src/main/java/fr/sncf/osrd/standalone_sim/result/ResultTrain.java index 99943a6caeb..480b149b461 100644 --- a/core/src/main/java/fr/sncf/osrd/standalone_sim/result/ResultTrain.java +++ b/core/src/main/java/fr/sncf/osrd/standalone_sim/result/ResultTrain.java @@ -74,6 +74,8 @@ public SpacingRequirement(String zone, double beginTime, double endTime) { this.endTime = endTime; assert !Double.isNaN(beginTime); assert !Double.isNaN(endTime); + assert Double.isFinite(beginTime); + assert Double.isFinite(endTime); } } diff --git a/core/src/test/kotlin/fr/sncf/osrd/sim_infra_adapter/PathPropertiesTests.kt b/core/src/test/kotlin/fr/sncf/osrd/sim_infra_adapter/PathPropertiesTests.kt index e836106bd21..0a368883846 100644 --- a/core/src/test/kotlin/fr/sncf/osrd/sim_infra_adapter/PathPropertiesTests.kt +++ b/core/src/test/kotlin/fr/sncf/osrd/sim_infra_adapter/PathPropertiesTests.kt @@ -341,4 +341,4 @@ class PathPropertiesTests { .forEach { dirChunk -> chunkList.add(dirChunk) } return buildPathPropertiesFrom(infra, chunkList, start, end) } -} \ No newline at end of file +}