// More TODO: Take a SharedGrid as argument or extract from one of the maps. // Use it for functions. public static List <Vector2Int> FindMax(List <AIMovement.WeightedMap> maps, Vector2Int start, float searchDist, Vector2Int flockPoint, float flockPointInfluence, float minInfluence = 0f) { var frontier = new Queue <Vector2Int>() { start }; var discovered = new HashSet <Vector2Int>() { start }; var distanceTo = new Dictionary <Vector2Int, float>() { { start, 0 } }; var prev = new Dictionary <Vector2Int, Vector2Int>(); var res = new List <Vector2Int>(); Vector2Int best = start; float bestInfluence = 0f; if (flockPoint == best) { bestInfluence += flockPointInfluence; } foreach (var map in maps) { bestInfluence += map.map.GetInfluence(best.x, best.y) * map.weight; } while (frontier.Count != 0) { var pos = frontier.Dequeue(); float influence = 0f; if (pos == flockPoint) { influence += flockPointInfluence; } foreach (var map in maps) { influence += map.map.GetInfluence(pos.x, pos.y) * map.weight; } // If new best found, store it if (influence > bestInfluence) { best = pos; bestInfluence = influence; } float currentDistance = distanceTo[pos]; if (searchDist > currentDistance) { bool foundImprovement = false; Vector2Int leastBadNode = pos; // This is used if all neighbours have influence lower than minInfluence float leastBadInfluence = Mathf.NegativeInfinity; foreach (var n in SharedGrid.GetNeighbors4(pos)) { if (maps[0].map.grid.InBounds(n)) { float distToN = currentDistance + Vector2Int.Distance(pos, n); float avgInfluence = 0f; int count = maps.Count; if (n == flockPoint) { avgInfluence += flockPointInfluence; count++; } foreach (var map in maps) { avgInfluence += map.map.GetInfluence(n.x, n.y) * map.weight; } avgInfluence /= count; if (avgInfluence > leastBadInfluence) { leastBadNode = n; leastBadInfluence = avgInfluence; } if (avgInfluence >= minInfluence && Mathf.Approximately(maps[0].map.obstacleHeights.GetHeight(n), 0) && ((distanceTo.ContainsKey(n) && distToN < distanceTo[n]) || !discovered.Contains(n))) { discovered.Add(n); frontier.Enqueue(n); distanceTo[n] = distToN; prev[n] = pos; foundImprovement = true; } } } float distToLBN = currentDistance + Vector2Int.Distance(pos, leastBadNode); if (!foundImprovement && Mathf.Approximately(maps[0].map.obstacleHeights.GetHeight(leastBadNode), 0) && ((distanceTo.ContainsKey(leastBadNode) && distToLBN < distanceTo[leastBadNode]) || !discovered.Contains(leastBadNode))) { discovered.Add(leastBadNode); frontier.Enqueue(leastBadNode); distanceTo[leastBadNode] = distToLBN; prev[leastBadNode] = pos; } } } // Reconstruct path var c = best; if (flockPoint == best) { return(new List <Vector2Int>() { flockPoint }); } else { while (prev.ContainsKey(c)) { res.Add(c); c = prev[c]; } res.Reverse(); return(res); } }