public void FindPath(PathRequest request, Action <PathResult> callback) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; ANode startNode = grid.GetNodeFromWorldPoint(request.pathStart); ANode targetNode = grid.GetNodeFromWorldPoint(request.pathEnd); if (startNode.walkable && targetNode.walkable) { List <ANode> openSet = new List <ANode>(); HashSet <ANode> closedSet = new HashSet <ANode>(); openSet.Add(startNode); while (openSet.Count > 0) { ANode 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) { sw.Stop(); print("Path Found : " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (ANode neighbour in grid.GetNeighbours(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); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }