From 86278162259e2c7e16dc86ec14792dfffb6fb6be Mon Sep 17 00:00:00 2001 From: Erashin Date: Thu, 3 Oct 2024 17:06:41 +0200 Subject: [PATCH] core: use infra explorer in pathfinding endpoint Signed-off-by: Erashin --- .../osrd/sim_infra/impl/BlockInfraImpl.kt | 2 +- .../IncompatibleConstraintsPostProcessing.kt | 148 ++++++++++ .../PathfindingBlocksEndpointV2.kt | 271 +++++++----------- .../pathfinding/PathfindingBlocksEndpoint.kt | 53 +++- .../fr/sncf/osrd/graph/PathfindingGraph.kt | 72 +++++ .../fr/sncf/osrd/stdcm/graph/STDCMGraph.kt | 5 +- .../fr/sncf/osrd/stdcm/graph/STDCMUtils.kt | 30 +- 7 files changed, 389 insertions(+), 192 deletions(-) create mode 100644 core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/IncompatibleConstraintsPostProcessing.kt create mode 100644 core/src/main/kotlin/fr/sncf/osrd/graph/PathfindingGraph.kt 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 8d502309de1..ceab0408365 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 @@ -218,7 +218,7 @@ private fun buildBlockName( val detectors = mutableListOf() detectors.add(rawInfra.getZonePathEntry(descriptor.path[0])) detectors.add(rawInfra.getZonePathExit(descriptor.path.last())) - val detectorStr = detectors.map { "${rawInfra.getDetectorName(it.value)}" } + val detectorStr = detectors.map { rawInfra.getDetectorName(it.value) } val rawStringId = "$signals;$detectorStr;$trackNodeConfigNames" // We need to hash the result, these strings are way too long for identifiers diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/IncompatibleConstraintsPostProcessing.kt b/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/IncompatibleConstraintsPostProcessing.kt new file mode 100644 index 00000000000..6e7c760fbed --- /dev/null +++ b/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/IncompatibleConstraintsPostProcessing.kt @@ -0,0 +1,148 @@ +package fr.sncf.osrd.api.api_v2.pathfinding + +import fr.sncf.osrd.api.FullInfra +import fr.sncf.osrd.api.pathfinding.constraints.ElectrificationConstraints +import fr.sncf.osrd.api.pathfinding.constraints.LoadingGaugeConstraints +import fr.sncf.osrd.api.pathfinding.constraints.SignalingSystemConstraints +import fr.sncf.osrd.api.pathfinding.makePathProps +import fr.sncf.osrd.graph.* +import fr.sncf.osrd.sim_infra.api.Block +import fr.sncf.osrd.sim_infra.api.BlockId +import fr.sncf.osrd.utils.DistanceRangeMap +import fr.sncf.osrd.utils.distanceRangeMapOf +import fr.sncf.osrd.utils.filterIntersection +import fr.sncf.osrd.utils.units.Distance +import fr.sncf.osrd.utils.units.Offset + +fun buildIncompatibleConstraintsResponse( + infra: FullInfra, + possiblePathWithoutErrorNoConstraints: PathfindingResultId?, + constraints: Collection>, + initialRequest: PathfindingBlockRequest, +): IncompatibleConstraintsPathResponse? { + if ( + possiblePathWithoutErrorNoConstraints == null || + possiblePathWithoutErrorNoConstraints.ranges.isEmpty() + ) + return null + + val pathRanges = possiblePathWithoutErrorNoConstraints.ranges + val pathProps = + makePathProps(infra.rawInfra, infra.blockInfra, pathRanges.map { it.edge }, Offset.zero()) + + val elecConstraints = constraints.filterIsInstance() + assert(elecConstraints.size < 2) + val elecBlockedRangeValues = + getConstraintsDistanceRange( + infra, + pathRanges, + pathProps.getElectrification(), + elecConstraints.firstOrNull() + ) + .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), it.value) } + + val gaugeConstraints = constraints.filterIsInstance() + assert(gaugeConstraints.size < 2) + val gaugeBlockedRanges = + getConstraintsDistanceRange( + infra, + pathRanges, + pathProps.getLoadingGauge(), + gaugeConstraints.firstOrNull() + ) + .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), null) } + + val signalingSystemConstraints = constraints.filterIsInstance() + assert(signalingSystemConstraints.size < 2) + val blockList = pathRanges.map { it.edge } + val pathSignalingSystem = getPathSignalingSystems(infra, blockList) + val signalingSystemBlockedRangeValues = + getConstraintsDistanceRange( + infra, + pathRanges, + pathSignalingSystem, + signalingSystemConstraints.firstOrNull() + ) + .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), it.value) } + + if ( + listOf(elecBlockedRangeValues, gaugeBlockedRanges, signalingSystemBlockedRangeValues).all { + it.isEmpty() + } + ) { + return null + } + + return IncompatibleConstraintsPathResponse( + runPathfindingPostProcessing(infra, initialRequest, possiblePathWithoutErrorNoConstraints), + IncompatibleConstraints( + elecBlockedRangeValues, + gaugeBlockedRanges, + signalingSystemBlockedRangeValues + ) + ) +} + +private fun getConstraintsDistanceRange( + infra: FullInfra, + pathRanges: List>, + pathConstrainedValues: DistanceRangeMap, + constraint: PathfindingConstraint? +): DistanceRangeMap { + if (constraint == null) { + return distanceRangeMapOf() + } + + val blockedRanges = getBlockedRanges(infra, pathRanges, constraint) + val filteredRangeValues = filterIntersection(pathConstrainedValues, blockedRanges) + val travelledPathOffset = pathRanges.first().start.distance + filteredRangeValues.shiftPositions(-travelledPathOffset) + return filteredRangeValues +} + +private fun getBlockedRanges( + infra: FullInfra, + pathRanges: List>, + currentConstraint: PathfindingConstraint +): DistanceRangeMap { + val blockList = pathRanges.map { it.edge } + val blockedRanges = distanceRangeMapOf() + var startBlockPathOffset = Distance.ZERO + for (block in blockList) { + currentConstraint.apply(block).map { + blockedRanges.put( + startBlockPathOffset + it.start.distance, + startBlockPathOffset + it.end.distance, + true + ) + } + startBlockPathOffset += infra.blockInfra.getBlockLength(block).distance + } + blockedRanges.truncate( + pathRanges.first().start.distance, + startBlockPathOffset - infra.blockInfra.getBlockLength(blockList.last()).distance + + pathRanges.last().end.distance + ) + return blockedRanges +} + +private fun getPathSignalingSystems( + infra: FullInfra, + blockList: List +): DistanceRangeMap { + val pathSignalingSystem = distanceRangeMapOf() + var startBlockPathOffset = Distance.ZERO + for (block in blockList) { + val blockLength = infra.blockInfra.getBlockLength(block).distance + val blockSignalingSystemIdx = infra.blockInfra.getBlockSignalingSystem(block) + val blockSignalingSystemName = + infra.signalingSimulator.sigModuleManager.getName(blockSignalingSystemIdx) + pathSignalingSystem.put( + startBlockPathOffset, + startBlockPathOffset + blockLength, + blockSignalingSystemName + ) + startBlockPathOffset += blockLength + } + return pathSignalingSystem +} diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt b/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt index 2adacd07df7..e6b9359ae4e 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/api/api_v2/pathfinding/PathfindingBlocksEndpointV2.kt @@ -4,19 +4,20 @@ import fr.sncf.osrd.api.ExceptionHandler import fr.sncf.osrd.api.FullInfra import fr.sncf.osrd.api.InfraManager import fr.sncf.osrd.api.api_v2.TrackLocation +import fr.sncf.osrd.api.pathfinding.* import fr.sncf.osrd.api.pathfinding.constraints.* -import fr.sncf.osrd.api.pathfinding.makeHeuristics -import fr.sncf.osrd.api.pathfinding.makePathProps import fr.sncf.osrd.graph.* import fr.sncf.osrd.graph.Pathfinding.EdgeLocation +import fr.sncf.osrd.graph.Pathfinding.EdgeRange 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.sim_infra.api.* +import fr.sncf.osrd.stdcm.graph.extendLookaheadUntilWithoutEnvelope +import fr.sncf.osrd.stdcm.infra_exploration.initInfraExplorer import fr.sncf.osrd.utils.* import fr.sncf.osrd.utils.indexing.* -import fr.sncf.osrd.utils.units.Distance import fr.sncf.osrd.utils.units.Length import fr.sncf.osrd.utils.units.Offset import fr.sncf.osrd.utils.units.meters @@ -51,20 +52,20 @@ class PathfindingBlocksEndpointV2(private val infraManager: InfraManager) : Take val body = RqPrint(req).printBody() val request = pathfindingRequestAdapter.fromJson(body) - ?: return RsWithStatus(RsText("missing request body"), 400) + ?: return RsWithStatus(RsText("Missing request body"), 400) // Load infra val infra = infraManager.getInfra(request.infra, request.expectedVersion, recorder) val res = runPathfinding(infra, request) - pathfindingLogger.info("success") + pathfindingLogger.info("Success") RsJson(RsWithBody(pathfindingResponseAdapter.toJson(res))) } catch (error: NoPathFoundException) { - pathfindingLogger.info("no path found") + pathfindingLogger.info("No path found") RsJson(RsWithBody(pathfindingResponseAdapter.toJson(error.response))) } catch (error: OSRDError) { if (!error.osrdErrorType.isCacheable) { ExceptionHandler.handle(error) } else { - pathfindingLogger.info("pathfinding failed: ${error.message}") + pathfindingLogger.info("Pathfinding failed: ${error.message}") val response = PathfindingFailed(error) RsJson(RsWithBody(pathfindingResponseAdapter.toJson(response))) } @@ -75,7 +76,6 @@ class PathfindingBlocksEndpointV2(private val infraManager: InfraManager) : Take } /** Runs the pathfinding with the infra and request already parsed */ -@JvmName("runPathfinding") @Throws(OSRDError::class) fun runPathfinding( infra: FullInfra, @@ -100,7 +100,8 @@ fun runPathfinding( request.rollingStockSupportedSignalingSystems, ) - val heuristics = makeHeuristics(infra, waypoints, request.rollingStockMaximumSpeed) + val heuristics = + makeHeuristicsForPathfindingEdges(infra, waypoints, request.rollingStockMaximumSpeed) // Compute the paths from the entry waypoint to the exit waypoint val path = computePaths(infra, waypoints, constraints, heuristics, request, request.timeout) @@ -138,7 +139,7 @@ private fun computePaths( infra: FullInfra, waypoints: ArrayList>>, constraints: List>, - remainingDistanceEstimators: List>, + remainingDistanceEstimators: List>, initialRequest: PathfindingBlockRequest, timeout: Double?, ): PathfindingResultId { @@ -151,35 +152,105 @@ private fun computePaths( initialRequest.rollingStockLength ) val pathFound = - Pathfinding(GraphAdapter(infra.blockInfra, infra.rawInfra)) + Pathfinding(PathfindingGraph()) .setTimeout(timeout) - .setEdgeToLength { block: BlockId -> infra.blockInfra.getBlockLength(block) } + .setEdgeToLength { edge -> edge.length } .setRangeCost { range -> - mrspBuilder.getBlockTime(range.edge, range.end) - - mrspBuilder.getBlockTime(range.edge, range.start) + mrspBuilder.getBlockTime(range.edge.block, Offset(range.end.distance)) - + mrspBuilder.getBlockTime(range.edge.block, Offset(range.start.distance)) } .setRemainingDistanceEstimator(remainingDistanceEstimators) - .addBlockedRangeOnEdges(constraints) - .runPathfinding(waypoints) + .runPathfinding( + getStartLocations(infra.rawInfra, infra.blockInfra, waypoints, constraints), + getTargetsOnEdges(waypoints) + ) + if (pathFound != null) { - pathfindingLogger.info("path found in block graph, start postprocessing") - return pathFound + pathfindingLogger.info("Path found, start postprocessing") + return makeBlockPath(pathFound)!! } - pathfindingLogger.info("no path found, identifying issues") // Handling errors // Check if pathfinding failed due to incompatible constraints + pathfindingLogger.info("No path found, identifying issues") val elapsedSeconds = Duration.between(start, Instant.now()).toSeconds() + throwNoPathFoundException( + infra, + waypoints, + constraints, + mrspBuilder, + remainingDistanceEstimators, + initialRequest, + timeout?.minus(elapsedSeconds) + ) +} + +private fun getStartLocations( + rawInfra: RawSignalingInfra, + blockInfra: BlockInfra, + waypoints: ArrayList>>, + constraints: List>, +): Collection> { + val res = mutableListOf>() + val firstStep = waypoints[0] + val stops = listOf(waypoints.last()) + for (location in firstStep) { + val infraExplorers = initInfraExplorer(rawInfra, blockInfra, location, stops, constraints) + val extended = infraExplorers.flatMap { extendLookaheadUntilWithoutEnvelope(it, 3) } + for (explorer in extended) { + val edge = + PathfindingEdge(explorer, location.offset, blockInfra.getBlockLength(location.edge)) + res.add(EdgeLocation(edge, location.offset)) + } + } + return res +} + +private fun getTargetsOnEdges( + waypoints: ArrayList>>, +): List> { + val targetsOnEdges = ArrayList>() + for (i in 1 until waypoints.size) { + targetsOnEdges.add { edge: PathfindingEdge -> + val res = HashSet>() + for (target in waypoints[i]) { + if (target.edge == edge.block) res.add(EdgeLocation(edge, target.offset)) + } + res + } + } + return targetsOnEdges +} + +private fun throwNoPathFoundException( + infra: FullInfra, + waypoints: ArrayList>>, + constraints: Collection>, + mrspBuilder: CachedBlockMRSPBuilder, + remainingDistanceEstimators: List>, + initialRequest: PathfindingBlockRequest, + timeout: Double? +): Nothing { try { + val possiblePathWithoutErrorNoConstraints = + Pathfinding(PathfindingGraph()) + .setTimeout(timeout) + .setEdgeToLength { edge -> edge.length } + .setRangeCost { range -> + mrspBuilder.getBlockTime(range.edge.block, Offset(range.end.distance)) - + mrspBuilder.getBlockTime(range.edge.block, Offset(range.start.distance)) + } + .setRemainingDistanceEstimator(remainingDistanceEstimators) + .runPathfinding( + getStartLocations(infra.rawInfra, infra.blockInfra, waypoints, listOf()), + getTargetsOnEdges(waypoints) + ) val incompatibleConstraintsResponse = buildIncompatibleConstraintsResponse( infra, - waypoints, + makeBlockPath(possiblePathWithoutErrorNoConstraints), constraints, - remainingDistanceEstimators, - mrspBuilder, - initialRequest, - timeout?.minus(elapsedSeconds) + initialRequest ) if (incompatibleConstraintsResponse != null) { throw NoPathFoundException(incompatibleConstraintsResponse) @@ -194,154 +265,16 @@ private fun computePaths( throw NoPathFoundException(NotFoundInBlocks(listOf(), Length(0.meters))) } -private fun buildIncompatibleConstraintsResponse( - infra: FullInfra, - waypoints: ArrayList>>, - constraints: List>, - remainingDistanceEstimators: List>, - mrspBuilder: CachedBlockMRSPBuilder, - initialRequest: PathfindingBlockRequest, - timeout: Double? -): IncompatibleConstraintsPathResponse? { - val possiblePathWithoutErrorNoConstraints = - Pathfinding(GraphAdapter(infra.blockInfra, infra.rawInfra)) - .setTimeout(timeout) - .setEdgeToLength { block -> infra.blockInfra.getBlockLength(block) } - .setRangeCost { range -> - mrspBuilder.getBlockTime(range.edge, range.end) - - mrspBuilder.getBlockTime(range.edge, range.start) - } - .setRemainingDistanceEstimator(remainingDistanceEstimators) - .runPathfinding(waypoints) - - if ( - possiblePathWithoutErrorNoConstraints == null || - possiblePathWithoutErrorNoConstraints.ranges.isEmpty() - ) { - return null - } - - val pathRanges = possiblePathWithoutErrorNoConstraints.ranges - val pathProps = - makePathProps(infra.rawInfra, infra.blockInfra, pathRanges.map { it.edge }, Offset.zero()) - - val elecConstraints = constraints.filterIsInstance() - assert(elecConstraints.size < 2) - val elecBlockedRangeValues = - getConstraintsDistanceRange( - infra, - pathRanges, - pathProps.getElectrification(), - elecConstraints.firstOrNull() - ) - .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), it.value) } - - val gaugeConstraints = constraints.filterIsInstance() - assert(gaugeConstraints.size < 2) - val gaugeBlockedRanges = - getConstraintsDistanceRange( - infra, - pathRanges, - pathProps.getLoadingGauge(), - gaugeConstraints.firstOrNull() - ) - .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), null) } - - val signalingSystemConstraints = constraints.filterIsInstance() - assert(signalingSystemConstraints.size < 2) - val blockList = pathRanges.map { it.edge } - val pathSignalingSystem = getPathSignalingSystems(infra, blockList) - val signalingSystemBlockedRangeValues = - getConstraintsDistanceRange( - infra, - pathRanges, - pathSignalingSystem, - signalingSystemConstraints.firstOrNull() - ) - .map { RangeValue(Pathfinding.Range(Offset(it.lower), Offset(it.upper)), it.value) } - - if ( - listOf(elecBlockedRangeValues, gaugeBlockedRanges, signalingSystemBlockedRangeValues).all { - it.isEmpty() - } - ) { - return null - } - - return IncompatibleConstraintsPathResponse( - runPathfindingPostProcessing(infra, initialRequest, possiblePathWithoutErrorNoConstraints), - IncompatibleConstraints( - elecBlockedRangeValues, - gaugeBlockedRanges, - signalingSystemBlockedRangeValues - ) +private fun makeBlockPath( + path: Pathfinding.Result? +): PathfindingResultId? { + if (path == null) return null + return Pathfinding.Result( + path.ranges.map { EdgeRange(it.edge.block, it.start, it.end) }, + path.waypoints.map { EdgeLocation(it.edge.block, it.offset) } ) } -private fun getConstraintsDistanceRange( - infra: FullInfra, - pathRanges: List>, - pathConstrainedValues: DistanceRangeMap, - constraint: PathfindingConstraint? -): DistanceRangeMap { - if (constraint == null) { - return distanceRangeMapOf() - } - - val blockedRanges = getBlockedRanges(infra, pathRanges, constraint) - val filteredRangeValues = filterIntersection(pathConstrainedValues, blockedRanges) - val travelledPathOffset = pathRanges.first().start.distance - filteredRangeValues.shiftPositions(-travelledPathOffset) - return filteredRangeValues -} - -private fun getBlockedRanges( - infra: FullInfra, - pathRanges: List>, - currentConstraint: PathfindingConstraint -): DistanceRangeMap { - val blockList = pathRanges.map { it.edge } - val blockedRanges = distanceRangeMapOf() - var startBlockPathOffset = Distance.ZERO - for (block in blockList) { - currentConstraint.apply(block).map { - blockedRanges.put( - startBlockPathOffset + it.start.distance, - startBlockPathOffset + it.end.distance, - true - ) - } - startBlockPathOffset += infra.blockInfra.getBlockLength(block).distance - } - blockedRanges.truncate( - pathRanges.first().start.distance, - startBlockPathOffset - infra.blockInfra.getBlockLength(blockList.last()).distance + - pathRanges.last().end.distance - ) - return blockedRanges -} - -private fun getPathSignalingSystems( - infra: FullInfra, - blockList: List -): DistanceRangeMap { - val pathSignalingSystem = distanceRangeMapOf() - var startBlockPathOffset = Distance.ZERO - for (block in blockList) { - val blockLength = infra.blockInfra.getBlockLength(block).distance - val blockSignalingSystemIdx = infra.blockInfra.getBlockSignalingSystem(block) - val blockSignalingSystemName = - infra.signalingSimulator.sigModuleManager.getName(blockSignalingSystemIdx) - pathSignalingSystem.put( - startBlockPathOffset, - startBlockPathOffset + blockLength, - blockSignalingSystemName - ) - startBlockPathOffset += blockLength - } - return pathSignalingSystem -} - /** * Returns all the EdgeLocations of a waypoint. * diff --git a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt index 13d85535278..7a89e4b6980 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/api/pathfinding/PathfindingBlocksEndpoint.kt @@ -197,16 +197,6 @@ private fun assertPathTracksAreComplete( } } -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) && - (track.end >= waypoint.offset || abs(track.end - waypoint.offset) < 1e-3)) -} - /** Runs the pathfinding with the infra and rolling stocks already parsed */ @JvmName("runPathfinding") @Throws(OSRDError::class) @@ -276,6 +266,49 @@ fun makeHeuristics( return remainingDistanceEstimators } +/** Initialize the heuristics */ +fun makeHeuristicsForPathfindingEdges( + infra: FullInfra, + waypoints: List>>, + rollingStockMaxSpeed: Double, +): ArrayList> { + // Compute the minimum distance between steps + val stepMinDistance = Array(waypoints.size - 1) { 0.meters } + for (i in 0 until waypoints.size - 2) { + stepMinDistance[i] = + minDistanceBetweenSteps( + infra.blockInfra, + infra.rawInfra, + waypoints[i + 1], + waypoints[i + 2] + ) + } + + // Reversed cumulative sum + for (i in stepMinDistance.size - 2 downTo 0) { + stepMinDistance[i] += stepMinDistance[i + 1] + } + + // Setup estimators foreach intermediate steps + val remainingDistanceEstimators = ArrayList>() + for (i in 0 until waypoints.size - 1) { + val remainingDistanceEstimator = + RemainingDistanceEstimator( + infra.blockInfra, + infra.rawInfra, + waypoints[i + 1], + stepMinDistance[i] + ) + + // Now that the cost function is an approximation of the remaining time, + // we need to return the smallest possible remaining time here + remainingDistanceEstimators.add { edge, offset -> + remainingDistanceEstimator.apply(edge.block, offset).meters / rollingStockMaxSpeed + } + } + return remainingDistanceEstimators +} + @Throws(OSRDError::class) private fun computePaths( infra: FullInfra, diff --git a/core/src/main/kotlin/fr/sncf/osrd/graph/PathfindingGraph.kt b/core/src/main/kotlin/fr/sncf/osrd/graph/PathfindingGraph.kt new file mode 100644 index 00000000000..a52ea739c60 --- /dev/null +++ b/core/src/main/kotlin/fr/sncf/osrd/graph/PathfindingGraph.kt @@ -0,0 +1,72 @@ +package fr.sncf.osrd.graph + +import fr.sncf.osrd.sim_infra.api.Block +import fr.sncf.osrd.stdcm.graph.extendLookaheadUntilWithoutEnvelope +import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorer +import fr.sncf.osrd.utils.units.Length +import fr.sncf.osrd.utils.units.Offset +import fr.sncf.osrd.utils.units.meters +import java.util.Objects + +data class PathfindingNode( + val infraExplorer: InfraExplorer, + val previousEdge: PathfindingEdge, + val locationOnEdge: Offset?, +) + +data class PathfindingEdge( + val infraExplorer: InfraExplorer, + val startOffset: Offset, + val length: Length, +) { + val block = infraExplorer.getCurrentBlock() + + fun getEdgeEnd(): PathfindingNode { + return PathfindingNode( + infraExplorer, + this, + this.length, + ) + } + + override fun equals(other: Any?): Boolean { + return if (other !is PathfindingEdge) false + else + this.infraExplorer.getLastEdgeIdentifier() == + other.infraExplorer.getLastEdgeIdentifier() && + this.startOffset == other.startOffset && + this.length == other.length + } + + override fun hashCode(): Int { + return Objects.hash(infraExplorer.getLastEdgeIdentifier(), startOffset, length) + } +} + +class PathfindingGraph : Graph { + override fun getEdgeEnd(edge: PathfindingEdge): PathfindingNode { + return edge.getEdgeEnd() + } + + override fun getAdjacentEdges(node: PathfindingNode): Collection { + val res = ArrayList() + val extended = mutableListOf() + if (node.infraExplorer.getLookahead().size > 0) { + extended.add(node.infraExplorer) + } else { + extended.addAll(extendLookaheadUntilWithoutEnvelope(node.infraExplorer, 1)) + } + for (newPath in extended) { + if (newPath.getLookahead().size == 0) continue + newPath.moveForward() + val newEdge = + PathfindingEdge( + newPath, + Offset(0.meters), + Length(newPath.getCurrentBlockLength().distance) + ) + res.add(newEdge) + } + return res + } +} diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMGraph.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMGraph.kt index b7b492d3bf3..074ec90ad49 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMGraph.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMGraph.kt @@ -132,7 +132,10 @@ class STDCMGraph( ) if (visitedNodes.isVisited(visitedNodesParameters)) return listOf() visitedNodes.markAsVisited(visitedNodesParameters) - res.addAll(STDCMEdgeBuilder.fromNode(this, node, newPath).makeAllEdges()) + res.addAll( + STDCMEdgeBuilder.fromNode(this, node, newPath) + .makeAllEdges() + ) } } return res diff --git a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMUtils.kt b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMUtils.kt index 40ea763f2e2..b0794240015 100644 --- a/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMUtils.kt +++ b/core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMUtils.kt @@ -3,6 +3,7 @@ package fr.sncf.osrd.stdcm.graph import fr.sncf.osrd.sim_infra.api.* import fr.sncf.osrd.sim_infra.impl.ChunkPath import fr.sncf.osrd.sim_infra.impl.buildChunkPath +import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorer import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorerWithEnvelope import fr.sncf.osrd.utils.indexing.MutableDirStaticIdxArrayList import fr.sncf.osrd.utils.units.Distance @@ -54,25 +55,17 @@ fun makeChunkPathFromEdges(graph: STDCMGraph, edges: List): ChunkPath return buildChunkPath(graph.rawInfra, chunks, firstOffset, lastOffset) } -/** Converts an offset on a block into an offset on its STDCM edge */ -fun convertOffsetToEdge( - blockOffset: Offset, - envelopeStartOffset: Offset -): Offset { - return Offset(blockOffset.distance - envelopeStartOffset.distance) -} - /** * Extends all the given infra explorers until they have the min amount of blocks in lookahead, or * they reach the destination. The min number of blocks is arbitrary, it should aim for the required * lookahead for proper spacing resource generation. If the value is too low, there would be - * exceptions thrown and we would try again with an extended path. If it's too large, we would + * exceptions thrown, and we would try again with an extended path. If it's too large, we would * "fork" too early. Either way the result wouldn't change, it's just a matter of performances. */ fun extendLookaheadUntil( input: InfraExplorerWithEnvelope, - minBlocks: Int -): Collection { + minBlocks: Int) +: Collection { val res = mutableListOf() val candidates = mutableListOf(input) while (candidates.isNotEmpty()) { @@ -86,3 +79,18 @@ fun extendLookaheadUntil( } return res } + +fun extendLookaheadUntilWithoutEnvelope(input: InfraExplorer, minBlocks: Int): Collection { + val res = mutableListOf() + val candidates = mutableListOf(input) + while (candidates.isNotEmpty()) { + val candidate = candidates.removeFirst() + if ( + candidate.getIncrementalPath().pathComplete || + candidate.getLookahead().size >= minBlocks + ) + res.add(candidate) + else candidates.addAll(candidate.cloneAndExtendLookahead()) + } + return res +}