// Warning: Some assembly references could not be resolved automatically. This might lead to incorrect decompilation of some parts,
// for ex. property getter/setter access. To get optimal decompilation results, please manually add the missing references to the list of loaded assemblies.
// EdenAutoMorpherScript, Version=0.0.0.0, Culture=neutral, PublicKeyToken=null
// Eden.AutoMorpher.BvhTriangleMesh
using System.Collections.Generic;
using UnityEngine;

public class BvhTriangleMesh
{
    public struct ClosestHit
    {
        public int triangleIndex;

        public Vector3 closestPoint;

        public Vector3 normal;

        public float sqrDistance;

        public HumanBodyBones mainHumanBone;
    }

    private TriangleUtil triangleUtil;

    private HumanBodyBones[] humanBones = new HumanBodyBones[34]
    {
        HumanBodyBones.Hips,
        HumanBodyBones.Spine,
        HumanBodyBones.Chest,
        HumanBodyBones.UpperChest,
        HumanBodyBones.Neck,
        HumanBodyBones.LeftShoulder,
        HumanBodyBones.LeftUpperArm,
        HumanBodyBones.LeftLowerArm,
        HumanBodyBones.LeftHand,
        HumanBodyBones.LeftThumbProximal,
        HumanBodyBones.LeftIndexProximal,
        HumanBodyBones.LeftMiddleProximal,
        HumanBodyBones.LeftRingProximal,
        HumanBodyBones.LeftLittleProximal,
        HumanBodyBones.RightShoulder,
        HumanBodyBones.RightUpperArm,
        HumanBodyBones.RightLowerArm,
        HumanBodyBones.RightHand,
        HumanBodyBones.RightThumbProximal,
        HumanBodyBones.RightIndexProximal,
        HumanBodyBones.RightMiddleProximal,
        HumanBodyBones.RightRingProximal,
        HumanBodyBones.RightLittleProximal,
        HumanBodyBones.LeftUpperLeg,
        HumanBodyBones.LeftLowerLeg,
        HumanBodyBones.LeftFoot,
        HumanBodyBones.LeftToes,
        HumanBodyBones.RightUpperLeg,
        HumanBodyBones.RightLowerLeg,
        HumanBodyBones.RightFoot,
        HumanBodyBones.RightToes,
        HumanBodyBones.Head,
        HumanBodyBones.LeftEye,
        HumanBodyBones.RightEye
    };

    public BvhTriangle[] triangles;

    public BvhNode[] nodes;

    public int[] triIndices;

    private const int LeafMaxTriangles = 4;

    public BvhTriangleMesh()
    {
        this.triangleUtil = new TriangleUtil();
    }

    private HumanBodyBones[] BuildVertexMainHumanBone(SkinnedMeshRenderer smr, Animator animator, HumanBodyBones[] bodyBones)
    {
        Mesh sharedMesh = smr.sharedMesh;
        BoneWeight[] boneWeights = sharedMesh.boneWeights;
        int[] boneToBodyIndex = this.BuildBoneToBodyIndexMap(smr, animator, bodyBones);
        HumanBodyBones[] array = new HumanBodyBones[sharedMesh.vertexCount];
        for (int i = 0; i < sharedMesh.vertexCount; i++)
        {
            BoneWeight boneWeight = boneWeights[i];
            int bestBodyIdx = -1;
            float bestWeight = 0f;
            Try(boneWeight.boneIndex0, boneWeight.weight0);
            Try(boneWeight.boneIndex1, boneWeight.weight1);
            Try(boneWeight.boneIndex2, boneWeight.weight2);
            Try(boneWeight.boneIndex3, boneWeight.weight3);
            array[i] = ((bestBodyIdx >= 0) ? bodyBones[bestBodyIdx] : HumanBodyBones.LastBone);
            void Try(int boneIdx, float w)
            {
                if (!(w <= 0f) && boneIdx >= 0 && boneIdx < boneToBodyIndex.Length)
                {
                    int num = boneToBodyIndex[boneIdx];
                    if (num >= 0 && w > bestWeight)
                    {
                        bestWeight = w;
                        bestBodyIdx = num;
                    }
                }
            }
        }
        return array;
    }

    public BvhTriangleMesh BuildFromSkinnedMeshes(IReadOnlyList<SkinnedMeshRenderer> renderers, Animator animator)
    {
        if (renderers == null || renderers.Count == 0)
        {
            return null;
        }
        BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
        int num = 0;
        foreach (SkinnedMeshRenderer renderer in renderers)
        {
            if (!(renderer == null) && !(renderer.sharedMesh == null))
            {
                num += renderer.sharedMesh.triangles.Length / 3;
            }
        }
        if (num == 0)
        {
            return null;
        }
        bvhTriangleMesh.triangles = new BvhTriangle[num];
        int num2 = 0;
        Mesh mesh = new Mesh();
        foreach (SkinnedMeshRenderer renderer2 in renderers)
        {
            if (renderer2 == null || renderer2.sharedMesh == null)
            {
                continue;
            }
            mesh.Clear();
            renderer2.BakeMesh(mesh);
            Vector3[] vertices = mesh.vertices;
            int[] array = renderer2.sharedMesh.triangles;
            BoneWeight[] boneWeights = renderer2.sharedMesh.boneWeights;
            int[] boneToBodyIndex = this.BuildBoneToBodyIndexMap(renderer2, animator, this.humanBones);
            int num3 = array.Length / 3;
            Transform transform = renderer2.transform;
            Vector3 lossyScale = transform.lossyScale;
            Vector3 vector = new Vector3(1f / Mathf.Max(lossyScale.x, 1E-08f), 1f / Mathf.Max(lossyScale.y, 1E-08f), 1f / Mathf.Max(lossyScale.z, 1E-08f));
            Matrix4x4 matrix4x = transform.localToWorldMatrix * Matrix4x4.Scale(vector);
            for (int i = 0; i < num3; i++)
            {
                int num4 = array[i * 3];
                int num5 = array[i * 3 + 1];
                int num6 = array[i * 3 + 2];
                Vector3 vector2 = matrix4x.MultiplyPoint3x4(vertices[num4]);
                Vector3 vector3 = matrix4x.MultiplyPoint3x4(vertices[num5]);
                Vector3 vector4 = matrix4x.MultiplyPoint3x4(vertices[num6]);
                Vector3 normal = Vector3.Cross(vector3 - vector2, vector4 - vector2);
                float magnitude = normal.magnitude;
                if (magnitude > 1E-08f)
                {
                    normal /= magnitude;
                }
                else
                {
                    normal = Vector3.up;
                }
                int num7 = this.ComputeTriangleMainHumanBoneIndex(num4, num5, num6, boneWeights, boneToBodyIndex, this.humanBones.Length);
                HumanBodyBones mainHumanBone = ((num7 >= 0) ? this.humanBones[num7] : HumanBodyBones.LastBone);
                bvhTriangleMesh.triangles[num2++] = new BvhTriangle
                {
                    a = vector2,
                    b = vector3,
                    c = vector4,
                    normal = normal,
                    mainHumanBone = mainHumanBone
                };
                mesh.Clear();
            }
        }
        int num8 = num;
        int[] array2 = new int[num8];
        for (int j = 0; j < num8; j++)
        {
            array2[j] = j;
        }
        bvhTriangleMesh.triIndices = array2;
        List<BvhNode> list = new List<BvhNode>();
        this.BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num8, list);
        bvhTriangleMesh.nodes = list.ToArray();
        return bvhTriangleMesh;
    }

    private int[] BuildBoneToBodyIndexMap(SkinnedMeshRenderer smr, Animator animator, HumanBodyBones[] bodyBones)
    {
        Transform[] bones = smr.bones;
        int[] array = new int[bones.Length];
        for (int i = 0; i < array.Length; i++)
        {
            array[i] = -1;
        }
        if (animator == null || bodyBones == null || bones == null)
        {
            return array;
        }
        Dictionary<Transform, int> dictionary = new Dictionary<Transform, int>();
        for (int j = 0; j < bones.Length; j++)
        {
            if (!(bones[j] == null) && !dictionary.ContainsKey(bones[j]))
            {
                dictionary.Add(bones[j], j);
            }
        }
        Dictionary<HumanBodyBones, HashSet<Transform>> dictionary2 = new MeshClassifier().MeshHumanoidBoneMatcher(animator, new SkinnedMeshRenderer[1] { smr });
        for (int k = 0; k < bodyBones.Length; k++)
        {
            HumanBodyBones key = bodyBones[k];
            if (!dictionary2.TryGetValue(key, out var value) || value == null)
            {
                continue;
            }
            foreach (Transform item in value)
            {
                if (!(item == null) && dictionary.TryGetValue(item, out var value2))
                {
                    array[value2] = k;
                }
            }
        }
        for (int l = 0; l < bones.Length; l++)
        {
            if (array[l] != -1)
            {
                continue;
            }
            Transform transform = bones[l];
            if (transform == null)
            {
                continue;
            }
            Transform parent = transform.parent;
            if (!(parent == null) && dictionary.TryGetValue(parent, out var value3))
            {
                int num = array[value3];
                if (num != -1)
                {
                    array[l] = num;
                }
            }
        }
        return array;
    }

    private int ComputeTriangleMainHumanBoneIndex(int vi0, int vi1, int vi2, BoneWeight[] weights, int[] boneToBodyIndex, int bodyBonesCount)
    {
        if (weights == null || weights.Length == 0 || boneToBodyIndex == null || boneToBodyIndex.Length == 0)
        {
            return -1;
        }
        float[] scores = new float[bodyBonesCount];
        Accumulate(vi0);
        Accumulate(vi1);
        Accumulate(vi2);
        int result = -1;
        float num = 0f;
        for (int i = 0; i < scores.Length; i++)
        {
            if (scores[i] > num)
            {
                num = scores[i];
                result = i;
            }
        }
        return result;
        void Accumulate(int v)
        {
            if (v >= 0 && v < weights.Length)
            {
                BoneWeight boneWeight = weights[v];
                Add(boneWeight.boneIndex0, boneWeight.weight0);
                Add(boneWeight.boneIndex1, boneWeight.weight1);
                Add(boneWeight.boneIndex2, boneWeight.weight2);
                Add(boneWeight.boneIndex3, boneWeight.weight3);
            }
        }
        void Add(int boneIdx, float w)
        {
            if (!(w <= 0f) && boneIdx >= 0 && boneIdx < boneToBodyIndex.Length)
            {
                int num2 = boneToBodyIndex[boneIdx];
                if (num2 >= 0)
                {
                    scores[num2] += w;
                }
            }
        }
    }

    public BvhTriangleMesh BuildFromSkinnedMeshes(IList<SkinnedMeshRenderer> renderers)
    {
        if (renderers == null || renderers.Count == 0)
        {
            return null;
        }
        BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
        int num = 0;
        foreach (SkinnedMeshRenderer renderer in renderers)
        {
            if (!(renderer == null) && !(renderer.sharedMesh == null))
            {
                num += renderer.sharedMesh.triangles.Length / 3;
            }
        }
        if (num == 0)
        {
            return null;
        }
        bvhTriangleMesh.triangles = new BvhTriangle[num];
        int num2 = 0;
        foreach (SkinnedMeshRenderer renderer2 in renderers)
        {
            if (renderer2 == null || renderer2.sharedMesh == null)
            {
                continue;
            }
            Mesh sharedMesh = renderer2.sharedMesh;
            Vector3[] vertices = sharedMesh.vertices;
            int[] array = sharedMesh.triangles;
            int num3 = array.Length / 3;
            for (int i = 0; i < num3; i++)
            {
                int num4 = array[i * 3];
                int num5 = array[i * 3 + 1];
                int num6 = array[i * 3 + 2];
                Vector3 vector = renderer2.transform.TransformPoint(vertices[num4]);
                Vector3 vector2 = renderer2.transform.TransformPoint(vertices[num5]);
                Vector3 vector3 = renderer2.transform.TransformPoint(vertices[num6]);
                Vector3 normal = Vector3.Cross(vector2 - vector, vector3 - vector);
                float magnitude = normal.magnitude;
                if (magnitude > 1E-08f)
                {
                    normal /= magnitude;
                }
                else
                {
                    normal = Vector3.up;
                }
                bvhTriangleMesh.triangles[num2++] = new BvhTriangle
                {
                    a = vector,
                    b = vector2,
                    c = vector3,
                    normal = normal
                };
            }
        }
        int num7 = num;
        int[] array2 = new int[num7];
        for (int j = 0; j < num7; j++)
        {
            array2[j] = j;
        }
        bvhTriangleMesh.triIndices = array2;
        List<BvhNode> list = new List<BvhNode>();
        this.BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num7, list);
        bvhTriangleMesh.nodes = list.ToArray();
        return bvhTriangleMesh;
    }

    public BvhTriangleMesh BuildFromMesh(Mesh mesh, Transform transform)
    {
        BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
        Vector3[] vertices = mesh.vertices;
        int[] array = mesh.triangles;
        int num = array.Length / 3;
        bvhTriangleMesh.triangles = new BvhTriangle[num];
        for (int i = 0; i < num; i++)
        {
            int num2 = array[i * 3];
            int num3 = array[i * 3 + 1];
            int num4 = array[i * 3 + 2];
            Vector3 vector = transform.TransformPoint(vertices[num2]);
            Vector3 vector2 = transform.TransformPoint(vertices[num3]);
            Vector3 vector3 = transform.TransformPoint(vertices[num4]);
            Vector3 normal = Vector3.Cross(vector2 - vector, vector3 - vector);
            float magnitude = normal.magnitude;
            if (magnitude > 1E-08f)
            {
                normal /= magnitude;
            }
            else
            {
                normal = Vector3.up;
            }
            bvhTriangleMesh.triangles[i] = new BvhTriangle
            {
                a = vector,
                b = vector2,
                c = vector3,
                normal = normal
            };
        }
        int[] array2 = new int[num];
        for (int j = 0; j < num; j++)
        {
            array2[j] = j;
        }
        bvhTriangleMesh.triIndices = array2;
        List<BvhNode> list = new List<BvhNode>();
        this.BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num, list);
        bvhTriangleMesh.nodes = list.ToArray();
        return bvhTriangleMesh;
    }

    private int BuildRecursive(BvhTriangle[] tris, int[] triIndices, int start, int count, List<BvhNode> outNodes)
    {
        int count2 = outNodes.Count;
        BvhNode bvhNode = default(BvhNode);
        Bounds bounds = default(Bounds);
        bool flag = true;
        for (int i = start; i < start + count; i++)
        {
            BvhTriangle bvhTriangle = tris[triIndices[i]];
            if (flag)
            {
                bounds = new Bounds(bvhTriangle.a, Vector3.zero);
                bounds.Encapsulate(bvhTriangle.b);
                bounds.Encapsulate(bvhTriangle.c);
                flag = false;
            }
            else
            {
                bounds.Encapsulate(bvhTriangle.a);
                bounds.Encapsulate(bvhTriangle.b);
                bounds.Encapsulate(bvhTriangle.c);
            }
        }
        bvhNode.bounds = bounds;
        if (count <= 4)
        {
            bvhNode.isLeaf = true;
            bvhNode.start = start;
            bvhNode.count = count;
            bvhNode.leftChild = -1;
            bvhNode.rightChild = -1;
            outNodes.Add(bvhNode);
            return count2;
        }
        Vector3 size = bounds.size;
        int num = 0;
        if (size.y > size.x && size.y > size.z)
        {
            num = 1;
        }
        else if (size.z > size.x && size.z > size.y)
        {
            num = 2;
        }
        float num2 = 0f;
        for (int j = start; j < start + count; j++)
        {
            BvhTriangle bvhTriangle2 = tris[triIndices[j]];
            num2 += ((bvhTriangle2.a + bvhTriangle2.b + bvhTriangle2.c) / 3f)[num];
        }
        num2 /= (float)count;
        int num3 = this.Partition(tris, triIndices, start, count, num, num2);
        if (num3 == start || num3 == start + count)
        {
            num3 = start + count / 2;
        }
        bvhNode.isLeaf = false;
        bvhNode.start = -1;
        bvhNode.count = 0;
        outNodes.Add(bvhNode);
        int leftChild = this.BuildRecursive(tris, triIndices, start, num3 - start, outNodes);
        int rightChild = this.BuildRecursive(tris, triIndices, num3, start + count - num3, outNodes);
        bvhNode.leftChild = leftChild;
        bvhNode.rightChild = rightChild;
        outNodes[count2] = bvhNode;
        return count2;
    }

    private int Partition(BvhTriangle[] tris, int[] triIndices, int start, int count, int axis, float splitPos)
    {
        int num = start;
        int num2 = start + count - 1;
        while (num <= num2)
        {
            BvhTriangle bvhTriangle = tris[triIndices[num]];
            BvhTriangle bvhTriangle2 = tris[triIndices[num2]];
            float num3 = (bvhTriangle.a[axis] + bvhTriangle.b[axis] + bvhTriangle.c[axis]) / 3f;
            _ = (bvhTriangle2.a[axis] + bvhTriangle2.b[axis] + bvhTriangle2.c[axis]) / 3f;
            if (num3 < splitPos)
            {
                num++;
                continue;
            }
            int num4 = triIndices[num];
            triIndices[num] = triIndices[num2];
            triIndices[num2] = num4;
            num2--;
        }
        return num;
    }

    public ClosestHit QueryClosest(Vector3 point)
    {
        ClosestHit best = new ClosestHit
        {
            triangleIndex = -1,
            sqrDistance = float.MaxValue
        };
        if (this.nodes == null || this.nodes.Length == 0)
        {
            return best;
        }
        this.QueryClosestRecursive(0, point, ref best);
        return best;
    }

    public ClosestHit QueryClosest(Vector3 point, HashSet<HumanBodyBones> allowedBones)
    {
        ClosestHit best = new ClosestHit
        {
            triangleIndex = -1,
            sqrDistance = float.MaxValue
        };
        if (this.nodes == null || this.nodes.Length == 0)
        {
            return best;
        }
        if (allowedBones == null || allowedBones.Count == 0)
        {
            this.QueryClosestRecursive(0, point, ref best);
        }
        else
        {
            this.QueryClosestRecursiveFiltered(0, point, ref best, allowedBones);
        }
        return best;
    }

    private void QueryClosestRecursiveFiltered(int nodeIndex, Vector3 p, ref ClosestHit best, HashSet<HumanBodyBones> allowedBones)
    {
        BvhNode bvhNode = this.nodes[nodeIndex];
        if (this.DistanceSqPointAABB(p, bvhNode.bounds) > best.sqrDistance)
        {
            return;
        }
        if (bvhNode.isLeaf)
        {
            int num = bvhNode.start + bvhNode.count;
            for (int i = bvhNode.start; i < num; i++)
            {
                int num2 = this.triIndices[i];
                BvhTriangle bvhTriangle = this.triangles[num2];
                if (allowedBones.Contains(bvhTriangle.mainHumanBone))
                {
                    Vector3 vector = this.triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
                    float sqrMagnitude = (p - vector).sqrMagnitude;
                    if (sqrMagnitude < best.sqrDistance)
                    {
                        best.sqrDistance = sqrMagnitude;
                        best.triangleIndex = num2;
                        best.closestPoint = vector;
                        best.normal = bvhTriangle.normal;
                        best.mainHumanBone = bvhTriangle.mainHumanBone;
                    }
                }
            }
        }
        else
        {
            int leftChild = bvhNode.leftChild;
            int rightChild = bvhNode.rightChild;
            float num3 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
            float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
            if (num3 < num4)
            {
                this.QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
                this.QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
            }
            else
            {
                this.QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
                this.QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
            }
        }
    }

    private void QueryClosestRecursive(int nodeIndex, Vector3 p, ref ClosestHit best)
    {
        BvhNode bvhNode = this.nodes[nodeIndex];
        if (this.DistanceSqPointAABB(p, bvhNode.bounds) > best.sqrDistance)
        {
            return;
        }
        if (bvhNode.isLeaf)
        {
            int num = bvhNode.start + bvhNode.count;
            for (int i = bvhNode.start; i < num; i++)
            {
                int num2 = this.triIndices[i];
                BvhTriangle bvhTriangle = this.triangles[num2];
                Vector3 vector = this.triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
                float sqrMagnitude = (p - vector).sqrMagnitude;
                if (sqrMagnitude < best.sqrDistance)
                {
                    best.sqrDistance = sqrMagnitude;
                    best.triangleIndex = num2;
                    best.closestPoint = vector;
                    best.normal = bvhTriangle.normal;
                    best.mainHumanBone = bvhTriangle.mainHumanBone;
                }
            }
        }
        else
        {
            int leftChild = bvhNode.leftChild;
            int rightChild = bvhNode.rightChild;
            float num3 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
            float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
            if (num3 < num4)
            {
                this.QueryClosestRecursive(leftChild, p, ref best);
                this.QueryClosestRecursive(rightChild, p, ref best);
            }
            else
            {
                this.QueryClosestRecursive(rightChild, p, ref best);
                this.QueryClosestRecursive(leftChild, p, ref best);
            }
        }
    }

    private float DistanceSqPointAABB(Vector3 p, Bounds b)
    {
        float num = Mathf.Max(b.min.x - p.x, 0f, p.x - b.max.x);
        float num2 = Mathf.Max(b.min.y - p.y, 0f, p.y - b.max.y);
        float num3 = Mathf.Max(b.min.z - p.z, 0f, p.z - b.max.z);
        return num * num + num2 * num2 + num3 * num3;
    }

    public int QueryClosestN(Vector3 point, int maxCount, float maxDistance, List<ClosestHit> results)
    {
        results.Clear();
        if (this.nodes == null || this.nodes.Length == 0 || maxCount <= 0)
        {
            return 0;
        }
        float num = maxDistance * maxDistance;
        float currentMaxSq = num;
        this.QueryClosestNRecursive(0, point, maxCount, num, results, ref currentMaxSq);
        return results.Count;
    }

    private void QueryClosestNRecursive(int nodeIndex, Vector3 p, int maxCount, float maxDistanceSq, List<ClosestHit> bestHits, ref float currentMaxSq)
    {
        BvhNode bvhNode = this.nodes[nodeIndex];
        if (this.DistanceSqPointAABB(p, bvhNode.bounds) > currentMaxSq)
        {
            return;
        }
        if (bvhNode.isLeaf)
        {
            int num = bvhNode.start + bvhNode.count;
            for (int i = bvhNode.start; i < num; i++)
            {
                int num2 = this.triIndices[i];
                BvhTriangle bvhTriangle = this.triangles[num2];
                Vector3 vector = this.triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
                float sqrMagnitude = (p - vector).sqrMagnitude;
                if (sqrMagnitude > maxDistanceSq)
                {
                    continue;
                }
                if (bestHits.Count < maxCount)
                {
                    bestHits.Add(new ClosestHit
                    {
                        triangleIndex = num2,
                        closestPoint = vector,
                        normal = bvhTriangle.normal,
                        sqrDistance = sqrMagnitude,
                        mainHumanBone = bvhTriangle.mainHumanBone
                    });
                    if (bestHits.Count == maxCount)
                    {
                        currentMaxSq = this.GetMaxSqrDistance(bestHits, maxDistanceSq);
                    }
                }
                else
                {
                    if (sqrMagnitude >= currentMaxSq)
                    {
                        continue;
                    }
                    int index = 0;
                    float sqrDistance = bestHits[0].sqrDistance;
                    for (int j = 1; j < bestHits.Count; j++)
                    {
                        if (bestHits[j].sqrDistance > sqrDistance)
                        {
                            sqrDistance = bestHits[j].sqrDistance;
                            index = j;
                        }
                    }
                    bestHits[index] = new ClosestHit
                    {
                        triangleIndex = num2,
                        closestPoint = vector,
                        normal = bvhTriangle.normal,
                        sqrDistance = sqrMagnitude
                    };
                    currentMaxSq = this.GetMaxSqrDistance(bestHits, maxDistanceSq);
                }
            }
        }
        else
        {
            int leftChild = bvhNode.leftChild;
            int rightChild = bvhNode.rightChild;
            float num3 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
            float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
            if (num3 < num4)
            {
                this.QueryClosestNRecursive(leftChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
                this.QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
            }
            else
            {
                this.QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
                this.QueryClosestNRecursive(leftChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
            }
        }
    }

    private float GetMaxSqrDistance(List<ClosestHit> bestHits, float maxDistanceSq)
    {
        if (bestHits.Count == 0)
        {
            return maxDistanceSq;
        }
        float sqrDistance = bestHits[0].sqrDistance;
        for (int i = 1; i < bestHits.Count; i++)
        {
            if (bestHits[i].sqrDistance > sqrDistance)
            {
                sqrDistance = bestHits[i].sqrDistance;
            }
        }
        return Mathf.Min(sqrDistance, maxDistanceSq);
    }

    public Vector3 ComputeMoveVectorToSurface(Vector3 p, float targetGap = 0f)
    {
        ClosestHit closestHit = this.QueryClosest(p);
        if (closestHit.triangleIndex < 0)
        {
            return Vector3.zero;
        }
        Vector3 result = closestHit.closestPoint - p;
        if (targetGap > 0f)
        {
            result += closestHit.normal.normalized * targetGap;
        }
        return result;
    }
}
