public void FindPath(PathRequest request, Action <PathResult> callback) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(request.pathStart); Node targetNode = grid.NodeFromWorldPoint(request.pathEnd); startNode.parent = startNode; 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) { sw.Stop(); //print ("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighborNodes(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } 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.callbackAction)); }
public static List <Node> GetAStarPath(Vector3 startPosition, Vector3 endPosition, NodeGrid grid) { _grid = grid; var startNode = _grid.NodeFromWorldPosition(startPosition); var endNode = _grid.NodeFromWorldPosition(endPosition); if (grid == null || startPosition == endPosition || startNode == null || endNode == null) { return(null); } var openList = new List <Node>(); var closedList = new HashSet <Node>(); openList.Add(startNode); while (openList.Count > 0) { var currentNode = openList[0]; for (int i = 1; i < openList.Count; i++) { if (openList[i].fCost <= currentNode.fCost && openList[i].hCost < currentNode.hCost) { currentNode = openList[i]; } } openList.Remove(currentNode); closedList.Add(currentNode); if (currentNode == endNode) { break; } var n = _grid.GetNeighborNodes(currentNode); foreach (var neighbor in n) { if (!neighbor.isObstacle || closedList.Contains(neighbor)) { continue; } var moveCost = currentNode.gCost + GetDistance(currentNode, neighbor); if (moveCost < neighbor.gCost || !openList.Contains(neighbor)) { neighbor.gCost = moveCost; neighbor.hCost = GetDistance(neighbor, endNode); neighbor.previous = currentNode; if (!openList.Contains(neighbor)) { openList.Add(neighbor); } } } } return(ThetaStarPath(GetFinalPath(startNode, endNode))); }