BITKit/Src/Unity/Scripts/Physics/GetClosestPointFromMesh.cs

using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEditor;
using UnityEngine;

namespace BITKit.Physics
{
    public static class PhysicsExtensions
    {
        public static bool TryGetClosestPointFromCollider(this Collider collider,Vector3 point,out Vector3 closestPoint)
        {
            closestPoint = default;
            switch (collider)
            {
                case BoxCollider:
                case SphereCollider:
                case CapsuleCollider:
                case MeshCollider { convex: true }:
                case TerrainCollider:
                    closestPoint = collider.ClosestPoint(point);
                    return true;
                case MeshCollider { convex: false } meshCollider when new GetClosestPointFromMesh(meshCollider.sharedMesh,point).TryGetValue(out closestPoint,out _):
                    closestPoint = collider.transform.TransformPoint(closestPoint);
                    return true;
            }

            return false;
        }
    }
    public readonly struct GetClosestPointFromMesh:IClosePointProvider
    {
        private readonly Vector3 _position;
        private readonly Mesh _mesh;

        public GetClosestPointFromMesh(Mesh mesh, Vector3 position)
        {
            _mesh = mesh;
            _position = position;
        }

        public bool TryGetValue(out Vector3 position, out Collider collider)
        {
            position = default;
            collider = default;
            
            if (_mesh.isReadable is false) return false;
            
            
            var vertices = _mesh.vertices;
            
            if (vertices.Length > 256) return false;
            
            var minPos = new Vector3(64, 64, 64);
            var minDistance = 64f;

            for (var index = 0; index < _mesh.triangles.Length; index+=3)
            {
                var x = vertices[_mesh.triangles[index]];

                if (Vector3.Distance(x, _position) > minDistance)
                {
                    continue;
                }
                
                var y = vertices[_mesh.triangles[index + 1]];
                var z = vertices[_mesh.triangles[index + 2]];

                var pos = GeometryUtils.GetPosInTriangle(x, y, z, _position);

                var distance = Vector3.Distance(pos, _position);

                if (!(distance < minDistance)) continue;
                
                minPos = pos;
                minDistance = distance;
            }

            position = minPos;

            return true;
        }
    }
}