/// <summary> /// Uses A* to find the distance from the startNode to the destinationNode. /// </summary> /// <param name="startNode">The starting waypoint.</param> /// <param name="destinationNode">The destination waypoint.</param> /// <param name="wrongTierPenalty">The penalty for the waypoints not being on the same tier.</param> /// <param name="emulatePodCarrying">Indicates whether to calculate a path that is ensured to be safe for carrying a pod on it.</param> /// <returns>The distance.</returns> private double CalculateShortestPath(Waypoint startNode, Waypoint destinationNode, double wrongTierPenalty, bool emulatePodCarrying) { if (startNode == null || destinationNode == null) { return(double.PositiveInfinity); } Dictionary <Waypoint, WaypointSearchDatapoint> openLocations = new Dictionary <Waypoint, WaypointSearchDatapoint>(); Dictionary <Waypoint, WaypointSearchDatapoint> closedLocations = new Dictionary <Waypoint, WaypointSearchDatapoint>(); openLocations[startNode] = new WaypointSearchDatapoint(0.0, startNode.GetDistance(destinationNode), startNode, null, 0); // Already at destination? if (startNode == destinationNode) { return(0); } // Loop until end is found while (true) { // Find lowest cost waypoint in openLocations KeyValuePair <Waypoint, WaypointSearchDatapoint> currentNodeKVP = openLocations.ArgMin(w => w.Value.DistanceTraveled + w.Value.DistanceToGoal); // Something wrong happened -can't find the end if (currentNodeKVP.Equals(default(KeyValuePair <Waypoint, WaypointSearchDatapoint>))) { return(double.PositiveInfinity); } Waypoint currentNode = currentNodeKVP.Key; // Grab the details about the current waypoint WaypointSearchDatapoint currentNodeData = currentNodeKVP.Value; // If the closest is also the destination or out of iterations if (currentNode == destinationNode) { // Return the travelled distance return(currentNodeData.DistanceTraveled); } // Transfer closest from open to closed list closedLocations[currentNode] = currentNodeData; openLocations.Remove(currentNode); // Expand all the moves foreach (var successorNode in currentNode.Paths) { // Check whether the node is already on closed if (closedLocations.ContainsKey(successorNode)) { // Don't deal with anything already on the closed list (don't want loops) continue; } // Can't use a pod storage location for the path if (emulatePodCarrying && successorNode.PodStorageLocation && successorNode != destinationNode) { continue; } // Tag on more distance for a node on the wrong level double additionalDistance = 0; if (successorNode.Tier != destinationNode.Tier) { additionalDistance += wrongTierPenalty; } // If it's not in the open list, add it if (!openLocations.ContainsKey(successorNode)) { openLocations[successorNode] = new WaypointSearchDatapoint( currentNodeData.DistanceTraveled + currentNode[successorNode], // The distance already traveled successorNode.GetDistance(destinationNode) + additionalDistance, // The approximate distance to the goal successorNode, // The node itself currentNodeData, // Parent data currentNodeData.Depth + 1); // The current depth } else { // It's already in the open list, but see if this new path is better WaypointSearchDatapoint oldPath = openLocations[successorNode]; // Update to the new path, if better if (oldPath.DistanceTraveled > currentNodeData.DistanceTraveled + currentNode[successorNode]) { oldPath.DistanceTraveled = currentNodeData.DistanceTraveled + currentNode[successorNode]; // The distance already traveled oldPath.DistanceToGoal = successorNode.GetDistance(destinationNode) + additionalDistance; // The approximate distance to the goal oldPath.Waypoint = successorNode; // The node itself oldPath.ParentMove = currentNodeData; // Parent data oldPath.Depth = currentNodeData.Depth + 1; // The current depth } } } } }
/// <summary> /// Creates a new waypoint search data object. /// </summary> /// <param name="distanceTraveled">The distance travelled so far.</param> /// <param name="distanceToGoal">The estimated distance towards the destination.</param> /// <param name="waypoint">The waypoint this data object belongs to.</param> /// <param name="parentMove">The data of the parent.</param> /// <param name="depth">The steps we did to come here.</param> public WaypointSearchDatapoint(double distanceTraveled, double distanceToGoal, Waypoint waypoint, WaypointSearchDatapoint parentMove, int depth) { DistanceTraveled = distanceTraveled; DistanceToGoal = distanceToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth; }