private GridNodeRecord GetSmallestElement(List <GridNodeRecord> openList)
{
float minDistance = Mathf.Infinity;
GridNodeRecord targetNode = new GridNodeRecord();
for (int i = 0; i < openList.Count; i++)
{
if (openList[i].estimatedTotalCost < minDistance)
{
minDistance = openList[i].estimatedTotalCost;
targetNode = openList[i];
}
}
open.Remove(targetNode);
return(targetNode);
}
private List <GridNodeRecord> Astar(GridNode start, GridNode end)
{
//Initialize start node
GridNodeRecord startRecord = new GridNodeRecord();
startRecord.node = start;
startRecord.connection = null;
startRecord.costSoFar = 0;
startRecord.estimatedTotalCost = 0;
//Initialize the open and closed lists
open = new List <GridNodeRecord>();
open.Add(startRecord);
closed = new List <GridNodeRecord>();
//Find the smallest element in the open list with estimated total cost
GridNodeRecord current = new GridNodeRecord();
//Iterate through processing each node
while (open.Count > 0)
{
current = GetSmallestElement(open);
closed.Add(current);
//If it is the goal node then terminate
if (current.node.nodeIndex == end.nodeIndex)
{
break;
}
//Otherwise get the connections
List <GridNodeConnection> connections = RetrieveConnections(current.node);
//Loop through each connection in turn
for (int i = 0; i < connections.Count; i++)
{
//Get the cost estimate for the end node
GridNode toNode = connections[i].toNode;
float toNodeCost = current.costSoFar + connections[i].cost;
GridNodeRecord endNodeRecord = new GridNodeRecord();
if (Contains(closed, toNode))
{
continue;
}
else
{
endNodeRecord.node = toNode;
}
//Update the node
endNodeRecord.costSoFar = toNodeCost;
endNodeRecord.connection = connections[i];
endNodeRecord.estimatedTotalCost = toNodeCost + Vector3.Distance(toNode.position, end.position);
if (!Contains(open, toNode))
{
open.Add(endNodeRecord);
}
}
}
if (current.node.nodeIndex != end.nodeIndex)
{
return(null);
}
else
{
List <GridNodeRecord> path = new List <GridNodeRecord>();
while (current.node.nodeIndex != startRecord.node.nodeIndex)
{
path.Add(current);
current = Find(closed, current.connection.fromNode);
}
path.Reverse();
DisplayPath(path);
return(path);
}
}