public void FindPath(Vector2 startPos, Vector2 tarPos) { Node StartNode = grid.NodeFromWorldPosition(startPos); Node TargetNode = grid.NodeFromWorldPosition(tarPos); List <Node> OpenList = new List <Node>(); HashSet <Node> ClosedList = new HashSet <Node>(); OpenList.Add(StartNode); while (OpenList.Count > 0) { Node CurrentNode = OpenList[0]; for (int i = 1; i < OpenList.Count; i++) { if (OpenList[i].FCost < CurrentNode.FCost || OpenList[i].FCost == CurrentNode.FCost && OpenList[i].hCost < CurrentNode.hCost) { CurrentNode = OpenList[i]; } } OpenList.Remove(CurrentNode); ClosedList.Add(CurrentNode); if (CurrentNode == TargetNode) { GetFinalPath(StartNode, TargetNode); } foreach (Node NeighborNode in grid.GetNeighboringNodes(CurrentNode)) { if (!NeighborNode.IsWall || ClosedList.Contains(NeighborNode)) { continue; } int MoveCost = CurrentNode.gCost + GetManhattenDistance(CurrentNode, NeighborNode); if (MoveCost < NeighborNode.gCost || !OpenList.Contains(NeighborNode)) { NeighborNode.gCost = MoveCost; NeighborNode.hCost = GetManhattenDistance(NeighborNode, TargetNode); NeighborNode.Parent = CurrentNode; if (!OpenList.Contains(NeighborNode)) { OpenList.Add(NeighborNode); } } } } }
public bool FindPath(Vector3 startWorldPoint, Vector3 endWorldPoint, ref List <Vector3> path, bool local) { PathfindingNode startNode = m_Grid.GetNodeFromWorldPoint(startWorldPoint); if (startNode == null) { return(false); } PathfindingNode targetNode = m_Grid.GetNodeFromWorldPoint(endWorldPoint); if (targetNode == null) { return(false); } if (startNode == targetNode) { return(false); } // List of nodes for the open list. List <PathfindingNode> openList = new List <PathfindingNode>(); // List of nodes for the closed list. HashSet <PathfindingNode> closedList = new HashSet <PathfindingNode>(); openList.Add(startNode); while (openList.Count > 0) { PathfindingNode currentNode = openList[0]; for (int i = 1; i < openList.Count; ++i) { if (openList[i].fCost < currentNode.fCost || openList[i].fCost == currentNode.fCost && openList[i].hCost < currentNode.hCost) { currentNode = openList[i]; } } openList.Remove(currentNode); closedList.Add(currentNode); if (currentNode == targetNode) { path = GetFinalPath(startNode, targetNode, local); return(true); } foreach (PathfindingNode neighborNode in m_Grid.GetNeighboringNodes(currentNode)) { if (neighborNode.isObstacle || closedList.Contains(neighborNode)) { continue; } int moveCost = currentNode.gCost + GetManhattenDistance(currentNode, neighborNode); if (moveCost < neighborNode.gCost || !openList.Contains(neighborNode)) { neighborNode.gCost = moveCost; neighborNode.hCost = GetManhattenDistance(neighborNode, targetNode); neighborNode.parentNode = currentNode; if (!openList.Contains(neighborNode)) { openList.Add(neighborNode); } } } } return(false); }