ulong GetPathPolyByPosition(ulong[] polyPath, uint polyPathSize, float[] point, ref float distance) { if (polyPath == null || polyPathSize == 0) { return(0); } ulong nearestPoly = 0; float minDist2d = float.MaxValue; float minDist3d = 0.0f; for (uint i = 0; i < polyPathSize; ++i) { float[] closestPoint = new float[3]; bool posOverPoly = false; if (Detour.dtStatusFailed(_navMeshQuery.closestPointOnPoly(polyPath[i], point, closestPoint, ref posOverPoly))) { continue; } float d = Detour.dtVdist2DSqr(point, closestPoint); if (d < minDist2d) { minDist2d = d; nearestPoly = polyPath[i]; minDist3d = Detour.dtVdistSqr(point, closestPoint); } if (minDist2d < 1.0f) // shortcut out - close enough for us { break; } } distance = (float)Math.Sqrt(minDist3d); return((minDist2d < 3.0f) ? nearestPoly : 0u); }