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