//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); }