private List <PathNode> FindPath(PathNode start, PathNode end, float blockageDPS, float moveSpeed) { List <PathNode> result = new List <PathNode>(); if (start == end) { return(result); } PathingInfo startInfo = new PathingInfo(start); startInfo.G = 0; startInfo.H = _heuristic.Calculate(start, end); List <PathingInfo> closedSet = new List <PathingInfo>(); List <PathingInfo> openSet = new List <PathingInfo>() { startInfo }; PathingInfo endInfo = null; while (endInfo == null && openSet.Count > 0) { // find tile with lowest score float minF = float.MaxValue; int minIndex = -1; for (int i = 0; i < openSet.Count; ++i) { float f = openSet[i].G + openSet[i].H; if (f < minF) { minF = f; minIndex = i; } } PathingInfo nodeInfo = openSet[minIndex]; // move to closed set closedSet.Add(nodeInfo); openSet.RemoveAt(minIndex); var neighbours = nodeInfo.node.Neighbours; for (int i = 0; i < neighbours.Count; ++i) { var node = neighbours[i].Node; // Check if in closed set if (FindInSet(closedSet, node) != null) { continue; } float costToEnter = node.CostToEnter; if (node.BlockageHealth > 0) { costToEnter += node.BlockageHealth /= blockageDPS; } if (neighbours[i].Distance > 0f) { costToEnter += neighbours[i].Distance / moveSpeed; } float newG = nodeInfo.G + costToEnter; // Check if in open set PathingInfo pi = FindInSet(openSet, node); if (pi == null) { pi = new PathingInfo(node); // Compute score pi.G = newG; pi.H = _heuristic.Calculate(node, end); // set parent pi.parent = nodeInfo; // add to open set openSet.Add(pi); if (node == end) { // we have reached the end node endInfo = pi; } } else { // get F score float prevF = pi.G + pi.H; float newF = newG + pi.H; if (prevF < newF) { // cost along current path is better - use that pi.G = newG; pi.parent = nodeInfo; if (node == end) { // we have reached the end node endInfo = pi; } } } } } // construct the result path while (endInfo != null) { result.Insert(0, endInfo.node); endInfo = endInfo.parent; } //Debug.Log("Path Length" + result.Count.ToString()); return(result); }