public KeyValuePair <Vector3[], bool> FindPathAstar(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
PathNode startNode = pathGrid.GetPathNodeFromWorldPos(startPos);
PathNode targetNode = pathGrid.GetPathNodeFromWorldPos(targetPos);
startNode.parentNode = startNode;
if (startNode.walkeable && targetNode.walkeable)
{
HeapStruct <PathNode> openSet = new HeapStruct <PathNode>(pathGrid.MaxGridSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (PathNode neighbour in pathGrid.GetPathNodeNeighbours(currentNode))
{
if (!neighbour.walkeable || closedSet.Contains(neighbour))
{
continue;
}
int movementCost = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementCost;
if (movementCost < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = movementCost;
neighbour.huristicCost = GetDistance(neighbour, neighbour);
neighbour.parentNode = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathSuccess)
{
waypoints = ReCreatePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0;
}
sw.Stop();
//UnityEngine.Debug.Log("Needed " + sw.ElapsedMilliseconds + "ms to find and recreate the path.");
KeyValuePair <Vector3[], bool> returnvalue = new KeyValuePair <Vector3[], bool>(waypoints, pathSuccess);
return(returnvalue);
}