public void FindPath(PathRequestData requestData, Action <PathResultData> callbackAction) { Stopwatch sw = new Stopwatch(); /* Used in order to see the performance of the path-finding system before/after heap tree optimization. * Stopwatch creates some kind of overhead so disable it in the final build.*/ sw.Start(); // CallbackPathResult elements Vector2[] pathWayPoints = new Vector2[0]; bool pathSuccess = false; #region A* Algorithm // Find the start and end nodes on the current grid system Node startNode = _gridSystem.NodeFromWorldPoint(requestData.PathStart); Node targetNode = _gridSystem.NodeFromWorldPoint(requestData.PathEnd); // Only do calculations if nodes are walkable if (/*startNode.Walkable &&*/ targetNode.Walkable) { HeapTree <Node> openNodeSet = new HeapTree <Node>(_gridSystem.MaxHeapSize); // Binary Heap Tree for A* iteration - optimization HashSet <Node> closedNodeSet = new HashSet <Node>(); // Hash Set for the closed node set of the A* algorithm // openNodeSet.AddItem(startNode); // Begin with the starting node while (openNodeSet.HeapTreeSize > 0) // While the open node set is not empty { Node currentNode = openNodeSet.RemoveFirst(); // First in heap tree is the one with the lowest score. closedNodeSet.Add(currentNode); // Transfer this node from open set to closed set if (currentNode == targetNode) // PathResult status check - if the target node is reached { sw.Stop(); print("Path found in : " + sw.ElapsedMilliseconds + " mili-seconds "); pathSuccess = true; break; // PathResult has been found. Exit the while loop } // If not found start neighbor cost calculations // List of the adjacent neighboring nodes. They are pre-calculated and stored in Nodes - optimization (Heap -> Stack Memory Allocation) List <Node> neighborNodeSet = currentNode.NeighborNodes; // Get the pre-computed neighbor nodes for (int i = 0; i < neighborNodeSet.Count; i++) { if (!neighborNodeSet[i].Walkable || closedNodeSet.Contains(neighborNodeSet[i])) { continue; // Non-walkable nodes and already closed nodes are passed } // New movement G cost with the path through current node int movementCostToNeighbor = currentNode.GCost + GetDistance(currentNode, neighborNodeSet[i]); // If the new distance from starting node is shorter and open node set does not contain this neighbor if (movementCostToNeighbor < neighborNodeSet[i].GCost || !openNodeSet.ContainsItem(neighborNodeSet[i])) { neighborNodeSet[i].GCost = movementCostToNeighbor; // Update the distance from starting node with its new path neighborNodeSet[i].HCost = GetDistance(neighborNodeSet[i], targetNode); neighborNodeSet[i].ParentNode = currentNode; // Set the new parent of the node for final path detection if (!openNodeSet.ContainsItem(neighborNodeSet[i])) // If it is not in the open set, add it to the set { openNodeSet.AddItem(neighborNodeSet[i]); } else { openNodeSet.UpdateItem(neighborNodeSet[i]); // If already in the set then the values are changed } } } } } #endregion // If an available path is found if (pathSuccess) { pathWayPoints = ReTracePath(startNode, targetNode); // Find the path way-points by re-tracing parent nodes pathSuccess = pathWayPoints.Length > 0; // Set path to success if there is at least one way-point } // Trigger the callback, path is found. This will trigger the queue in the path-finding request manager callbackAction(new PathResultData(pathWayPoints, _wayPointNodesCache, pathSuccess, requestData.CallbackPathRequest)); }