Multiple stations 1 call

src/main/java/com/simibubi/create/content/trains/entity/CarriageContraptionEntity.java

@@ -1,6 +1,7 @@
 package com.simibubi.create.content.trains.entity;
 
 import java.lang.ref.WeakReference;
+import java.util.ArrayList;
 import java.util.Collection;
 import java.util.HashSet;
 import java.util.List;
@@ -54,6 +55,8 @@ import net.minecraftforge.api.distmarker.Dist;
 import net.minecraftforge.api.distmarker.OnlyIn;
 import net.minecraftforge.network.PacketDistributor;
 
+import org.openjdk.nashorn.internal.objects.Global;
+
 public class CarriageContraptionEntity extends OrientedContraptionEntity {
 
 	private static final EntityDataAccessor<CarriageSyncData> CARRIAGE_DATA =

src/main/java/com/simibubi/create/content/trains/entity/Navigation.java

@@ -439,6 +439,13 @@ public class Navigation {
 
 	@Nullable
 	public DiscoveredPath findPathTo(GlobalStation destination, double maxCost) {
+		ArrayList<GlobalStation> destinations = new ArrayList<>();
+		destinations.add(destination);
+		return findPathTo(destinations, maxCost);
+	}
+
+	@Nullable
+	public DiscoveredPath findPathTo(ArrayList<GlobalStation> destinations, double maxCost) {
 		TrackGraph graph = train.graph;
 		if (graph == null)
 			return null;
@@ -458,10 +465,9 @@ public class Navigation {
 				: graph.getConnectionsFrom(initialPoint.node2)
 					.get(initialPoint.node1);
 
-			search(Double.MAX_VALUE, maxCost, forward, destination, (distance, cost, reachedVia, currentEntry, globalStation) -> {
+			search(Double.MAX_VALUE, maxCost, forward, destinations, (distance, cost, reachedVia, currentEntry, globalStation) -> {
-				if (globalStation != destination)
+				for (GlobalStation destination : destinations){
-					return false;
+					if (globalStation == destination) {
-
 						TrackEdge edge = currentEntry.getSecond();
 						TrackNode node1 = currentEntry.getFirst()
 								.getFirst();
@@ -486,6 +492,9 @@ public class Navigation {
 						double distanceToDestination = distance - position;
 						results.set(forward, new DiscoveredPath((forward ? 1 : -1) * distanceToDestination, cost, currentPath, destination));
 						return true;
+					}
+				}
+				return false;
 			});
 		}
 
@@ -534,18 +543,15 @@ public class Navigation {
 		return result.getValue();
 	}
 
-	public void search(double maxDistance, boolean forward, GlobalStation destination, StationTest stationTest) {
+	public void search(double maxDistance, boolean forward, ArrayList<GlobalStation> destinations, StationTest stationTest) {
-		search(maxDistance, -1, forward, destination, stationTest);
+		search(maxDistance, -1, forward, destinations, stationTest);
 	}
 
-	public void search(double maxDistance, double maxCost, boolean forward, GlobalStation destination, StationTest stationTest) {
+	public void search(double maxDistance, double maxCost, boolean forward, ArrayList<GlobalStation> destinations, StationTest stationTest) {
 		TrackGraph graph = train.graph;
 		if (graph == null)
 			return;
 
-		// Cache the position of a node on the station edge if provided
-		Vec3 destinationNodePosition = destination == null ? null : destination.edgeLocation.getSecond().getLocation();
-
 		// Cache the list of track types that the train can travel on
 		Set<TrackMaterial.TrackType> validTypes = new HashSet<>();
 		for (int i = 0; i < train.carriages.size(); i++) {
@@ -603,10 +609,57 @@ 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()) {
+		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, false, initialNode1, initialNode2, initialEdge));
+
+		while (!frontier.isEmpty()) {
 			FrontierEntry entry = frontier.poll();
 			if (!visited.add(entry.edge))
 				continue;
@@ -621,50 +674,18 @@ public class Navigation {
 			TrackNode node1 = entry.node1;
 			TrackNode node2 = entry.node2;
 
-			if (costRelevant)
+			if (entry.hasDestination) {
-				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;
-						}
-						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();
+							if (station.canApproachFrom(node2) && stationTest.test(distance, penalty, reachedVia,
-						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<Entry<TrackNode, TrackEdge>> validTargets = new ArrayList<>();
 			Map<TrackNode, TrackEdge> connectionsFrom = graph.getConnectionsFrom(node2);
@@ -679,19 +700,73 @@ public class Navigation {
 			if (validTargets.isEmpty())
 				continue;
 
-			for (Entry<TrackNode, TrackEdge> target : validTargets) {
+			Search: for (Entry<TrackNode, TrackEdge> 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);
+
+				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;
 
-				if (destination != null) {
+				if (destinations != null && !destinations.isEmpty()) {
+					remainingDist = Double.MAX_VALUE;
 					Vec3 newNodePosition = newNode.getLocation().getLocation();
-					double dMin = Math.abs(newNodePosition.x - destinationNodePosition.x);
+					for (GlobalStation destination : destinations) {
-					double dMid = Math.abs(newNodePosition.y - destinationNodePosition.y);
+						TrackNodeLocation destinationNode = destination.edgeLocation.getFirst();
-					double dMax = Math.abs(newNodePosition.z - destinationNodePosition.z);
+						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) {
@@ -710,14 +785,15 @@ public class Navigation {
 							dMid = temp;
 						}
 						// Octile distance from newNode to station node
-					remainingDist = 0.317837245 * dMin + 0.414213562 * dMid + dMax;
+						double currentRemaining = 0.317837245 * dMin + 0.414213562 * 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);
+					}
 				}
 
-				if (destination != null && Math.round(remainingDist) == 0)
-					remainingDist = -999999; // Ensure edges containing the station node get checked first
-
 				reachedVia.putIfAbsent(newEdge, Pair.of(validTargets.size() > 1, Couple.create(node1, node2)));
-				frontier.add(new FrontierEntry(newDistance, penalty, remainingDist, node2, newNode, newEdge));
+				frontier.add(new FrontierEntry(newDistance, newPenalty, remainingDist, hasDestination, node2, newNode, newEdge));
 			}
 		}
 	}
@@ -727,22 +803,25 @@ public class Navigation {
 		double distance;
 		int penalty;
 		double remaining;
+		boolean hasDestination;
 		TrackNode node1;
 		TrackNode node2;
 		TrackEdge edge;
 
-		public FrontierEntry(double distance, int penalty, TrackNode node1, TrackNode node2, TrackEdge edge) {
+		public FrontierEntry(double distance, int penalty, boolean hasDestination, TrackNode node1, TrackNode node2, TrackEdge edge) {
 			this.distance = distance;
 			this.penalty = penalty;
 			this.remaining = 0;
+			this.hasDestination = hasDestination;
 			this.node1 = node1;
 			this.node2 = node2;
 			this.edge = edge;
 		}
-		public FrontierEntry(double distance, int penalty, double remaining, TrackNode node1, TrackNode node2, TrackEdge 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;

src/main/java/com/simibubi/create/content/trains/graph/DiscoveredPath.java

@@ -3,8 +3,6 @@ package com.simibubi.create.content.trains.graph;
 import com.simibubi.create.content.trains.station.GlobalStation;
 import com.simibubi.create.foundation.utility.Couple;
 
-import org.openjdk.nashorn.internal.objects.Global;
-
 import java.util.List;
 
 public class DiscoveredPath {

src/main/java/com/simibubi/create/content/trains/schedule/ScheduleRuntime.java

@@ -175,9 +175,8 @@ public class ScheduleRuntime {
 
 		if (instruction instanceof DestinationInstruction destination) {
 			String regex = destination.getFilterForRegex();
-			DiscoveredPath best = null;
-			double bestCost = Double.MAX_VALUE;
 			boolean anyMatch = false;
+			ArrayList<GlobalStation> validStations = new ArrayList<>();
 
 			if (!train.hasForwardConductor() && !train.hasBackwardConductor()) {
 				train.status.missingConductor();
@@ -189,23 +188,9 @@ public class ScheduleRuntime {
 				if (!globalStation.name.matches(regex))
 					continue;
 				anyMatch = true;
-				boolean matchesCurrent = train.currentStation != null && train.currentStation.equals(globalStation.id);
+				validStations.add(globalStation);
-				double cost;
-				DiscoveredPath path = train.navigation.findPathTo(globalStation, bestCost);
-				if (matchesCurrent) {
-					cost = 0;
-				} else {
-					cost = path == null ? -1 : path.cost;
 			}
-
+			DiscoveredPath best = train.navigation.findPathTo(validStations, Double.MAX_VALUE);
-				if (cost < 0)
-					continue;
-				if (cost > bestCost)
-					continue;
-				best = path;
-				bestCost = cost;
-			}
-
 			if (best == null) {
 				if (anyMatch)
 					train.status.failedNavigation();