//path calculation
void FindPath(Vector3 startPos, Vector3 TargetPos)
{
node startNode = Grid.NodeFromWorldPoint(startPos); // where i stand
node targetNode = Grid.NodeFromWorldPoint(TargetPos); // where i wanna get
List <node> openSet = new List <node>(); //the set of nodes to be evaluated
HashSet <node> closedSet = new HashSet <node>(); // the set of nodes already evaluated
openSet.Add(startNode); // add a evalutaed node to the start node
while (openSet.Count > 0)
{
node CurrentNode = openSet[0];
// loop through all the nodes in the openSet
for (int i = 1; i < openSet.Count; i++)
{ // if the 2 nodes have equal f.cost we take the closeset one to the open set with the lowest f.cost
if (openSet[i].fCost() < CurrentNode.fCost() || openSet[i].fCost() == CurrentNode.fCost() && openSet[i].hCost < CurrentNode.hCost)
{
CurrentNode = openSet[i];
}
}
openSet.Remove(CurrentNode);
closedSet.Add(CurrentNode);
// we found our path if this is true
if (CurrentNode == targetNode)
{
RetracePath(startNode, targetNode); // give vlaue to the RetracePathe mathod
return;
}
//check if the nieghbour node is walkable or not walkable
foreach (node niebghbour in Grid.GetNeighbours(CurrentNode))
{
if (!niebghbour.walkable || closedSet.Contains(niebghbour))
{
continue;
}
// move from one node to the other by calculation in GetDistance
int newMovmentCostToNieghbour = CurrentNode.gCost + GetDistance(CurrentNode, niebghbour);
if (newMovmentCostToNieghbour < niebghbour.gCost || !openSet.Contains(niebghbour))
{
niebghbour.gCost = newMovmentCostToNieghbour;
niebghbour.hCost = GetDistance(niebghbour, targetNode);
niebghbour.parent = CurrentNode;
if (!openSet.Contains(niebghbour))
{
openSet.Add(niebghbour);
}
}
}
}
}
/// <summary>
/// this method will create the path that the object has to take.
/// </summary>
/// <param name="startPosition"></param>
/// <param name="endPosition"></param>
IEnumerator FindPath(Vector3 startPosition, Vector3 endPosition)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool PathSuccess = false;
//converts the vector3 position to the node on the map.
Node startNode = Grid.NodeFromWorldPoint(startPosition);
Node endNode = Grid.NodeFromWorldPoint(endPosition);
//list for containing all the open nodes.
Heap <Node> openSet = new Heap <Node>(Grid.MaxSize);
//HashSEt holds a set of objects making it easy to check if a certain object is already in it.
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
//find node in open set with lowest f cost
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
//if the node found with lowest fcost is the target, return.
if (currentNode == endNode)
{
//if path is found, stop the stopWatch
sw.Stop();
print("path found " + sw.ElapsedMilliseconds + "ms");
//if path is found, it is successful.
PathSuccess = true;
//if pathFound, exit out of the loop.
break;
}
foreach (Node neighbor in Grid.getNeighbors(currentNode))
{
//if the neighbor is not traversable or is already in the closed set, skip
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
//add the movement penalty to the cost to the neighbor too.
int newMovementCostToNeighbor = currentNode.gCost + getDistance(currentNode, neighbor) + neighbor.movementPenalty;
// if the distance between the current node and the neighbor is less than the gcost or neighbor is not in the open list
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = getDistance(neighbor, endNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
//makes it wait for one frame.
yield return(null);
//after finding the path:
//if a path has been found, store each node of the path into the array.
if (PathSuccess)
{
waypoints = ReTracePath(startNode, endNode);
}
//the reqested data has been obtained.Process is finished
requestManager.FinishedProcessingPath(waypoints, PathSuccess);
}