public static PathNode GetLowestFCostNodeIndex(NativeArray <PathNode> openPathNodes, int2 endPosition) { PathNode lowestCostPathNode = openPathNodes[0]; lowestCostPathNode.HCost = AStarUtility.CalculateHCost(new int2(lowestCostPathNode.X, lowestCostPathNode.Y), endPosition); //Ignore first node, because we cant walk from a to a (atleast it will be ignored outright) for (int i = 1; i < openPathNodes.Length; ++i) { PathNode testPathNode = openPathNodes[i]; testPathNode.HCost = AStarUtility.CalculateHCost(new int2(testPathNode.X, testPathNode.Y), endPosition); if (testPathNode.FCost < lowestCostPathNode.FCost) { lowestCostPathNode = testPathNode; } } return(lowestCostPathNode); }
//Find path from startPosition to endPosition, bound by grid size. Add resulting path into PathCell buffer public static void FindPath(int2 startPosition, int2 endPosition, int2 gridSize, DynamicBuffer <PathCell> pathCellBuffer, NativeArray <int2> neighbourOffsetArray, ref PathFollow pathFollow, NativeArray <PathNode> pathNodesTraversableLookupReadOnly) { //Keeping internal copy of calculated path nodes for thread safety, otherwise multiple threads (jobs) use the pathNodesReadOnly Array directly //Using NativeHashMap because it has faster lookup speed (O(1) best) (0(n) worst) than (list and array 0(n) best) NativeHashMap <int, PathNode> calculatedPathNodes = new NativeHashMap <int, PathNode>(pathNodesTraversableLookupReadOnly.Length, Allocator.Temp); //Add Start Node PathNode startNode = pathNodesTraversableLookupReadOnly[AStarUtility.CalculateIndex(startPosition.x, startPosition.y, gridSize.x)]; startNode.GCost = 0; startNode.HCost = AStarUtility.CalculateHCost(new int2(startNode.X, startNode.Y), endPosition); startNode.PreviousNodeIndex = -1; calculatedPathNodes.Add(startNode.Index, startNode); int endNodeIndex = AStarUtility.CalculateIndex(endPosition.x, endPosition.y, gridSize.x); if (!calculatedPathNodes.ContainsKey(endNodeIndex)) { calculatedPathNodes.Add(endNodeIndex, pathNodesTraversableLookupReadOnly[endNodeIndex]); } //Using NativeHashMap because it has faster lookup speed (O(1) best) (0(n) worst) than (list and array 0(n) best) NativeHashMap <int, PathNode> openHashMap = new NativeHashMap <int, PathNode>(pathNodesTraversableLookupReadOnly.Length, Allocator.Temp); NativeHashMap <int, int> closedHashMap = new NativeHashMap <int, int>(pathNodesTraversableLookupReadOnly.Length, Allocator.Temp); openHashMap.Add(startNode.Index, startNode); while (openHashMap.Count() > 0) { NativeArray <PathNode> openPathNodesValueArray = openHashMap.GetValueArray(Allocator.Temp); PathNode currentNode = AStarUtility.GetLowestFCostNodeIndex(openPathNodesValueArray, endPosition); //Overwrite entry calculatedPathNodes[currentNode.Index] = currentNode; //? Reached EndNode if (currentNode.Index == endNodeIndex) { break; } //Remove current node from Open Lists for (int i = 0; i < openPathNodesValueArray.Length; ++i) { if (openPathNodesValueArray[i].Index == currentNode.Index) { openHashMap.Remove(openPathNodesValueArray[i].Index); break; } } closedHashMap.Add(currentNode.Index, currentNode.Index); // Testing all directions for (int i = 0; i < neighbourOffsetArray.Length; ++i) { int2 neighbourOffset = neighbourOffsetArray[i]; int2 neighbourPosition = new int2(currentNode.X + neighbourOffset.x, currentNode.Y + neighbourOffset.y); if (!AStarUtility.IsPositionInsideGrid(neighbourPosition, gridSize)) { //Not valid position continue; } int neighbourNodeIndex = AStarUtility.CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x); if (closedHashMap.ContainsKey(neighbourNodeIndex)) { //Already searched this node continue; } PathNode neighbourNode = pathNodesTraversableLookupReadOnly[neighbourNodeIndex]; if (!neighbourNode.IsTraversable) { //Can not walk on this node continue; } //Calculate GCost for Neighbour int gCostNext = currentNode.GCost + AStarUtility.CalculateHCost(new int2(currentNode.X, currentNode.Y), neighbourPosition); if (gCostNext < neighbourNode.GCost) { neighbourNode.PreviousNodeIndex = currentNode.Index; neighbourNode.GCost = gCostNext; if (!calculatedPathNodes.ContainsKey(neighbourNode.Index)) { calculatedPathNodes.Add(neighbourNode.Index, neighbourNode); } if (!openHashMap.ContainsKey(neighbourNode.Index)) { openHashMap.Add(neighbourNode.Index, neighbourNode); } } } } closedHashMap.Dispose(); openHashMap.Dispose(); pathCellBuffer.Clear(); CalculatePath(calculatedPathNodes, calculatedPathNodes[endNodeIndex], pathCellBuffer, ref pathFollow); calculatedPathNodes.Dispose(); }