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

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.

Browse Source
This commit is contained in:
axelrodR 2014-02-10 00:30:46 +02:00
parent 72b7123e72
commit e2722403be
4 changed files with 103 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Detour/Include/DetourNavMesh.h
View File

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

8
Detour/Include/DetourNavMeshQuery.h
View File

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

34
Detour/Source/DetourNavMesh.cpp
View File

@ -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,
@ -695,21 +698,40 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
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;
float nearestDistanceSqr = FLT_MAX;
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;
}
}
}

71
Detour/Source/DetourNavMeshQuery.cpp
View File

@ -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)
{
@ -740,21 +745,47 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
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;
float nearestDistanceSqr = FLT_MAX;
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;
}
}
}
@ -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, 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);
float d = dtVdistSqr(center, closestPtPoly);
if (d < nearestDistanceSqr)
{
if (nearestPt)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = ref;
if (d == 0)
{
nearestDistanceSqr = 0;
break;
}
}
}