public List <Node> FindPath(Vector3 sPos, Vector3 ePos)
{
List <Node> openList = new List <Node>();
List <Node> closedList = new List <Node>();
Node startNode = grid.NodeFromWorldPoint(sPos);
Node endNode = grid.NodeFromWorldPoint(ePos);
startNode.gCost = 0;
startNode.hCost = ComputeDistance(startNode, endNode);
startNode.parent = null;
openList.Add(startNode);
while (openList.Count != 0)
{
Node currentNode = NodeWithMinimumCost(openList);
// We reached the end
if (currentNode == endNode)
{
return(MakePath(startNode, endNode));
}
//Moving current node from open to closed list
openList.Remove(currentNode);
closedList.Add(currentNode);
List <Node> neighbourList = grid.GetNeighbourNodes(currentNode);
foreach (Node neighbour in neighbourList)
{
// Skipping already seen nodes AND obstacles nodes
if (closedList.Contains(neighbour) || neighbour.isSolid)
{
continue;
}
// Computing current cost and changing values if needed
int gCost = currentNode.gCost + ComputeDistance(neighbour, currentNode);
int hCost = ComputeDistance(neighbour, endNode);
if (!openList.Contains(neighbour))
{
neighbour.gCost = gCost;
neighbour.hCost = hCost;
neighbour.parent = currentNode;
openList.Add(neighbour);
}
else
{
int fCost = gCost + hCost;
if (fCost < neighbour.fCost)
{
neighbour.gCost = gCost;
neighbour.hCost = hCost;
neighbour.parent = currentNode;
}
}
}
}
return(new List <Node>());
}
