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

using System.Collections.Generic;
using UnityEngine;
using UnityEngine.AI;

namespace InfiniteWorld.VoxelWorld.NavMesh
{
    internal static class NavMeshBoundsUtility
    {
        public static Bounds CalculateBounds(List<NavMeshBuildSource> sources)
        {
            Bounds bounds = GetSourceBounds(sources[0]);
            for (int i = 1; i < sources.Count; i++)
            {
                bounds.Encapsulate(GetSourceBounds(sources[i]));
            }

            return bounds;
        }

        public static void ExpandBounds(ref Bounds bounds, float horizontalPadding, float verticalPadding)
        {
            Vector3 size = bounds.size;
            size.x = Mathf.Max(size.x + horizontalPadding * 2f, 0.1f);
            size.z = Mathf.Max(size.z + horizontalPadding * 2f, 0.1f);
            size.y = Mathf.Max(size.y + verticalPadding * 2f, 0.1f);
            bounds.size = size;
        }

        public static Bounds CreateQuantizedCoverageBounds(Bounds rawBounds, float padding, float minSize, float quantizationStep)
        {
            Vector3 min = rawBounds.min;
            Vector3 max = rawBounds.max;

            min.x -= padding;
            min.z -= padding;
            max.x += padding;
            max.z += padding;

            EnsureMinimumSpan(ref min.x, ref max.x, minSize);
            EnsureMinimumSpan(ref min.z, ref max.z, minSize);

            min.x = quantizationStep * Mathf.Floor(min.x / quantizationStep);
            min.z = quantizationStep * Mathf.Floor(min.z / quantizationStep);
            max.x = quantizationStep * Mathf.Ceil(max.x / quantizationStep);
            max.z = quantizationStep * Mathf.Ceil(max.z / quantizationStep);

            Vector3 center = new Vector3((min.x + max.x) * 0.5f, 0f, (min.z + max.z) * 0.5f);
            Vector3 size = new Vector3(Mathf.Max(max.x - min.x, minSize), 0.1f, Mathf.Max(max.z - min.z, minSize));
            return new Bounds(center, size);
        }

        public static float DistanceToBoundsXZ(Bounds bounds, Vector3 point)
        {
            float dx = Mathf.Max(bounds.min.x - point.x, 0f, point.x - bounds.max.x);
            float dz = Mathf.Max(bounds.min.z - point.z, 0f, point.z - bounds.max.z);
            return Mathf.Sqrt(dx * dx + dz * dz);
        }

        public static float DistanceBetweenBoundsXZ(Bounds left, Bounds right)
        {
            float dx = Mathf.Max(left.min.x - right.max.x, 0f, right.min.x - left.max.x);
            float dz = Mathf.Max(left.min.z - right.max.z, 0f, right.min.z - left.max.z);
            return Mathf.Sqrt(dx * dx + dz * dz);
        }

        public static bool ContainsXZ(Bounds bounds, Vector3 position)
        {
            return position.x >= bounds.min.x && position.x <= bounds.max.x
                && position.z >= bounds.min.z && position.z <= bounds.max.z;
        }

        public static bool IntersectsXZ(Bounds left, Bounds right)
        {
            return left.min.x <= right.max.x && left.max.x >= right.min.x
                && left.min.z <= right.max.z && left.max.z >= right.min.z;
        }

        public static bool BoundsApproximatelyEqual(Bounds left, Bounds right)
        {
            return Vector3.SqrMagnitude(left.center - right.center) < 0.0001f
                && Vector3.SqrMagnitude(left.size - right.size) < 0.0001f;
        }

        private static Bounds GetSourceBounds(NavMeshBuildSource source)
        {
            if (source.shape == NavMeshBuildSourceShape.Box)
            {
                return TransformBounds(source.transform, new Bounds(Vector3.zero, source.size));
            }

            Mesh mesh = source.sourceObject as Mesh;
            if (mesh != null)
            {
                return TransformBounds(source.transform, mesh.bounds);
            }

            return new Bounds(source.transform.GetColumn(3), Vector3.zero);
        }

        private static Bounds TransformBounds(Matrix4x4 matrix, Bounds localBounds)
        {
            Vector3 center = localBounds.center;
            Vector3 extents = localBounds.extents;

            Vector3[] corners =
            {
                new Vector3(center.x - extents.x, center.y - extents.y, center.z - extents.z),
                new Vector3(center.x - extents.x, center.y - extents.y, center.z + extents.z),
                new Vector3(center.x - extents.x, center.y + extents.y, center.z - extents.z),
                new Vector3(center.x - extents.x, center.y + extents.y, center.z + extents.z),
                new Vector3(center.x + extents.x, center.y - extents.y, center.z - extents.z),
                new Vector3(center.x + extents.x, center.y - extents.y, center.z + extents.z),
                new Vector3(center.x + extents.x, center.y + extents.y, center.z - extents.z),
                new Vector3(center.x + extents.x, center.y + extents.y, center.z + extents.z)
            };

            Bounds worldBounds = new Bounds(matrix.MultiplyPoint3x4(corners[0]), Vector3.zero);
            for (int i = 1; i < corners.Length; i++)
            {
                worldBounds.Encapsulate(matrix.MultiplyPoint3x4(corners[i]));
            }

            return worldBounds;
        }

        private static void EnsureMinimumSpan(ref float min, ref float max, float minimumSize)
        {
            float currentSize = max - min;
            if (currentSize >= minimumSize)
            {
                return;
            }

            float halfPadding = (minimumSize - currentSize) * 0.5f;
            min -= halfPadding;
            max += halfPadding;
        }
    }
}