server_common/recastnavigation_v1.6.0
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make closestPointOnPoly and getPolyHeight more robust

Browse Source 
* 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.
This commit is contained in:
Jakob Botsch Nielsen 2019-03-16 15:03:50 +01:00
parent 33a9eb1cce
commit 54bb0943e5
4 changed files with 135 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Detour/Include/DetourNavMesh.h
View File

@ -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;

18
Detour/Source/DetourCommon.cpp
View File

@ -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;

156
Detour/Source/DetourNavMesh.cpp
View File

@ -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<bool onlyBoundary>
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 (!dtPointInPolygon(pos, verts, nv))
return false;
if (posOverPoly)
*posOverPoly = false;
}
else
{
if (posOverPoly)
*posOverPoly = true;
}
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<false>(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<true>(tile, poly, pos, closest);
}
dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,

41
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -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
@ -603,43 +603,24 @@ 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