diff --git a/src/main/java/com/simibubi/create/content/trains/entity/CarriageContraptionEntity.java b/src/main/java/com/simibubi/create/content/trains/entity/CarriageContraptionEntity.java index eeb5d679e..dc750775d 100644 --- a/src/main/java/com/simibubi/create/content/trains/entity/CarriageContraptionEntity.java +++ b/src/main/java/com/simibubi/create/content/trains/entity/CarriageContraptionEntity.java @@ -640,7 +640,7 @@ public class CarriageContraptionEntity extends OrientedContraptionEntity { if (lookAhead != null) { if (spaceDown) { carriage.train.manualTick = true; - nav.startNavigation(lookAhead, -1, false); + nav.startNavigation(nav.findPathTo(lookAhead, -1)); carriage.train.manualTick = false; navDistanceTotal = nav.distanceToDestination; return true; diff --git a/src/main/java/com/simibubi/create/content/trains/entity/Navigation.java b/src/main/java/com/simibubi/create/content/trains/entity/Navigation.java index b8fdd98d4..a27a52b7c 100644 --- a/src/main/java/com/simibubi/create/content/trains/entity/Navigation.java +++ b/src/main/java/com/simibubi/create/content/trains/entity/Navigation.java @@ -15,6 +15,8 @@ import java.util.UUID; import javax.annotation.Nullable; +import com.simibubi.create.content.trains.graph.DiscoveredPath; + import org.apache.commons.lang3.mutable.MutableDouble; import org.apache.commons.lang3.mutable.MutableObject; @@ -380,15 +382,11 @@ public class Navigation { train.reservedSignalBlocks.clear(); } - public double startNavigation(GlobalStation destination, double maxCost, boolean simulate) { - DiscoveredPath pathTo = findPathTo(destination, maxCost); + public double startNavigation(DiscoveredPath pathTo) { boolean noneFound = pathTo == null; double distance = noneFound ? -1 : Math.abs(pathTo.distance); double cost = noneFound ? -1 : pathTo.cost; - if (simulate) - return cost; - distanceToDestination = distance; if (noneFound) { @@ -407,10 +405,10 @@ public class Navigation { train.reservedSignalBlocks.clear(); train.navigation.waitingForSignal = null; - if (this.destination == null && !simulate) + if (this.destination == null) distanceStartedAt = distance; - if (this.destination == destination) + if (this.destination == pathTo.destination) return 0; if (!train.runtime.paused) { @@ -435,12 +433,19 @@ public class Navigation { train.status.foundConductor(); } - this.destination = destination; + this.destination = pathTo.destination; return cost; } @Nullable - private DiscoveredPath findPathTo(GlobalStation destination, double maxCost) { + public DiscoveredPath findPathTo(GlobalStation destination, double maxCost) { + ArrayList destinations = new ArrayList<>(); + destinations.add(destination); + return findPathTo(destinations, maxCost); + } + + @Nullable + public DiscoveredPath findPathTo(ArrayList destinations, double maxCost) { TrackGraph graph = train.graph; if (graph == null) return null; @@ -460,34 +465,36 @@ public class Navigation { : graph.getConnectionsFrom(initialPoint.node2) .get(initialPoint.node1); - search(Double.MAX_VALUE, maxCost, forward, (distance, cost, reachedVia, currentEntry, globalStation) -> { - if (globalStation != destination) - return false; + search(Double.MAX_VALUE, maxCost, forward, destinations, (distance, cost, reachedVia, currentEntry, globalStation) -> { + for (GlobalStation destination : destinations){ + if (globalStation == destination) { + TrackEdge edge = currentEntry.getSecond(); + TrackNode node1 = currentEntry.getFirst() + .getFirst(); + TrackNode node2 = currentEntry.getFirst() + .getSecond(); - TrackEdge edge = currentEntry.getSecond(); - TrackNode node1 = currentEntry.getFirst() - .getFirst(); - TrackNode node2 = currentEntry.getFirst() - .getSecond(); + List> currentPath = new ArrayList<>(); + Pair> backTrack = reachedVia.get(edge); + Couple toReach = Couple.create(node1, node2); + TrackEdge edgeReached = edge; + while (backTrack != null) { + if (edgeReached == initialEdge) + break; + if (backTrack.getFirst()) + currentPath.add(0, toReach); + toReach = backTrack.getSecond(); + edgeReached = graph.getConnection(toReach); + backTrack = reachedVia.get(edgeReached); + } - List> currentPath = new ArrayList<>(); - Pair> backTrack = reachedVia.get(edge); - Couple toReach = Couple.create(node1, node2); - TrackEdge edgeReached = edge; - while (backTrack != null) { - if (edgeReached == initialEdge) - break; - if (backTrack.getFirst()) - currentPath.add(0, toReach); - toReach = backTrack.getSecond(); - edgeReached = graph.getConnection(toReach); - backTrack = reachedVia.get(edgeReached); + double position = edge.getLength() - destination.getLocationOn(edge); + double distanceToDestination = distance - position; + results.set(forward, new DiscoveredPath((forward ? 1 : -1) * distanceToDestination, cost, currentPath, destination)); + return true; + } } - - double position = edge.getLength() - destination.getLocationOn(edge); - double distanceToDestination = distance - position; - results.set(forward, new DiscoveredPath((forward ? 1 : -1) * distanceToDestination, cost, currentPath)); - return true; + return false; }); } @@ -508,18 +515,6 @@ public class Navigation { return frontBetter ? front : back; } - public class DiscoveredPath { - List> path; - double distance; - double cost; - - public DiscoveredPath(double distance, double cost, List> path) { - this.distance = distance; - this.cost = cost; - this.path = path; - } - } - public GlobalStation findNearestApproachable(boolean forward) { TrackGraph graph = train.graph; if (graph == null) @@ -530,7 +525,7 @@ public class Navigation { double minDistance = .75f * (train.speed * train.speed) / (2 * acceleration); double maxDistance = Math.max(32, 1.5f * (train.speed * train.speed) / (2 * acceleration)); - search(maxDistance, forward, (distance, cost, reachedVia, currentEntry, globalStation) -> { + search(maxDistance, forward, null, (distance, cost, reachedVia, currentEntry, globalStation) -> { if (distance < minDistance) return false; @@ -548,11 +543,11 @@ public class Navigation { return result.getValue(); } - public void search(double maxDistance, boolean forward, StationTest stationTest) { - search(maxDistance, -1, forward, stationTest); + public void search(double maxDistance, boolean forward, ArrayList destinations, StationTest stationTest) { + search(maxDistance, -1, forward, destinations, stationTest); } - public void search(double maxDistance, double maxCost, boolean forward, StationTest stationTest) { + public void search(double maxDistance, double maxCost, boolean forward, ArrayList destinations, StationTest stationTest) { TrackGraph graph = train.graph; if (graph == null) return; @@ -614,10 +609,58 @@ public class Navigation { double distanceToNode2 = forward ? initialEdge.getLength() - startingPoint.position : startingPoint.position; - frontier.add(new FrontierEntry(distanceToNode2, 0, initialNode1, initialNode2, initialEdge)); int signalWeight = Mth.clamp(ticksWaitingForSignal * 2, Train.Penalties.RED_SIGNAL, 200); - Search: while (!frontier.isEmpty()) { + // Apply penalties to initial edge + int initialPenalty = 0; + if (costRelevant) + initialPenalty += penalties.getOrDefault(initialEdge, 0); + + EdgeData initialSignalData = initialEdge.getEdgeData(); + if (initialSignalData.hasPoints()) { + for (TrackEdgePoint point : initialSignalData.getPoints()) { + if (point.getLocationOn(initialEdge) < initialEdge.getLength() - distanceToNode2) + continue; + if (costRelevant && distanceToNode2 + initialPenalty > maxCost) + return; + if (!point.canNavigateVia(initialNode2)) + return; + if (point instanceof SignalBoundary signal) { + if (signal.isForcedRed(initialNode2)) { + initialPenalty += Train.Penalties.REDSTONE_RED_SIGNAL; + continue; + } + UUID group = signal.getGroup(initialNode2); + if (group == null) + continue; + SignalEdgeGroup signalEdgeGroup = Create.RAILWAYS.signalEdgeGroups.get(group); + if (signalEdgeGroup == null) + continue; + if (signalEdgeGroup.isOccupiedUnless(signal)) { + initialPenalty += signalWeight; + signalWeight /= 2; + } + } + if (point instanceof GlobalStation station) { + Train presentTrain = station.getPresentTrain(); + boolean isOwnStation = presentTrain == train; + if (presentTrain != null && !isOwnStation) + initialPenalty += Train.Penalties.STATION_WITH_TRAIN; + if (station.canApproachFrom(initialNode2) && stationTest.test(distanceToNode2, distanceToNode2 + initialPenalty, reachedVia, + Pair.of(Couple.create(initialNode1, initialNode2), initialEdge), station)) + return; + if (!isOwnStation) + initialPenalty += Train.Penalties.STATION; + } + } + } + + if (costRelevant && distanceToNode2 + initialPenalty > maxCost) + return; + + frontier.add(new FrontierEntry(distanceToNode2, initialPenalty, initialNode1, initialNode2, initialEdge)); + + while (!frontier.isEmpty()) { FrontierEntry entry = frontier.poll(); if (!visited.add(entry.edge)) continue; @@ -632,51 +675,19 @@ public class Navigation { TrackNode node1 = entry.node1; TrackNode node2 = entry.node2; - if (costRelevant) - penalty += penalties.getOrDefault(edge, 0); - - EdgeData signalData = edge.getEdgeData(); - if (signalData.hasPoints()) { - for (TrackEdgePoint point : signalData.getPoints()) { - if (node1 == initialNode1 && point.getLocationOn(edge) < edge.getLength() - distanceToNode2) - continue; - if (costRelevant && distance + penalty > maxCost) - continue Search; - if (!point.canNavigateVia(node2)) - continue Search; - if (point instanceof SignalBoundary signal) { - if (signal.isForcedRed(node2)) { - penalty += Train.Penalties.REDSTONE_RED_SIGNAL; - continue; + if (entry.hasDestination) { + EdgeData signalData = edge.getEdgeData(); + if (signalData.hasPoints()) { + for (TrackEdgePoint point : signalData.getPoints()) { + if (point instanceof GlobalStation station) { + if (station.canApproachFrom(node2) && stationTest.test(distance, penalty, reachedVia, + Pair.of(Couple.create(node1, node2), edge), station)) + return; } - UUID group = signal.getGroup(node2); - if (group == null) - continue; - SignalEdgeGroup signalEdgeGroup = Create.RAILWAYS.signalEdgeGroups.get(group); - if (signalEdgeGroup == null) - continue; - if (signalEdgeGroup.isOccupiedUnless(signal)) { - penalty += signalWeight; - signalWeight /= 2; - } - } - if (point instanceof GlobalStation station) { - Train presentTrain = station.getPresentTrain(); - boolean isOwnStation = presentTrain == train; - if (presentTrain != null && !isOwnStation) - penalty += Train.Penalties.STATION_WITH_TRAIN; - if (station.canApproachFrom(node2) && stationTest.test(distance, distance + penalty, reachedVia, - Pair.of(Couple.create(node1, node2), edge), station)) - return; - if (!isOwnStation) - penalty += Train.Penalties.STATION; } } } - if (costRelevant && distance + penalty > maxCost) - continue; - List> validTargets = new ArrayList<>(); Map connectionsFrom = graph.getConnectionsFrom(node2); for (Entry connection : connectionsFrom.entrySet()) { @@ -690,15 +701,101 @@ public class Navigation { if (validTargets.isEmpty()) continue; - for (Entry target : validTargets) { + Search: for (Entry target : validTargets) { if (!validTypes.contains(target.getValue().getTrackMaterial().trackType)) continue; TrackNode newNode = target.getKey(); TrackEdge newEdge = target.getValue(); - double newDistance = newEdge.getLength() + distance; int newPenalty = penalty; + double edgeLength = newEdge.getLength(); + double newDistance = distance + edgeLength; + + if (costRelevant) + newPenalty += penalties.getOrDefault(newEdge, 0); + + // Apply penalty to next connected edge + boolean hasDestination = false; + EdgeData signalData = newEdge.getEdgeData(); + if (signalData.hasPoints()) { + for (TrackEdgePoint point : signalData.getPoints()) { + if (node2 == initialNode1 && point.getLocationOn(newEdge) < edgeLength - distanceToNode2) + continue; + if (costRelevant && newDistance + newPenalty > maxCost) + continue Search; + if (!point.canNavigateVia(newNode)) + continue Search; + if (point instanceof SignalBoundary signal) { + if (signal.isForcedRed(newNode)) { + newPenalty += Train.Penalties.REDSTONE_RED_SIGNAL; + continue; + } + UUID group = signal.getGroup(newNode); + if (group == null) + continue; + SignalEdgeGroup signalEdgeGroup = Create.RAILWAYS.signalEdgeGroups.get(group); + if (signalEdgeGroup == null) + continue; + if (signalEdgeGroup.isOccupiedUnless(signal)) { + newPenalty += signalWeight; + signalWeight /= 2; + } + } + if (point instanceof GlobalStation station) { + Train presentTrain = station.getPresentTrain(); + boolean isOwnStation = presentTrain == train; + if (presentTrain != null && !isOwnStation) + newPenalty += Train.Penalties.STATION_WITH_TRAIN; + if (station.canApproachFrom(newNode) && stationTest.test(newDistance, newDistance + newPenalty, reachedVia, + Pair.of(Couple.create(node2, newNode), newEdge), station)) { + hasDestination = true; + continue; + } + if (!isOwnStation) + newPenalty += Train.Penalties.STATION; + } + } + } + + if (costRelevant && newDistance + newPenalty > maxCost) + continue; + + double remainingDist = 0; + // Calculate remaining distance estimator for next connected edge + if (destinations != null && !destinations.isEmpty()) { + remainingDist = Double.MAX_VALUE; + Vec3 newNodePosition = newNode.getLocation().getLocation(); + for (GlobalStation destination : destinations) { + TrackNodeLocation destinationNode = destination.edgeLocation.getFirst(); + double dMin = Math.abs(newNodePosition.x - destinationNode.getLocation().x); + double dMid = Math.abs(newNodePosition.y - destinationNode.getLocation().y); + double dMax = Math.abs(newNodePosition.z - destinationNode.getLocation().z); + // Sort distance vector in ascending order + double temp; + if (dMin > dMid) { + temp = dMid; + dMid = dMin; + dMin = temp; + } + if (dMin > dMax) { + temp = dMax; + dMax = dMin; + dMin = temp; + } + if (dMid > dMax) { + temp = dMax; + dMax = dMid; + dMid = temp; + } + // Octile distance from newNode to station node + double currentRemaining = 0.317837245195782 * dMin + 0.414213562373095 * dMid + dMax + destination.position; + if (node2.getLocation().equals(destinationNode)) + currentRemaining -= newEdge.getLength() * 2; // Correct the distance estimator for station edge + remainingDist = Math.min(remainingDist, currentRemaining); + } + } + reachedVia.putIfAbsent(newEdge, Pair.of(validTargets.size() > 1, Couple.create(node1, node2))); - frontier.add(new FrontierEntry(newDistance, newPenalty, node2, newNode, newEdge)); + frontier.add(new FrontierEntry(newDistance, newPenalty, remainingDist, hasDestination, node2, newNode, newEdge)); } } } @@ -707,6 +804,8 @@ public class Navigation { double distance; int penalty; + double remaining; + boolean hasDestination; TrackNode node1; TrackNode node2; TrackEdge edge; @@ -714,6 +813,17 @@ public class Navigation { public FrontierEntry(double distance, int penalty, TrackNode node1, TrackNode node2, TrackEdge edge) { this.distance = distance; this.penalty = penalty; + this.remaining = 0; + this.hasDestination = false; + this.node1 = node1; + this.node2 = node2; + this.edge = edge; + } + public FrontierEntry(double distance, int penalty, double remaining, boolean hasDestination, TrackNode node1, TrackNode node2, TrackEdge edge) { + this.distance = distance; + this.penalty = penalty; + this.remaining = remaining; + this.hasDestination = hasDestination; this.node1 = node1; this.node2 = node2; this.edge = edge; @@ -721,7 +831,7 @@ public class Navigation { @Override public int compareTo(FrontierEntry o) { - return Double.compare(distance + penalty, o.distance + o.penalty); + return Double.compare(distance + penalty + remaining, o.distance + o.penalty + o.remaining); } } diff --git a/src/main/java/com/simibubi/create/content/trains/entity/Train.java b/src/main/java/com/simibubi/create/content/trains/entity/Train.java index 612c8fd7c..ee73a2d89 100644 --- a/src/main/java/com/simibubi/create/content/trains/entity/Train.java +++ b/src/main/java/com/simibubi/create/content/trains/entity/Train.java @@ -17,6 +17,8 @@ import java.util.function.Consumer; import javax.annotation.Nullable; +import com.simibubi.create.content.trains.graph.DiscoveredPath; + import org.apache.commons.lang3.mutable.MutableBoolean; import org.apache.commons.lang3.mutable.MutableObject; @@ -539,14 +541,12 @@ public class Train { if (!reservedSignalBlocks.isEmpty()) return; - GlobalStation destination = navigation.destination; if (!navigatingManually && fullRefresh) { - GlobalStation preferredDestination = runtime.startCurrentInstruction(); - if (preferredDestination != null) - destination = preferredDestination; + DiscoveredPath preferredPath = runtime.startCurrentInstruction(); + if (preferredPath != null){ + navigation.startNavigation(preferredPath); + } } - - navigation.startNavigation(destination, navigatingManually ? -1 : Double.MAX_VALUE, false); } private void tickDerailedSlowdown() { @@ -1040,7 +1040,7 @@ public class Train { } public static class Penalties { - static final int STATION = 200, STATION_WITH_TRAIN = 300; + static final int STATION = 50, STATION_WITH_TRAIN = 300; static final int MANUAL_TRAIN = 200, IDLE_TRAIN = 700, ARRIVING_TRAIN = 50, WAITING_TRAIN = 50, ANY_TRAIN = 25, RED_SIGNAL = 25, REDSTONE_RED_SIGNAL = 400; } diff --git a/src/main/java/com/simibubi/create/content/trains/graph/DiscoveredPath.java b/src/main/java/com/simibubi/create/content/trains/graph/DiscoveredPath.java new file mode 100644 index 000000000..398b261e1 --- /dev/null +++ b/src/main/java/com/simibubi/create/content/trains/graph/DiscoveredPath.java @@ -0,0 +1,20 @@ +package com.simibubi.create.content.trains.graph; + +import com.simibubi.create.content.trains.station.GlobalStation; +import com.simibubi.create.foundation.utility.Couple; + +import java.util.List; + +public class DiscoveredPath { + public List> path; + public GlobalStation destination; + public double distance; + public double cost; + + public DiscoveredPath(double distance, double cost, List> path, GlobalStation destination) { + this.distance = distance; + this.cost = cost; + this.path = path; + this.destination = destination; + } +} diff --git a/src/main/java/com/simibubi/create/content/trains/schedule/ScheduleRuntime.java b/src/main/java/com/simibubi/create/content/trains/schedule/ScheduleRuntime.java index 5147edf96..85521e83a 100644 --- a/src/main/java/com/simibubi/create/content/trains/schedule/ScheduleRuntime.java +++ b/src/main/java/com/simibubi/create/content/trains/schedule/ScheduleRuntime.java @@ -4,11 +4,14 @@ import java.util.ArrayList; import java.util.Collection; import java.util.List; import java.util.Objects; +import java.util.regex.PatternSyntaxException; import com.simibubi.create.AllItems; import com.simibubi.create.content.trains.display.GlobalTrainDisplayData.TrainDeparturePrediction; import com.simibubi.create.content.trains.entity.Carriage; +import com.simibubi.create.content.trains.entity.Navigation; import com.simibubi.create.content.trains.entity.Train; +import com.simibubi.create.content.trains.graph.DiscoveredPath; import com.simibubi.create.content.trains.graph.EdgePointType; import com.simibubi.create.content.trains.schedule.condition.ScheduleWaitCondition; import com.simibubi.create.content.trains.schedule.condition.ScheduledDelay; @@ -122,17 +125,17 @@ public class ScheduleRuntime { return; } - GlobalStation nextStation = startCurrentInstruction(); - if (nextStation == null) + DiscoveredPath nextPath = startCurrentInstruction(); + if (nextPath == null) return; train.status.successfulNavigation(); - if (nextStation == train.getCurrentStation()) { + if (nextPath.destination == train.getCurrentStation()) { state = State.IN_TRANSIT; destinationReached(); return; } - if (train.navigation.startNavigation(nextStation, Double.MAX_VALUE, false) != TBD) { + if (train.navigation.startNavigation(nextPath) != TBD) { state = State.IN_TRANSIT; ticksInTransit = 0; } @@ -167,36 +170,31 @@ public class ScheduleRuntime { carriage.storage.tickIdleCargoTracker(); } - public GlobalStation startCurrentInstruction() { + public DiscoveredPath startCurrentInstruction() { ScheduleEntry entry = schedule.entries.get(currentEntry); ScheduleInstruction instruction = entry.instruction; if (instruction instanceof DestinationInstruction destination) { String regex = destination.getFilterForRegex(); - GlobalStation best = null; - double bestCost = Double.MAX_VALUE; boolean anyMatch = false; + ArrayList validStations = new ArrayList<>(); if (!train.hasForwardConductor() && !train.hasBackwardConductor()) { train.status.missingConductor(); cooldown = INTERVAL; return null; } - - for (GlobalStation globalStation : train.graph.getPoints(EdgePointType.STATION)) { - if (!globalStation.name.matches(regex)) - continue; - anyMatch = true; - boolean matchesCurrent = train.currentStation != null && train.currentStation.equals(globalStation.id); - double cost = matchesCurrent ? 0 : train.navigation.startNavigation(globalStation, bestCost, true); - if (cost < 0) - continue; - if (cost > bestCost) - continue; - best = globalStation; - bestCost = cost; - } - + + try { + for (GlobalStation globalStation : train.graph.getPoints(EdgePointType.STATION)) { + if (!globalStation.name.matches(regex)) + continue; + anyMatch = true; + validStations.add(globalStation); + } + } catch (PatternSyntaxException ignored) {} + + DiscoveredPath best = train.navigation.findPathTo(validStations, Double.MAX_VALUE); if (best == null) { if (anyMatch) train.status.failedNavigation(); @@ -348,7 +346,7 @@ public class ScheduleRuntime { predictions.add(createPrediction(index, filter.getFilter(), currentTitle, accumulatedTime)); - if (accumulatedTime != TBD) + if (accumulatedTime != TBD) accumulatedTime += departureTime; if (departureTime == INVALID) diff --git a/src/main/java/com/simibubi/create/content/trains/station/StationBlockEntity.java b/src/main/java/com/simibubi/create/content/trains/station/StationBlockEntity.java index 44657b81f..86318d1a1 100644 --- a/src/main/java/com/simibubi/create/content/trains/station/StationBlockEntity.java +++ b/src/main/java/com/simibubi/create/content/trains/station/StationBlockEntity.java @@ -13,6 +13,8 @@ import java.util.function.Consumer; import javax.annotation.Nullable; +import com.simibubi.create.content.trains.graph.DiscoveredPath; + import org.jetbrains.annotations.NotNull; import com.simibubi.create.AllBlocks; @@ -374,8 +376,8 @@ public class StationBlockEntity extends SmartBlockEntity implements ITransformab if (train.navigation.destination != station) continue; - GlobalStation preferredDestination = train.runtime.startCurrentInstruction(); - train.navigation.startNavigation(preferredDestination != null ? preferredDestination : station, Double.MAX_VALUE, false); + DiscoveredPath preferredPath = train.runtime.startCurrentInstruction(); + train.navigation.startNavigation(preferredPath != null ? preferredPath : train.navigation.findPathTo(station, Double.MAX_VALUE)); } }