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);
}
