TheDeclineOfWarriors/Assets/Features/VoxelWorldNavMesh/Runtime/NavCoveragePlanning.cs

using System.Collections.Generic;
using InfiniteWorld.VoxelWorld.Contracts;
using UnityEngine;

namespace InfiniteWorld.VoxelWorld.NavMesh
{
    internal static class NavCoveragePlanning
    {
        public static void BuildDesiredCoverageWindows(
            List<WorldInterestPoint> interestPoints,
            VoxelWorldNavMeshConfig config,
            float chunkWorldSize,
            List<DesiredCoverageWindow> desiredCoverageWindows,
            List<ClusterAccumulator> clusterAccumulators)
        {
            desiredCoverageWindows.Clear();
            clusterAccumulators.Clear();

            if (interestPoints.Count == 0)
            {
                return;
            }

            float mergeDistance = Mathf.Max(0f, config.clusterMergeDistanceInChunks) * chunkWorldSize;
            float quantizationStep = Mathf.Max(0.25f, config.coverageQuantizationInChunks) * chunkWorldSize;
            float padding = Mathf.Max(0f, config.coveragePaddingInChunks) * chunkWorldSize;
            float minWindowSize = Mathf.Max(1f, config.minCoverageWindowSizeInChunks) * chunkWorldSize;

            for (int i = 0; i < interestPoints.Count; i++)
            {
                WorldInterestPoint point = interestPoints[i];
                int bestClusterIndex = -1;
                float bestDistance = float.MaxValue;

                for (int clusterIndex = 0; clusterIndex < clusterAccumulators.Count; clusterIndex++)
                {
                    float distance = NavMeshBoundsUtility.DistanceToBoundsXZ(clusterAccumulators[clusterIndex].RawBounds, point.Position);
                    if (distance <= mergeDistance && distance < bestDistance)
                    {
                        bestDistance = distance;
                        bestClusterIndex = clusterIndex;
                    }
                }

                if (bestClusterIndex >= 0)
                {
                    ClusterAccumulator cluster = clusterAccumulators[bestClusterIndex];
                    cluster.Add(point);
                    clusterAccumulators[bestClusterIndex] = cluster;
                }
                else
                {
                    clusterAccumulators.Add(new ClusterAccumulator(point));
                }
            }

            MergeNearbyClusters(clusterAccumulators, mergeDistance);

            for (int i = 0; i < clusterAccumulators.Count; i++)
            {
                ClusterAccumulator cluster = clusterAccumulators[i];
                Bounds coverageBounds = NavMeshBoundsUtility.CreateQuantizedCoverageBounds(cluster.RawBounds, padding, minWindowSize, quantizationStep);
                desiredCoverageWindows.Add(new DesiredCoverageWindow(coverageBounds, cluster.Priority, cluster.InterestCount));
            }

            desiredCoverageWindows.Sort((left, right) =>
            {
                int priorityCompare = right.Priority.CompareTo(left.Priority);
                if (priorityCompare != 0)
                {
                    return priorityCompare;
                }

                int interestCompare = right.InterestCount.CompareTo(left.InterestCount);
                if (interestCompare != 0)
                {
                    return interestCompare;
                }

                return left.CoverageBounds.center.sqrMagnitude.CompareTo(right.CoverageBounds.center.sqrMagnitude);
            });

            int maxWindows = Mathf.Max(1, config.maxActiveCoverageWindows);
            if (desiredCoverageWindows.Count > maxWindows)
            {
                desiredCoverageWindows.RemoveRange(maxWindows, desiredCoverageWindows.Count - maxWindows);
            }
        }

        public static NavCoverageWindowRuntime FindBestMatchingCoverageWindow(
            DesiredCoverageWindow desiredWindow,
            Dictionary<int, NavCoverageWindowRuntime> coverageWindows,
            float chunkWorldSize,
            VoxelWorldNavMeshConfig config)
        {
            NavCoverageWindowRuntime bestMatch = null;
            float bestDistance = float.MaxValue;
            float matchThreshold = Mathf.Max(config.minCoverageWindowSizeInChunks, config.clusterMergeDistanceInChunks + config.coveragePaddingInChunks) * chunkWorldSize;

            foreach (KeyValuePair<int, NavCoverageWindowRuntime> pair in coverageWindows)
            {
                NavCoverageWindowRuntime candidate = pair.Value;
                if (candidate.MatchedThisFrame)
                {
                    continue;
                }

                float distance = Vector2.Distance(
                    new Vector2(candidate.CoverageBounds.center.x, candidate.CoverageBounds.center.z),
                    new Vector2(desiredWindow.CoverageBounds.center.x, desiredWindow.CoverageBounds.center.z));

                if (distance > matchThreshold || distance >= bestDistance)
                {
                    continue;
                }

                bestDistance = distance;
                bestMatch = candidate;
            }

            return bestMatch;
        }

        public static float GetCoveragePriorityScore(NavCoverageWindowRuntime window, List<WorldInterestPoint> interestPoints)
        {
            if (interestPoints.Count == 0)
            {
                return 0f;
            }

            Vector3 center = window.CoverageBounds.center;
            float bestDistance = float.MaxValue;
            for (int i = 0; i < interestPoints.Count; i++)
            {
                float priority = Mathf.Max(0.01f, interestPoints[i].Priority);
                float distance = Vector2.SqrMagnitude(
                    new Vector2(center.x, center.z) - new Vector2(interestPoints[i].Position.x, interestPoints[i].Position.z)) / priority;
                if (distance < bestDistance)
                {
                    bestDistance = distance;
                }
            }

            return bestDistance;
        }

        private static void MergeNearbyClusters(List<ClusterAccumulator> clusterAccumulators, float mergeDistance)
        {
            if (clusterAccumulators.Count < 2)
            {
                return;
            }

            bool merged;
            do
            {
                merged = false;
                for (int i = 0; i < clusterAccumulators.Count; i++)
                {
                    for (int j = i + 1; j < clusterAccumulators.Count; j++)
                    {
                        if (NavMeshBoundsUtility.DistanceBetweenBoundsXZ(clusterAccumulators[i].RawBounds, clusterAccumulators[j].RawBounds) > mergeDistance)
                        {
                            continue;
                        }

                        ClusterAccumulator combined = clusterAccumulators[i];
                        combined.Merge(clusterAccumulators[j]);
                        clusterAccumulators[i] = combined;
                        clusterAccumulators.RemoveAt(j);
                        merged = true;
                        break;
                    }

                    if (merged)
                    {
                        break;
                    }
                }
            }
            while (merged);
        }
    }

    internal readonly struct DesiredCoverageWindow
    {
        public DesiredCoverageWindow(Bounds coverageBounds, float priority, int interestCount)
        {
            CoverageBounds = coverageBounds;
            Priority = priority;
            InterestCount = interestCount;
        }

        public Bounds CoverageBounds { get; }
        public float Priority { get; }
        public int InterestCount { get; }
    }

    internal struct ClusterAccumulator
    {
        public ClusterAccumulator(WorldInterestPoint point)
        {
            RawBounds = new Bounds(new Vector3(point.Position.x, 0f, point.Position.z), new Vector3(0.1f, 0.1f, 0.1f));
            Priority = point.Priority;
            InterestCount = 1;
        }

        public Bounds RawBounds;
        public float Priority;
        public int InterestCount;

        public void Add(WorldInterestPoint point)
        {
            RawBounds.Encapsulate(new Vector3(point.Position.x, 0f, point.Position.z));
            Priority = Mathf.Max(Priority, point.Priority);
            InterestCount++;
        }

        public void Merge(ClusterAccumulator other)
        {
            RawBounds.Encapsulate(other.RawBounds.min);
            RawBounds.Encapsulate(other.RawBounds.max);
            Priority = Mathf.Max(Priority, other.Priority);
            InterestCount += other.InterestCount;
        }
    }
}