public WaypointSearchData(double distanceTraveled, double distanceToGoal, DTOWaypoint waypoint, WaypointSearchData parentMove, int depth)
{
DistanceTraveled = distanceTraveled; DistanceToGoal = distanceToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth;
}
public WaypointSearchResult PlanPath(DTOWaypoint startNode, DTOWaypoint destinationNode)
{
Dictionary <DTOWaypoint, WaypointSearchData> openLocations = new Dictionary <DTOWaypoint, WaypointSearchData>();
Dictionary <DTOWaypoint, WaypointSearchData> closedLocations = new Dictionary <DTOWaypoint, WaypointSearchData>();
openLocations[startNode] = new WaypointSearchData(0.0, GetDistance(startNode, destinationNode), startNode, null, 0);
// Don't move if already at destination
if (startNode == destinationNode)
{
return(null);
}
// Maximum number of waypoints to look at in search
int maxNumIterations = 3000;
int numIterations = 0;
// Loop until end is found
while (true)
{
// Find lowest cost waypoint in openLocations
DTOWaypoint currentNode = null;
double lowestCost = double.PositiveInfinity;
foreach (var w in openLocations.Keys)
{
if (openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal < lowestCost)
{
currentNode = w;
lowestCost = openLocations[w].DistanceTraveled + openLocations[w].DistanceToGoal;
}
}
// Something wrong happened -can't find the end
if (currentNode == null)
{
return(null);
}
// Grab the details about the current waypoint
WaypointSearchData currentNodeData = openLocations[currentNode];
// If the closest is also the destination or out of iterations
if (currentNode == destinationNode || numIterations++ == maxNumIterations)
{
// Init result
WaypointSearchResult result = new WaypointSearchResult();
// Found it on the first move
if (currentNodeData.ParentMove == null)
{
return(null);
}
// Go back to the first move made
while (currentNodeData != null)
{
result.AddStep(currentNodeData.Waypoint);
currentNodeData = currentNodeData.ParentMove;
}
return(result);
}
// Transfer closest from open to closed list
closedLocations[currentNode] = openLocations[currentNode];
openLocations.Remove(currentNode);
// Expand all the moves
foreach (var successorNode in currentNode.Paths.Select(p => _waypoints[p]))
{
// 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;
}
// If it's not in the open list, add it
if (!openLocations.ContainsKey(successorNode))
{
openLocations[successorNode] =
new WaypointSearchData(
currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled
GetDistance(successorNode, destinationNode), // 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
WaypointSearchData oldPath = openLocations[successorNode];
// Replace it with the new one
if (oldPath.DistanceTraveled + oldPath.DistanceToGoal > currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode))
{
openLocations[successorNode] =
new WaypointSearchData(
currentNodeData.DistanceTraveled + GetDistance(currentNode, successorNode), // The distance already traveled
GetDistance(successorNode, destinationNode), // The approximate distance to the goal
successorNode, // The node itself
currentNodeData, // Parent data
currentNodeData.Depth + 1); // The current depth
}
}
}
}
}
