DotRecastNetSim/src/DotRecast.Detour/FindNearestPolyQuery.cs

using System;

namespace DotRecast.Detour
{
    using static DotRecast.Core.RecastMath;

    public class FindNearestPolyQuery : PolyQuery
    {
        private readonly NavMeshQuery query;
        private readonly float[] center;
        private long nearestRef;
        private float[] nearestPt;
        private bool overPoly;
        private float nearestDistanceSqr;

        public FindNearestPolyQuery(NavMeshQuery query, float[] center)
        {
            this.query = query;
            this.center = center;
            nearestDistanceSqr = float.MaxValue;
            nearestPt = new float[] { center[0], center[1], center[2] };
        }

        public void process(MeshTile tile, Poly poly, long refs)
        {
            // Find nearest polygon amongst the nearby polygons.
            Result<ClosestPointOnPolyResult> closest = query.closestPointOnPoly(refs, center);
            bool posOverPoly = closest.result.isPosOverPoly();
            float[] closestPtPoly = closest.result.getClosest();

            // If a point is directly over a polygon and closer than
            // climb height, favor that instead of straight line nearest point.
            float d = 0;
            float[] diff = vSub(center, closestPtPoly);
            if (posOverPoly)
            {
                d = Math.Abs(diff[1]) - tile.data.header.walkableClimb;
                d = d > 0 ? d * d : 0;
            }
            else
            {
                d = vLenSqr(diff);
            }

            if (d < nearestDistanceSqr)
            {
                nearestPt = closestPtPoly;
                nearestDistanceSqr = d;
                nearestRef = refs;
                overPoly = posOverPoly;
            }
        }

        public FindNearestPolyResult result()
        {
            return new FindNearestPolyResult(nearestRef, nearestPt, overPoly);
        }
    }
}