dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) { float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; float closestPtPoly[3]; if (closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly) != DT_SUCCESS) continue; float d = dtVdistSqr(center, closestPtPoly); if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } } return nearest; }
dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const { float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; float closestPtPoly[3]; float diff[3]; bool posOverPoly = false; float d = 0; closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly); // If a point is directly over a polygon and closer than // climb height, favor that instead of straight line nearest point. dtVsub(diff, center, closestPtPoly); if (posOverPoly) { d = dtAbs(diff[1]) - tile->header->walkableClimb; d = d > 0 ? d*d : 0; } else { d = dtVlenSqr(diff); } if (d < nearestDistanceSqr) { dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; } } return nearest; }