/// <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;
}
