From 54bb0943e5174a71eeeca11919920f685760a4f0 Mon Sep 17 00:00:00 2001 From: Jakob Botsch Nielsen Date: Sat, 16 Mar 2019 15:03:50 +0100 Subject: [PATCH] Make closestPointOnPoly and getPolyHeight more robust * Make dtClosestHeightPointTriangle use no epsilon (except for checking denominator) * When point is outside poly, directly find closest height from boundary detail edges instead of first interpolating and then finding height from detail tris. * When point is inside poly, if all dtClosestHeightPointTriangle queries failed, then find height from detail edges. This should only happen if the point is right on top of an internal detail tri edge. --- Detour/Include/DetourNavMesh.h | 2 + Detour/Source/DetourCommon.cpp | 18 +-- Detour/Source/DetourNavMesh.cpp | 160 +++++++++++++++++++-------- Detour/Source/DetourNavMeshQuery.cpp | 45 +++----- 4 files changed, 135 insertions(+), 90 deletions(-) diff --git a/Detour/Include/DetourNavMesh.h b/Detour/Include/DetourNavMesh.h index 42affe2..98293c4 100644 --- a/Detour/Include/DetourNavMesh.h +++ b/Detour/Include/DetourNavMesh.h @@ -651,6 +651,8 @@ private: /// Find nearest polygon within a tile. dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* halfExtents, float* nearestPt) const; + /// Returns whether position is over the poly and the height at the position if so. + bool getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const; /// Returns closest point on polygon. void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const; diff --git a/Detour/Source/DetourCommon.cpp b/Detour/Source/DetourCommon.cpp index 3886f14..b89d751 100644 --- a/Detour/Source/DetourCommon.cpp +++ b/Detour/Source/DetourCommon.cpp @@ -203,14 +203,18 @@ void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const floa bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) { + const float EPS = 1e-6f; float v0[3], v1[3], v2[3]; - dtVsub(v0, c,a); - dtVsub(v1, b,a); - dtVsub(v2, p,a); + dtVsub(v0, c, a); + dtVsub(v1, b, a); + dtVsub(v2, p, a); // Compute scaled barycentric coordinates float denom = v0[0] * v1[2] - v0[2] * v1[0]; + if (fabsf(denom) < EPS) + return false; + float u = v1[2] * v2[0] - v1[0] * v2[2]; float v = v0[0] * v2[2] - v0[2] * v2[0]; @@ -220,13 +224,9 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b v = -v; } - // The (sloppy) epsilon is needed to allow to get height of points which - // are interpolated along the edges of the triangles. - float epsilon = - 1e-4f * denom; - // If point lies inside the triangle, return interpolated ycoord. - if (u >= epsilon && v >= epsilon && (u+v) <= denom - epsilon) { - h = a[1] + (v0[1]*u + v1[1]*v) / denom; + if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) { + h = a[1] + (v0[1] * u + v1[1] * v) / denom; return true; } return false; diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp index 17df26d..a655d1d 100644 --- a/Detour/Source/DetourNavMesh.cpp +++ b/Detour/Source/DetourNavMesh.cpp @@ -616,63 +616,84 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile) } } -void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +namespace { - 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) + template + void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) { - const float* v0 = &tile->verts[poly->verts[0]*3]; - const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist(pos, v0); - 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]; + + float dmin = FLT_MAX; + float tmin = 0; + const float* pmin = 0; + const float* pmax = 0; + + for (int i = 0; i < pd->triCount; i++) + { + const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4]; + const int ANY_BOUNDARY_EDGE = + (DT_DETAIL_EDGE_BOUNDARY << 0) | + (DT_DETAIL_EDGE_BOUNDARY << 2) | + (DT_DETAIL_EDGE_BOUNDARY << 4); + if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0) + continue; + + const float* v[3]; + for (int j = 0; j < 3; ++j) + { + if (tris[j] < poly->vertCount) + v[j] = &tile->verts[poly->verts[tris[j]] * 3]; + else + v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3]; + } + + for (int k = 0, j = 2; k < 3; j = k++) + { + if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 && + (onlyBoundary || tris[j] < tris[k])) + { + // Only looking at boundary edges and this is internal, or + // this is an inner edge that we will see again or have already seen. + continue; + } + + float t; + float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t); + if (d < dmin) + { + dmin = d; + tmin = t; + pmin = v[j]; + pmax = v[k]; + } + } + } + + dtVlerp(closest, pmin, pmax, tmin); } - +} + +bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const +{ + // Off-mesh connections do not have detail polys and getting height + // over them does not make sense. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + return false; + 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]; - float edget[DT_VERTS_PER_POLYGON]; const int nv = poly->vertCount; for (int i = 0; i < nv; ++i) dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]); - dtVcopy(closest, pos); - if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) - { - // Point is outside the polygon, dtClamp to nearest edge. - float dmin = edged[0]; - int imin = 0; - for (int i = 1; i < nv; ++i) - { - if (edged[i] < dmin) - { - dmin = edged[i]; - imin = i; - } - } - 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; - } + if (!dtPointInPolygon(pos, verts, nv)) + return false; + + if (!height) + return true; // Find height at the location. for (int j = 0; j < pd->triCount; ++j) @@ -687,12 +708,53 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3]; } float h; - if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h)) + if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) { - closest[1] = h; - break; + *height = h; + return true; } } + + // If all triangle checks failed above (can happen with degenerate triangles + // or larger floating point values) the point is on an edge, so just select + // closest. This should almost never happen so the extra iteration here is + // ok. + float closest[3]; + closestPointOnDetailEdges(tile, poly, pos, closest); + *height = closest[1]; + return true; +} + +void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const +{ + const dtMeshTile* tile = 0; + const dtPoly* poly = 0; + getTileAndPolyByRefUnsafe(ref, &tile, &poly); + + dtVcopy(closest, pos); + if (getPolyHeight(tile, poly, pos, &closest[1])) + { + if (posOverPoly) + *posOverPoly = true; + return; + } + + if (posOverPoly) + *posOverPoly = false; + + // Off-mesh connections don't have detail polygons. + if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) + { + const float* v0 = &tile->verts[poly->verts[0]*3]; + const float* v1 = &tile->verts[poly->verts[1]*3]; + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); + dtVlerp(closest, v0, v1, t); + return; + } + + // Outside poly that is not an offmesh connection. + closestPointOnDetailEdges(tile, poly, pos, closest); } dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile, diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 520f915..839ee1e 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -588,7 +588,7 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float* /// @par /// -/// Will return #DT_FAILURE if the provided position is outside the xz-bounds +/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds /// of the polygon. /// dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const @@ -602,44 +602,25 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h if (!pos || !dtVisfinite2D(pos)) return DT_FAILURE | DT_INVALID_PARAM; - + + // We used to return success for offmesh connections, but the + // getPolyHeight in DetourNavMesh does not do this, so special + // case it here. if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) { const float* v0 = &tile->verts[poly->verts[0]*3]; const float* v1 = &tile->verts[poly->verts[1]*3]; - const float d0 = dtVdist2D(pos, v0); - const float d1 = dtVdist2D(pos, v1); - const float u = d0 / (d0+d1); + float t; + dtDistancePtSegSqr2D(pos, v0, v1, t); if (height) - *height = v0[1] + (v1[1] - v0[1]) * u; + *height = v0[1] + (v1[1] - v0[1])*t; + return DT_SUCCESS; } - else - { - const unsigned int ip = (unsigned int)(poly - tile->polys); - const dtPolyDetail* pd = &tile->detailMeshes[ip]; - 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 h; - if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h)) - { - if (height) - *height = h; - return DT_SUCCESS; - } - } - } - - return DT_FAILURE | DT_INVALID_PARAM; + + return m_nav->getPolyHeight(tile, poly, pos, height) + ? DT_SUCCESS + : DT_FAILURE | DT_INVALID_PARAM; } class dtFindNearestPolyQuery : public dtPolyQuery