aboutsummaryrefslogtreecommitdiff
path: root/dep/recastnavigation/Detour/Source
diff options
context:
space:
mode:
Diffstat (limited to 'dep/recastnavigation/Detour/Source')
-rw-r--r--dep/recastnavigation/Detour/Source/DetourCommon.cpp26
-rw-r--r--dep/recastnavigation/Detour/Source/DetourNavMesh.cpp160
-rw-r--r--dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp129
3 files changed, 138 insertions, 177 deletions
diff --git a/dep/recastnavigation/Detour/Source/DetourCommon.cpp b/dep/recastnavigation/Detour/Source/DetourCommon.cpp
index e10e6276170..b89d7512c4b 100644
--- a/dep/recastnavigation/Detour/Source/DetourCommon.cpp
+++ b/dep/recastnavigation/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,21 +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) {
-
- if (denom == 0.f && u == 0.f && v == 0.f)
- h = a[1];
- else
- h = a[1] + (v0[1]*u + v1[1]*v) / denom;
-
- if (!dtMathIsfinite(h))
- return false;
-
+ 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/dep/recastnavigation/Detour/Source/DetourNavMesh.cpp b/dep/recastnavigation/Detour/Source/DetourNavMesh.cpp
index 17df26d8c07..a655d1d89a7 100644
--- a/dep/recastnavigation/Detour/Source/DetourNavMesh.cpp
+++ b/dep/recastnavigation/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<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 (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<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,
diff --git a/dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp
index 9e470824287..85ad33259c7 100644
--- a/dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp
+++ b/dep/recastnavigation/Detour/Source/DetourNavMeshQuery.cpp
@@ -514,88 +514,14 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
{
dtAssert(m_nav);
- const dtMeshTile* tile = 0;
- const dtPoly* poly = 0;
- if (dtStatusFailed(m_nav->getTileAndPolyByRef(ref, &tile, &poly)))
- return DT_FAILURE | DT_INVALID_PARAM;
- if (!tile)
- return DT_FAILURE | DT_INVALID_PARAM;
-
- if (!pos || !dtVisfinite(pos) || !closest)
- return DT_FAILURE | DT_INVALID_PARAM;
-
- // 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];
- 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 DT_SUCCESS;
- }
-
- 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 (!m_nav->isValidPolyRef(ref) ||
+ !pos || !dtVisfinite(pos) ||
+ !closest)
{
- if (posOverPoly)
- *posOverPoly = true;
+ return DT_FAILURE | DT_INVALID_PARAM;
}
- // Find height at the location.
- 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(closest, v[0], v[1], v[2], h))
- {
- closest[1] = h;
- break;
- }
- }
-
+ m_nav->closestPointOnPoly(ref, pos, closest, posOverPoly);
return DT_SUCCESS;
}
@@ -662,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
@@ -676,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