/// <summary>
/// Uses A* to find the distance from the startNode to the destinationNode.
/// </summary>
/// <param name="start">The starting waypoint.</param>
/// <param name="destination">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 start, Waypoint destination, double wrongTierPenalty, bool emulatePodCarrying)
{
if (start == null || destination == null)
{
return(double.PositiveInfinity);
}
if (start == destination)
{
return(0);
}
// Determine shortest time
double shortestTime = double.PositiveInfinity;
foreach (var startNode in _timeGraph.TimeWaypointsOfOriginalWaypoints[start])
{
Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint> openLocations = new Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint>();
Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint> closedLocations = new Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint>();
openLocations[startNode] = new TimeWaypointSearchDatapoint(0.0, EstimateTravelTime(startNode, destination), startNode, null, 0);
// Loop until end is found
while (true)
{
// Find lowest cost waypoint in openLocations
KeyValuePair <TimeWaypoint, TimeWaypointSearchDatapoint> currentNodeKVP = openLocations.ArgMin(w => w.Value.TimeTraveled + w.Value.TimeToGoal);
// Something wrong happened -can't find the end
if (currentNodeKVP.Equals(default(KeyValuePair <TimeWaypoint, TimeWaypointSearchDatapoint>)))
{
break;
}
TimeWaypoint currentNode = currentNodeKVP.Key;
// Grab the details about the current waypoint
TimeWaypointSearchDatapoint currentNodeData = currentNodeKVP.Value;
// If the closest is also the destination or out of iterations
if (currentNode.OriginalWaypoint == destination)
{
// Update traveled time
shortestTime = Math.Min(shortestTime, currentNodeData.TimeTraveled);
break;
}
// Transfer closest from open to closed list
closedLocations[currentNode] = currentNodeKVP.Value;
openLocations.Remove(currentNode);
// Expand all the moves
foreach (var edge in currentNode.Edges)
{
// Check whether the node is already on closed
if (closedLocations.ContainsKey(edge.Item1))
{
// 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 && edge.Item1.OriginalWaypoint.PodStorageLocation && edge.Item1.OriginalWaypoint != destination)
{
continue;
}
// Tag on more distance for a node on the wrong level
double additionalDistance = 0;
if (edge.Item1.OriginalWaypoint.Tier != destination.Tier)
{
additionalDistance += wrongTierPenalty;
}
// If it's not in the open list, add it
if (!openLocations.ContainsKey(edge.Item1))
{
openLocations[edge.Item1] =
new TimeWaypointSearchDatapoint(
currentNodeData.TimeTraveled + edge.Item2, // The distance already traveled
EstimateTravelTime(edge.Item1, destination) + additionalDistance, // The approximate distance to the goal
edge.Item1, // 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
TimeWaypointSearchDatapoint oldPath = openLocations[edge.Item1];
// Replace it with the new one, if better
if (oldPath.TimeTraveled > currentNodeData.TimeTraveled + edge.Item2)
{
oldPath.TimeTraveled = currentNodeData.TimeTraveled + edge.Item2; // The distance already traveled
oldPath.TimeToGoal = EstimateTravelTime(edge.Item1, destination) + additionalDistance; // The approximate distance to the goal
oldPath.Waypoint = edge.Item1; // The node itself
oldPath.ParentMove = currentNodeData; // Parent data
oldPath.Depth = currentNodeData.Depth + 1; // The current depth
}
}
}
}
}
// Return time
return(shortestTime);
}
/// <summary>
/// Creates a new waypoint search data object.
/// </summary>
/// <param name="timeTraveled">The time traveled so far.</param>
/// <param name="timeToGoal">The estimated time 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 TimeWaypointSearchDatapoint(double timeTraveled, double timeToGoal, TimeWaypoint waypoint, TimeWaypointSearchDatapoint parentMove, int depth)
{
TimeTraveled = timeTraveled; TimeToGoal = timeToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth;
}
