From e2722403bebf119d6f8246ab746c56073e91d461 Mon Sep 17 00:00:00 2001 From: axelrodR Date: Mon, 10 Feb 2014 00:30:46 +0200 Subject: [PATCH 1/3] bugfix: raycasts sometimes fail when the origin is near a boundary of a triangle with a slope. The fix of considers polygons to have a thickness for the purpose of finding closest nodes. --- Detour/Include/DetourNavMesh.h | 10 +++- Detour/Include/DetourNavMeshQuery.h | 8 +-- Detour/Source/DetourNavMesh.cpp | 34 ++++++++++--- Detour/Source/DetourNavMeshQuery.cpp | 75 +++++++++++++++++++++++----- 4 files changed, 103 insertions(+), 24 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index d87fee8..a90c1e2 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -94,6 +94,10 @@ static const unsigned int DT_OFFMESH_CON_BIDIR = 1; /// @ingroup detour static const int DT_MAX_AREAS = 64; +/// Polygon thickness relative to agent height (for correct scaling) +/// We define the polygon to have a slight thickness so that things slightly above and below will be considered on the polygon. +static const float SCALE_POLY_THICK = 1.f / 6.f; + /// Tile flags used for various functions and fields. /// For an example, see dtNavMesh::addTile(). enum dtTileFlags @@ -612,8 +616,10 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const; - /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, + + /// find the closest point on polygon. + /// @return whether the point is straight above the closest polygon + bool closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const; dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice. diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 6edf5bf..8f6589e 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -378,8 +378,9 @@ public: /// @param[in] ref The reference id of the polygon. /// @param[in] pos The position to check. [(x, y, z)] /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] inside2D true if the closest point is above/below the polygon /// @returns The status flags for the query. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* inside2D=0) const; /// Returns a point on the boundary closest to the source point if the source point is outside the /// polygon's xz-bounds. @@ -431,8 +432,9 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, const dtQueryFilter* filter, float* nearestPt) const; - /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; + /// finds the closest point on a polygon. + /// returns whether the point is inside the polygon (in 2D) + bool closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; /// Returns portal points between two polygons. dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 3bc2b73..407611b 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -617,7 +617,7 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) } } -void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, +bool dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, const float* pos, float* closest) const { const dtPoly* poly = &tile->polys[ip]; @@ -630,7 +630,7 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); - return; + return false; } const dtPolyDetail* pd = &tile->detailMeshes[ip]; @@ -644,7 +644,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (!inside) { // Point is outside the polygon, dtClamp to nearest edge. float dmin = FLT_MAX; @@ -681,6 +682,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip break; } } + + return inside; } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, @@ -694,6 +697,9 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, // Get nearby polygons from proximity grid. dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128); + + // tile thickness is scaled relative to an agent height + const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK; // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; @@ -701,15 +707,31 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2D = closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); + + dtVsub(offset, center, closestPtPoly); + + if (inside2D) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 1beb065..052bbf6 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -501,7 +501,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f /// /// See closestPointOnPolyBoundary() for a limited but faster option. /// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* inside2D) const { dtAssert(m_nav); const dtMeshTile* tile = 0; @@ -511,12 +511,14 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo if (!tile) return DT_FAILURE | DT_INVALID_PARAM; - closestPointOnPolyInTile(tile, poly, pos, closest); + bool in2D = closestPointOnPolyInTile(tile, poly, pos, closest); + if (inside2D) + *inside2D = in2D; return DT_SUCCESS; } -void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, +bool dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const { // Off-mesh connections don't have detail polygons. @@ -528,7 +530,7 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); - return; + return false; } const unsigned int ip = (unsigned int)(poly - tile->polys); @@ -543,7 +545,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) + bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget); + if (!inside) { // Point is outside the polygon, dtClamp to nearest edge. float dmin = FLT_MAX; @@ -581,6 +584,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo } } + return inside; + /* float closestDistSqr = FLT_MAX; for (int j = 0; j < pd->triCount; ++j) { @@ -739,6 +744,15 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten int polyCount = 0; if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128))) return DT_FAILURE | DT_INVALID_PARAM; + + if (polyCount == 0) + return DT_SUCCESS; + + // tile thickness is scaled relative to an agent height + const dtMeshTile* tile0; + const dtPoly* poly0; + m_nav->getTileAndPolyByRefUnsafe(polys[0], &tile0, &poly0); + const float polyThick = tile0->header->walkableHeight * SCALE_POLY_THICK; // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; @@ -746,15 +760,32 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten for (int i = 0; i < polyCount; ++i) { dtPolyRef ref = polys[i]; - float closestPtPoly[3]; - closestPointOnPoly(ref, center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2d; + closestPointOnPoly(ref, center, closestPtPoly, &inside2d); + + dtVsub(offset, center, closestPtPoly); + + if (inside2d) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } @@ -768,7 +799,7 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl const dtQueryFilter* filter, float* nearestPt) const { dtAssert(m_nav); - + float bmin[3], bmax[3]; dtVsub(bmin, center, extents); dtVadd(bmax, center, extents); @@ -777,6 +808,9 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl dtPolyRef polys[128]; int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128); + // tile thickness is scaled relative to an agent height + const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK; + // Find nearest polygon amongst the nearby polygons. dtPolyRef nearest = 0; float nearestDistanceSqr = FLT_MAX; @@ -784,16 +818,31 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl { dtPolyRef ref = polys[i]; const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - closestPointOnPolyInTile(tile, poly, center, closestPtPoly); - - float d = dtVdistSqr(center, closestPtPoly); + float d, closestPtPoly[3], offset[3]; + bool inside2d = closestPointOnPolyInTile(tile, poly, center, closestPtPoly); + + dtVsub(offset, center, closestPtPoly); + + if (inside2d) + { + // right above the polygon is considered on the polygon + d = dtAbs(offset[1]) - polyThick; + d = d > 0 ? d*d : 0; + } + else + d = dtVlenSqr(offset); + if (d < nearestDistanceSqr) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; nearest = ref; + if (d == 0) + { + nearestDistanceSqr = 0; + break; + } } } From 6702143bc1d526e0ea98b0c1b6bcdc31490c5155 Mon Sep 17 00:00:00 2001 From: Mikko Mononen Date: Tue, 11 Feb 2014 20:31:52 +0200 Subject: [PATCH 2/3] Fix for broken raycasts, changes how findNearestPoly works, slight API change - test case supports raycasts - changes how findNearestPoly works directly over polygons - API change: findNearestPoly has additional parameter --- Detour/Include/DetourNavMesh.h | 3 +- Detour/Include/DetourNavMeshQuery.h | 10 +- Detour/Source/DetourNavMesh.cpp | 46 ++++++-- Detour/Source/DetourNavMeshQuery.cpp | 108 ++++++------------ DetourCrowd/Source/DetourCrowd.cpp | 6 +- RecastDemo/Bin/Tests/raycast_test.txt | 5 + RecastDemo/Include/TestCase.h | 2 + RecastDemo/Source/NavMeshTesterTool.cpp | 12 +- RecastDemo/Source/TestCase.cpp | 139 +++++++++++++++++++----- RecastDemo/Source/main.cpp | 7 +- 10 files changed, 210 insertions(+), 128 deletions(-) create mode 100644 RecastDemo/Bin/Tests/raycast_test.txt diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index d87fee8..95a63e4 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -613,8 +613,7 @@ private: dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, float* nearestPt) const; /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const; + void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; dtNavMeshParams m_params; ///< Current initialization params. TODO: do not store this info twice. float m_orig[3]; ///< Origin of the tile (0,0) diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 6edf5bf..4a5112c 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -378,8 +378,9 @@ public: /// @param[in] ref The reference id of the polygon. /// @param[in] pos The position to check. [(x, y, z)] /// @param[out] closest The closest point on the polygon. [(x, y, z)] + /// @param[out] posOverPoly True of the position is over the polygon. /// @returns The status flags for the query. - dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const; + dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; /// Returns a point on the boundary closest to the source point if the source point is outside the /// polygon's xz-bounds. @@ -428,12 +429,7 @@ private: /// Queries polygons within a tile. int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyRef* polys, const int maxPolys) const; - /// Find nearest polygon within a tile. - dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const; - /// Returns closest point on polygon. - void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const; - + /// Returns portal points between two polygons. dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right, unsigned char& fromType, unsigned char& toType) const; diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 3bc2b73..1063b10 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -617,10 +617,12 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) } } -void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip, - const float* pos, float* closest) const +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { - const dtPoly* poly = &tile->polys[ip]; + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); + // Off-mesh connections don't have detail polygons. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) { @@ -630,11 +632,14 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); + if (posOverPoly) + *posOverPoly = false; return; } + const unsigned int ip = (unsigned int)(poly - tile->polys); const dtPolyDetail* pd = &tile->detailMeshes[ip]; - + // Clamp point to be inside the polygon. float verts[DT_VERTS_PER_POLYGON*3]; float edged[DT_VERTS_PER_POLYGON]; @@ -660,6 +665,14 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip const float* va = &verts[imin*3]; const float* vb = &verts[((imin+1)%nv)*3]; dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; } // Find height at the location. @@ -698,17 +711,32 @@ 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]; - closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) + float diff[3]; + bool posOverPoly = false; + 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) { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); + const dtMeshTile* tile = 0; + if (dtAbs(diff[1]) > tile->header->walkableClimb) + posOverPoly = false; + } + + float d = dtVlenSqr(diff); + + if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly)) + { + dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; + nearestOverPoly = posOverPoly; nearest = ref; } } diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index bdbee00..ba67957 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -501,7 +501,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f /// /// See closestPointOnPolyBoundary() for a limited but faster option. /// -dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const +dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const { dtAssert(m_nav); const dtMeshTile* tile = 0; @@ -511,14 +511,6 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo if (!tile) return DT_FAILURE | DT_INVALID_PARAM; - closestPointOnPolyInTile(tile, poly, pos, closest); - - return DT_SUCCESS; -} - -void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, - const float* pos, float* closest) const -{ // Off-mesh connections don't have detail polygons. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) { @@ -528,7 +520,9 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo const float d1 = dtVdist(pos, v1); const float u = d0 / (d0+d1); dtVlerp(closest, v0, v1, u); - return; + if (posOverPoly) + *posOverPoly = false; + return DT_SUCCESS; } const unsigned int ip = (unsigned int)(poly - tile->polys); @@ -559,6 +553,14 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo const float* va = &verts[imin*3]; const float* vb = &verts[((imin+1)%nv)*3]; dtVlerp(closest, va, vb, edget[imin]); + + if (posOverPoly) + *posOverPoly = false; + } + else + { + if (posOverPoly) + *posOverPoly = true; } // Find height at the location. @@ -580,30 +582,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo break; } } - -/* float closestDistSqr = FLT_MAX; - for (int j = 0; j < pd->triCount; ++j) - { - const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4]; - const float* v[3]; - for (int k = 0; k < 3; ++k) - { - if (t[k] < poly->vertCount) - v[k] = &tile->verts[poly->verts[t[k]]*3]; - else - v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; - } - - float pt[3]; - dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]); - float d = dtVdistSqr(pos, pt); - - if (d < closestDistSqr) - { - dtVcopy(closest, pt); - closestDistSqr = d; - } - }*/ + + return DT_SUCCESS; } /// @par @@ -743,17 +723,35 @@ 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]; - closestPointOnPoly(ref, center, closestPtPoly); - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) + float diff[3]; + bool posOverPoly = false; + 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) + { + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly); + if (dtAbs(diff[1]) > tile->header->walkableClimb) + posOverPoly = false; + } + + float d = dtVlenSqr(diff); + + if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly)) { if (nearestPt) dtVcopy(nearestPt, closestPtPoly); nearestDistanceSqr = d; + nearestOverPoly = posOverPoly; nearest = ref; } } @@ -764,42 +762,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten return DT_SUCCESS; } -dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents, - const dtQueryFilter* filter, float* nearestPt) const -{ - dtAssert(m_nav); - - 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, filter, 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]; - const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)]; - float closestPtPoly[3]; - closestPointOnPolyInTile(tile, poly, center, closestPtPoly); - - float d = dtVdistSqr(center, closestPtPoly); - if (d < nearestDistanceSqr) - { - if (nearestPt) - dtVcopy(nearestPt, closestPtPoly); - nearestDistanceSqr = d; - nearest = ref; - } - } - - return nearest; -} - int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter, dtPolyRef* polys, const int maxPolys) const diff --git a/DetourCrowd/Source/DetourCrowd.cpp b/DetourCrowd/Source/DetourCrowd.cpp index 42b5b19..cf7a027 100644 --- a/DetourCrowd/Source/DetourCrowd.cpp +++ b/DetourCrowd/Source/DetourCrowd.cpp @@ -711,7 +711,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) if (reqPath[reqPathCount-1] != ag->targetRef) { // Partial path, constrain target position inside the last polygon. - status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos); + status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0); if (dtStatusFailed(status)) reqPathCount = 0; } @@ -855,7 +855,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/) { // Partial path, constrain target position inside the last polygon. float nearest[3]; - status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest); + status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0); if (dtStatusSucceed(status)) dtVcopy(targetPos, nearest); else @@ -949,7 +949,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const float nearest[3]; dtVcopy(nearest, agentPos); agentRef = 0; - dtStatus status = m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest); + m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest); dtVcopy(agentPos, nearest); if (!agentRef) diff --git a/RecastDemo/Bin/Tests/raycast_test.txt b/RecastDemo/Bin/Tests/raycast_test.txt new file mode 100644 index 0000000..a410aea --- /dev/null +++ b/RecastDemo/Bin/Tests/raycast_test.txt @@ -0,0 +1,5 @@ +s Tile Mesh +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 diff --git a/RecastDemo/Include/TestCase.h b/RecastDemo/Include/TestCase.h index 19d1b91..74994f1 100644 --- a/RecastDemo/Include/TestCase.h +++ b/RecastDemo/Include/TestCase.h @@ -26,6 +26,7 @@ class TestCase enum TestType { TEST_PATHFIND, + TEST_RAYCAST, }; struct Test @@ -39,6 +40,7 @@ class TestCase TestType type; float spos[3], epos[3]; + float nspos[3], nepos[3]; float radius; int includeFlags, excludeFlags; bool expand; diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 3b49d4b..9839d2f 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -506,8 +506,8 @@ void NavMeshTesterTool::handleToggle() if (m_pathIterPolyCount) { // Iterate over the path to find smooth path on the detail mesh surface. - m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos); - m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos); + m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos, 0); + m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos, 0); m_nsmoothPath = 0; @@ -650,7 +650,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/) float epos[3]; dtVcopy(epos, m_epos); if (m_polys[m_npolys-1] != m_endRef) - m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos); + m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0); m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys, m_straightPath, m_straightPathFlags, @@ -715,8 +715,8 @@ void NavMeshTesterTool::recalc() int npolys = m_npolys; float iterPos[3], targetPos[3]; - m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos); - m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos); + m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos, 0); + m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos, 0); static const float STEP_SIZE = 0.5f; static const float SLOP = 0.01f; @@ -855,7 +855,7 @@ void NavMeshTesterTool::recalc() float epos[3]; dtVcopy(epos, m_epos); if (m_polys[m_npolys-1] != m_endRef) - m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos); + m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0); m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys, m_straightPath, m_straightPathFlags, diff --git a/RecastDemo/Source/TestCase.cpp b/RecastDemo/Source/TestCase.cpp index 95293f5..7d4052f 100644 --- a/RecastDemo/Source/TestCase.cpp +++ b/RecastDemo/Source/TestCase.cpp @@ -141,6 +141,20 @@ bool TestCase::load(const char* filePath) &test->epos[0], &test->epos[1], &test->epos[2], &test->includeFlags, &test->excludeFlags); } + else if (row[0] == 'r' && row[1] == 'c') + { + // Pathfind test. + Test* test = new Test; + memset(test, 0, sizeof(Test)); + test->type = TEST_RAYCAST; + test->expand = false; + test->next = m_tests; + m_tests = test; + sscanf(row+2, "%f %f %f %f %f %f %x %x", + &test->spos[0], &test->spos[1], &test->spos[2], + &test->epos[0], &test->epos[1], &test->epos[2], + &test->includeFlags, &test->excludeFlags); + } } delete [] buf; @@ -187,8 +201,8 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery) TimeVal findNearestPolyStart = getPerfTime(); dtPolyRef startRef, endRef; - navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, 0); - navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, 0); + navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, iter->nspos); + navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, iter->nepos); TimeVal findNearestPolyEnd = getPerfTime(); iter->findNearestPolyTime += getPerfDeltaTimeUsec(findNearestPolyStart, findNearestPolyEnd); @@ -196,35 +210,82 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery) if (!startRef || ! endRef) continue; - // Find path - TimeVal findPathStart = getPerfTime(); + if (iter->type == TEST_PATHFIND) + { + // Find path + TimeVal findPathStart = getPerfTime(); - navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS); - - TimeVal findPathEnd = getPerfTime(); - iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd); - - // Find straight path - if (iter->npolys) - { - TimeVal findStraightPathStart = getPerfTime(); + navquery->findPath(startRef, endRef, iter->spos, iter->epos, &filter, polys, &iter->npolys, MAX_POLYS); - navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys, - straight, 0, 0, &iter->nstraight, MAX_POLYS); - TimeVal findStraightPathEnd = getPerfTime(); - iter->findStraightPathTime += getPerfDeltaTimeUsec(findStraightPathStart, findStraightPathEnd); - } + TimeVal findPathEnd = getPerfTime(); + iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd); - // Copy results - if (iter->npolys) - { - iter->polys = new dtPolyRef[iter->npolys]; - memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys); + // Find straight path + if (iter->npolys) + { + TimeVal findStraightPathStart = getPerfTime(); + + navquery->findStraightPath(iter->spos, iter->epos, polys, iter->npolys, + straight, 0, 0, &iter->nstraight, MAX_POLYS); + TimeVal findStraightPathEnd = getPerfTime(); + iter->findStraightPathTime += getPerfDeltaTimeUsec(findStraightPathStart, findStraightPathEnd); + } + + // Copy results + if (iter->npolys) + { + iter->polys = new dtPolyRef[iter->npolys]; + memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys); + } + if (iter->nstraight) + { + iter->straight = new float[iter->nstraight*3]; + memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight); + } } - if (iter->nstraight) + else if (iter->type == TEST_RAYCAST) { - iter->straight = new float[iter->nstraight*3]; - memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight); + float t = 0; + float hitNormal[3], hitPos[3]; + + iter->straight = new float[2*3]; + iter->nstraight = 2; + + iter->straight[0] = iter->spos[0]; + iter->straight[1] = iter->spos[1]; + iter->straight[2] = iter->spos[2]; + + TimeVal findPathStart = getPerfTime(); + + navquery->raycast(startRef, iter->spos, iter->epos, &filter, &t, hitNormal, polys, &iter->npolys, MAX_POLYS); + + TimeVal findPathEnd = getPerfTime(); + iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd); + + if (t > 1) + { + // No hit + dtVcopy(hitPos, iter->epos); + } + else + { + // Hit + dtVlerp(hitPos, iter->spos, iter->epos, t); + } + // Adjust height. + if (iter->npolys > 0) + { + float h = 0; + navquery->getPolyHeight(polys[iter->npolys-1], hitPos, &h); + hitPos[1] = h; + } + dtVcopy(&iter->straight[3], hitPos); + + if (iter->npolys) + { + iter->polys = new dtPolyRef[iter->npolys]; + memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys); + } } } @@ -259,6 +320,32 @@ void TestCase::handleRender() glColor4ub(51,102,0,129); glVertex3f(iter->epos[0],iter->epos[1]-0.3f,iter->epos[2]); glVertex3f(iter->epos[0],iter->epos[1]+0.3f,iter->epos[2]); + + if (iter->expand) + { + const float s = 0.1f; + glColor4ub(255,32,0,128); + glVertex3f(iter->spos[0]-s,iter->spos[1],iter->spos[2]); + glVertex3f(iter->spos[0]+s,iter->spos[1],iter->spos[2]); + glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]-s); + glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]+s); + glColor4ub(255,192,0,255); + glVertex3f(iter->nspos[0]-s,iter->nspos[1],iter->nspos[2]); + glVertex3f(iter->nspos[0]+s,iter->nspos[1],iter->nspos[2]); + glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]-s); + glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]+s); + + glColor4ub(255,32,0,128); + glVertex3f(iter->epos[0]-s,iter->epos[1],iter->epos[2]); + glVertex3f(iter->epos[0]+s,iter->epos[1],iter->epos[2]); + glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]-s); + glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]+s); + glColor4ub(255,192,0,255); + glVertex3f(iter->nepos[0]-s,iter->nepos[1],iter->nepos[2]); + glVertex3f(iter->nepos[0]+s,iter->nepos[1],iter->nepos[2]); + glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]-s); + glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]+s); + } if (iter->expand) glColor4ub(255,192,0,255); diff --git a/RecastDemo/Source/main.cpp b/RecastDemo/Source/main.cpp index fc85942..248e2fb 100644 --- a/RecastDemo/Source/main.cpp +++ b/RecastDemo/Source/main.cpp @@ -817,7 +817,6 @@ int main(int /*argc*/, char** /*argv*/) { delete geom; geom = 0; - showLog = true; logScroll = 0; ctx.dumpLog("Geom load log %s:", meshName); @@ -827,6 +826,10 @@ int main(int /*argc*/, char** /*argv*/) sample->handleMeshChanged(geom); } + // This will ensure that tile & poly bits are updated in tiled sample. + if (sample) + sample->handleSettings(); + ctx.resetLog(); if (sample && !sample->handleBuild()) { @@ -860,7 +863,7 @@ int main(int /*argc*/, char** /*argv*/) } rx = 45; ry = -45; - glFogf(GL_FOG_START, camr*0.1f); + glFogf(GL_FOG_START, camr*0.2f); glFogf(GL_FOG_END, camr*1.25f); } From 19e2d8dbe61c25c5aa447a0dd0b802d472b83dc0 Mon Sep 17 00:00:00 2001 From: Mikko Mononen Date: Sun, 16 Feb 2014 11:27:45 +0200 Subject: [PATCH 3/3] 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