From 19e2d8dbe61c25c5aa447a0dd0b802d472b83dc0 Mon Sep 17 00:00:00 2001 From: Mikko Mononen Date: Sun, 16 Feb 2014 11:27:45 +0200 Subject: [PATCH] Fixed pos-over-poly case for finding nearest polygon --- Detour/Source/DetourNavMesh.cpp | 16 ++++++++-------- Detour/Source/DetourNavMeshQuery.cpp | 15 ++++++++------- RecastDemo/Bin/Tests/raycast_test.txt | 1 + 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 1063b10..9d627be 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -711,13 +711,13 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; - bool nearestOverPoly = false; 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 @@ -725,18 +725,18 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, dtVsub(diff, center, closestPtPoly); if (posOverPoly) { - const dtMeshTile* tile = 0; - if (dtAbs(diff[1]) > tile->header->walkableClimb) - posOverPoly = false; + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); } - float d = dtVlenSqr(diff); - - if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly)) + if (d < nearestDistanceSqr) { dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; - nearestOverPoly = posOverPoly; nearest = ref; } } diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index ba67957..cc16946 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -723,13 +723,13 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; - bool nearestOverPoly = false; 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 @@ -740,18 +740,19 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten const dtMeshTile* tile = 0; const dtPoly* poly = 0; m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly); - if (dtAbs(diff[1]) > tile->header->walkableClimb) - posOverPoly = false; + d = dtAbs(diff[1]) - tile->header->walkableClimb; + d = d > 0 ? d*d : 0; + } + else + { + d = dtVlenSqr(diff); } - float d = dtVlenSqr(diff); - - if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly)) + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; - nearestOverPoly = posOverPoly; nearest = ref; } } diff --git a/RecastDemo/Bin/Tests/raycast_test.txt b/RecastDemo/Bin/Tests/raycast_test.txt index a410aea..3fde0ee 100644 --- a/RecastDemo/Bin/Tests/raycast_test.txt +++ b/RecastDemo/Bin/Tests/raycast_test.txt @@ -3,3 +3,4 @@ f nav_test.obj rc 45.133884 -0.533207 -3.775568 47.078232 7.797605 14.293253 0xffef 0x0 rc 52.979847 -2.778793 -2.914886 50.628868 -2.350212 13.917850 0xffef 0x0 rc 45.209217 2.024442 1.838851 46.888412 7.797606 15.772338 0xffef 0x0 +rc 45.388317 -0.562073 -3.673226 46.651001 7.797606 15.513507 0xffef 0x0 \ No newline at end of file