public void FindPath(PathRequest request, Action <PathResult> callback) { Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; Node startNode = gridPath.NodeFromWorldPoint(request.pathStart); Node targetNode = gridPath.NodeFromWorldPoint(request.pathEnd); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(gridPath.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 (Edge edge in gridPath.GetNeighbors(currentNode)) { Node neighbor = edge.nodeB; if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int airWeight = 0; if (!gridPath.IsNodeGround(edge.nodeB)) { airWeight = 4; } int newMovementCost = currentNode.gCost + GetDistance(currentNode, neighbor) + (int)edge.weight + airWeight; if (newMovementCost < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCost; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } } if (pathSuccess) { wayPoints = RetracePath(startNode, targetNode); pathSuccess = wayPoints.Length > 0; } callback(new PathResult(wayPoints, pathSuccess, request.controller, request.callback)); }