IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
void FindPath(Vector3 startPos, Vector3 targetPos) { Node startNode = nGrid.NodeFromWorldPoint(startPos); Node targetNode = nGrid.NodeFromWorldPoint(targetPos); //List<Node> openSet = new List<Node>(); Heap <Node> openSet = new Heap <Node>(nGrid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); //Node currentNode = openSet[0]; //for(int i = 1; i < openSet.Count; i++) //{ // 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); if (currentNode == targetNode) { RetracePath(startNode, targetNode); return; } foreach (Node neighbour in nGrid.GetNeighbours(currentNode)) { if (!neighbour.isWalkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } }
//A* pathfinding executed in this function public void FindPath(Vector3 startPos, Vector3 targetPos) { path.Clear(); Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { RetracePath(startNode, targetNode); return; } //check values of neighboring nodes foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } //if value is lowest, make it the next node int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { //openSet.UpdateItem(neighbour); } } } } }
/// <summary> /// Finds the shortest path with help of nodes, and according to the A-star algorithm /// </summary> /// <param name="request"></param> /// <param name="callback"></param> public void FindPath(PathRequest request, Action <PathResult> callback) { NodeGrid nodeGrid = NodeGrid.Instance; Vector3[] waypoints = new Vector3[0]; bool pathFound = false; Node startNode = nodeGrid.NodeFromWorldPoint(request.pathStart); Node targetNode = nodeGrid.NodeFromWorldPoint(request.pathEnd); if (startNode == null || targetNode == null) { callback(new PathResult(new Vector3[0], false, request.callback)); } else if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node activeNode = openSet.RemoveFirst(); closedSet.Add(activeNode); if (activeNode == targetNode) { pathFound = true; break; } foreach (Node neighbour in nodeGrid.GetNeighbours(activeNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newCostToNeighbour = activeNode.gCost + GetDistance(activeNode, neighbour); if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = activeNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } if (pathFound) { waypoints = RetracePath(startNode, targetNode); pathFound = waypoints.Length > 0; } callback(new PathResult(waypoints, pathFound, request.callback)); } }
//! Returns List<Node> public List <Node> FindPath(Node startNode, Node targetNode, ref NodeGrid grid, bool isPathSimplified = false, bool isPathFromStartToTarget = true, bool hasDiagonalMovement = true) { System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch(); sw.Start(); bool pathSuccess = false; //Node startNode = grid.NodeFromWorldPosition(startPos); //Node targetNode = grid.NodeFromWorldPosition(targetPos); if (startNode.isWalkable && targetNode.isWalkable) { //Heap<Node> openSet = new Heap<Node>(grid.MaxHeapSize); Heap <Node> openSet = new Heap <Node>(grid.MaxHeapSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.Remove(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); pathSuccess = true; Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms"); break; } foreach (var neighbourNode in grid.GetNeighbours(currentNode, hasDiagonalMovement)) { if (!neighbourNode.isWalkable || closedSet.Contains(neighbourNode)) { continue; } //count distance for each neighbour at grid generation => create set/array/list of neighbours? int movementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode) + neighbourNode.movementPenalty; bool isInOpenSet = openSet.Contains(neighbourNode); if (movementCostToNeighbour < neighbourNode.gCost || !isInOpenSet) { neighbourNode.gCost = movementCostToNeighbour; neighbourNode.hCost = GetDistance(neighbourNode, targetNode); neighbourNode.parent = currentNode; if (!isInOpenSet) { openSet.Add(neighbourNode); } else { openSet.UpdateItem(neighbourNode); } } } } } if (pathSuccess) { List <Node> path = RetracePath(startNode, targetNode); if (isPathSimplified) { SimplifyPathReturnAsNodes(ref path); pathSuccess = path.Count > 0; } if (isPathFromStartToTarget) { ReversePath(ref path); } if (pathSuccess) { return(path); } } return(null); }
public void FindPath(PathRequest request, Action <PathResult> callback) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(request.pathStart); Node targetNode = grid.NodeFromWorldPoint(request.pathEnd); startNode.parent = startNode; if (targetNode.walkable) { openSet = new Heap <Node>(); closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } if (request.randomize) { neighbour.movementPenalty = UnityEngine.Random.Range(0, 50); //To make AI go randomly } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; // First, we'll need to know which node we will be starting from, and which node we will be going to Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); // Check if the start and target nodes are walkable (since all would be useless if either are in places that are supposed to // be inaccessible) if (startNode.walkable && targetNode.walkable) { // We need an open set and closed set of nodes to tell us which nodes have been considered and which will need // to be considered. // The Open Set contains nodes that are next to the nodes that have been considered, which are within the Closed Set. // When the node has been thoroughly considered, we will move this node into the closed set. // When first initialized, the closed set will be empty, and the open set will only have the starting node within to be // considered. Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); // While there are nodes to be considered, we keep the first node in mind while we scan through the list of nodes after it. // If we find a node that has a lower overall cost than the current node, we will shift the focus to this new node instead. // If that node has the same overall cost as the focused node, we look at the herustic cost of both nodes, and focus on the // one with the lower hCost instead. Once done, we need to shift the previously focused node to the Closed Set, and remove // it from the Open Set. while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); // If we have reached the target node, we can leave the loop if (currentNode == targetNode) { sw.Stop(); print("Path found in: " + sw.ElapsedMilliseconds + "ms"); pathSuccess = true; break; } // Check each neighbouring node foreach (Node neighbour in grid.GetNeighbours(currentNode)) { // Skip the checking for the focused node if it is unwalkable, or if it has already been considered before. if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } // Calculate the new distance from the start node to the neighbouring node of the current node. Only add the neighbour node to the // open set for consideration if the new distance from the neighbouring node to the start node is smaller than before, or if it has not // yet been considered before. This ignores nodes that have already been considered thanks to the if statement above. int newMoveCost2Neighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.weight; if (newMoveCost2Neighbour < neighbour.gCost || !openSet.Contains(neighbour)) { // Recalculate costs of neighbour node based on current node. We'll also parent the node to this one, so we can use the parent list to // backtrace our path to the start node. neighbour.gCost = newMoveCost2Neighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } // If loop is completed without finding a path, we can conclude that the target cannot be reached. if (!pathSuccess) { UnityEngine.Debug.Log("No path found. Target cannot be reached"); } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
public List <Node> FindPath(Vector2Int start, Vector2Int end) { List <Node> closedList = new List <Node>(); List <Node> openList = new List <Node>(); Node startNode = grid.GetNode(start); Node endNode = grid.GetNode(end); openList.Add(startNode); InitializeNodes(); startNode.Gcost = 0; startNode.Hcost = CalculateDistance(startNode, endNode); while (openList.Count > 0) { Node currentNode = GetLowestFcostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (Node n in grid.GetNeighbours(currentNode, 1)) { if (closedList.Contains(n)) { continue; } int tempGcost = currentNode.Gcost + CalculateDistance(currentNode, n); if (tempGcost < n.Fcost) { n.previousNode = currentNode; n.Gcost = tempGcost; n.Hcost = CalculateDistance(n, endNode); n.CalculateFcost(); if (!openList.Contains(n)) { openList.Add(n); } } } } //No more nodes Debug.LogError($"Could not find path cList:{closedList.Count}"); string pathString = "Closed list: "; foreach (Node n in closedList) { pathString += $"\n {n.position}"; } Debug.Log(pathString); return(null); }