aboutsummaryrefslogtreecommitdiff
path: root/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'dep/recastnavigation/Detour/DetourNavMeshQuery.cpp')
-rw-r--r--dep/recastnavigation/Detour/DetourNavMeshQuery.cpp1066
1 files changed, 780 insertions, 286 deletions
diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
index ec0c30460a9..f1709dfd4cf 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
+++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
@@ -81,7 +81,7 @@ float dtQueryFilter::getCost(const float* pa, const float* pb,
const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
{
- return dtVdist(pa, pb) * m_areaCost[curPoly->area];
+ return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
}
#else
inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
@@ -217,6 +217,281 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
return DT_SUCCESS;
}
+dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
+ dtPolyRef* randomRef, float* randomPt) const
+{
+ dtAssert(m_nav);
+
+ // Randomly pick one tile. Assume that all tiles cover roughly the same area.
+ const dtMeshTile* tile = 0;
+ float tsum = 0.0f;
+ for (int i = 0; i < m_nav->getMaxTiles(); i++)
+ {
+ const dtMeshTile* t = m_nav->getTile(i);
+ if (!t || !t->header) continue;
+
+ // Choose random tile using reservoi sampling.
+ const float area = 1.0f; // Could be tile area too.
+ tsum += area;
+ const float u = frand();
+ if (u*tsum <= area)
+ tile = t;
+ }
+ if (!tile)
+ return DT_FAILURE;
+
+ // Randomly pick one polygon weighted by polygon area.
+ const dtPoly* poly = 0;
+ dtPolyRef polyRef = 0;
+ const dtPolyRef base = m_nav->getPolyRefBase(tile);
+
+ float areaSum = 0.0f;
+ for (int i = 0; i < tile->header->polyCount; ++i)
+ {
+ const dtPoly* p = &tile->polys[i];
+ // Do not return off-mesh connection polygons.
+ if (p->getType() != DT_POLYTYPE_GROUND)
+ continue;
+ // Must pass filter
+ const dtPolyRef ref = base | (dtPolyRef)i;
+ if (!filter->passFilter(ref, tile, p))
+ continue;
+
+ // Calc area of the polygon.
+ float polyArea = 0.0f;
+ for (int j = 2; j < p->vertCount; ++j)
+ {
+ const float* va = &tile->verts[p->verts[0]*3];
+ const float* vb = &tile->verts[p->verts[j-1]*3];
+ const float* vc = &tile->verts[p->verts[j]*3];
+ polyArea += dtTriArea2D(va,vb,vc);
+ }
+
+ // Choose random polygon weighted by area, using reservoi sampling.
+ areaSum += polyArea;
+ const float u = frand();
+ if (u*areaSum <= polyArea)
+ {
+ poly = p;
+ polyRef = ref;
+ }
+ }
+
+ if (!poly)
+ return DT_FAILURE;
+
+ // Randomly pick point on polygon.
+ const float* v = &tile->verts[poly->verts[0]*3];
+ float verts[3*DT_VERTS_PER_POLYGON];
+ float areas[DT_VERTS_PER_POLYGON];
+ dtVcopy(&verts[0*3],v);
+ for (int j = 1; j < poly->vertCount; ++j)
+ {
+ v = &tile->verts[poly->verts[j]*3];
+ dtVcopy(&verts[j*3],v);
+ }
+
+ const float s = frand();
+ const float t = frand();
+
+ float pt[3];
+ dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
+
+ float h = 0.0f;
+ dtStatus status = getPolyHeight(polyRef, pt, &h);
+ if (dtStatusFailed(status))
+ return status;
+ pt[1] = h;
+
+ dtVcopy(randomPt, pt);
+ *randomRef = polyRef;
+
+ return DT_SUCCESS;
+}
+
+dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
+ const dtQueryFilter* filter, float (*frand)(),
+ dtPolyRef* randomRef, float* randomPt) const
+{
+ dtAssert(m_nav);
+ dtAssert(m_nodePool);
+ dtAssert(m_openList);
+
+ // Validate input
+ if (!startRef || !m_nav->isValidPolyRef(startRef))
+ return DT_FAILURE | DT_INVALID_PARAM;
+
+ const dtMeshTile* startTile = 0;
+ const dtPoly* startPoly = 0;
+ m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
+ if (!filter->passFilter(startRef, startTile, startPoly))
+ return DT_FAILURE | DT_INVALID_PARAM;
+
+ m_nodePool->clear();
+ m_openList->clear();
+
+ dtNode* startNode = m_nodePool->getNode(startRef);
+ dtVcopy(startNode->pos, centerPos);
+ startNode->pidx = 0;
+ startNode->cost = 0;
+ startNode->total = 0;
+ startNode->id = startRef;
+ startNode->flags = DT_NODE_OPEN;
+ m_openList->push(startNode);
+
+ dtStatus status = DT_SUCCESS;
+
+ const float radiusSqr = dtSqr(radius);
+ float areaSum = 0.0f;
+
+ const dtMeshTile* randomTile = 0;
+ const dtPoly* randomPoly = 0;
+ dtPolyRef randomPolyRef = 0;
+
+ while (!m_openList->empty())
+ {
+ dtNode* bestNode = m_openList->pop();
+ bestNode->flags &= ~DT_NODE_OPEN;
+ bestNode->flags |= DT_NODE_CLOSED;
+
+ // Get poly and tile.
+ // The API input has been cheked already, skip checking internal data.
+ const dtPolyRef bestRef = bestNode->id;
+ const dtMeshTile* bestTile = 0;
+ const dtPoly* bestPoly = 0;
+ m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
+
+ // Place random locations on on ground.
+ if (bestPoly->getType() == DT_POLYTYPE_GROUND)
+ {
+ // Calc area of the polygon.
+ float polyArea = 0.0f;
+ for (int j = 2; j < bestPoly->vertCount; ++j)
+ {
+ const float* va = &bestTile->verts[bestPoly->verts[0]*3];
+ const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
+ const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
+ polyArea += dtTriArea2D(va,vb,vc);
+ }
+ // Choose random polygon weighted by area, using reservoi sampling.
+ areaSum += polyArea;
+ const float u = frand();
+ if (u*areaSum <= polyArea)
+ {
+ randomTile = bestTile;
+ randomPoly = bestPoly;
+ randomPolyRef = bestRef;
+ }
+ }
+
+
+ // Get parent poly and tile.
+ dtPolyRef parentRef = 0;
+ const dtMeshTile* parentTile = 0;
+ const dtPoly* parentPoly = 0;
+ if (bestNode->pidx)
+ parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
+ if (parentRef)
+ m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
+
+ for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
+ {
+ const dtLink* link = &bestTile->links[i];
+ dtPolyRef neighbourRef = link->ref;
+ // Skip invalid neighbours and do not follow back to parent.
+ if (!neighbourRef || neighbourRef == parentRef)
+ continue;
+
+ // Expand to neighbour
+ const dtMeshTile* neighbourTile = 0;
+ const dtPoly* neighbourPoly = 0;
+ m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
+
+ // Do not advance if the polygon is excluded by the filter.
+ if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
+ continue;
+
+ // Find edge and calc distance to the edge.
+ float va[3], vb[3];
+ if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
+ continue;
+
+ // If the circle is not touching the next polygon, skip it.
+ float tseg;
+ float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
+ if (distSqr > radiusSqr)
+ continue;
+
+ dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
+ if (!neighbourNode)
+ {
+ status |= DT_OUT_OF_NODES;
+ continue;
+ }
+
+ if (neighbourNode->flags & DT_NODE_CLOSED)
+ continue;
+
+ // Cost
+ if (neighbourNode->flags == 0)
+ dtVlerp(neighbourNode->pos, va, vb, 0.5f);
+
+ const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
+
+ // The node is already in open list and the new result is worse, skip.
+ if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
+ continue;
+
+ neighbourNode->id = neighbourRef;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+ neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
+ neighbourNode->total = total;
+
+ if (neighbourNode->flags & DT_NODE_OPEN)
+ {
+ m_openList->modify(neighbourNode);
+ }
+ else
+ {
+ neighbourNode->flags = DT_NODE_OPEN;
+ m_openList->push(neighbourNode);
+ }
+ }
+ }
+
+ if (!randomPoly)
+ return DT_FAILURE;
+
+ // Randomly pick point on polygon.
+ const float* v = &randomTile->verts[randomPoly->verts[0]*3];
+ float verts[3*DT_VERTS_PER_POLYGON];
+ float areas[DT_VERTS_PER_POLYGON];
+ dtVcopy(&verts[0*3],v);
+ for (int j = 1; j < randomPoly->vertCount; ++j)
+ {
+ v = &randomTile->verts[randomPoly->verts[j]*3];
+ dtVcopy(&verts[j*3],v);
+ }
+
+ const float s = frand();
+ const float t = frand();
+
+ float pt[3];
+ dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
+
+ float h = 0.0f;
+ dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
+ if (dtStatusFailed(status))
+ return stat;
+ pt[1] = h;
+
+ dtVcopy(randomPt, pt);
+ *randomRef = randomPolyRef;
+
+ return DT_SUCCESS;
+}
+
+
//////////////////////////////////////////////////////////////////////////////////////////
/// @par
@@ -227,7 +502,7 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
///
/// 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* posOverPoly) const
{
dtAssert(m_nav);
const dtMeshTile* tile = 0;
@@ -236,97 +511,80 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
return DT_FAILURE | DT_INVALID_PARAM;
if (!tile)
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;
+ }
- // Edited by TC
- if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
- return DT_FAILURE;
+ const unsigned int ip = (unsigned int)(poly - tile->polys);
+ const dtPolyDetail* pd = &tile->detailMeshes[ip];
- if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS)
- return DT_FAILURE;
- return DT_SUCCESS;
-}
+ // 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 = FLT_MAX;
+ int imin = -1;
+ for (int i = 0; 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]);
-dtStatus dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
- const float* pos, float* closest) const
-{
- const unsigned int ip = (unsigned int)(poly - tile->polys);
- const dtPolyDetail* pd = &tile->detailMeshes[ip];
-
- // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh.
- // Test and enable.
- /*
- // 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 = FLT_MAX;
- int imin = -1;
- for (int i = 0; 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]);
- }
-
- // 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(pos, v[0], v[1], v[2], h))
- {
- closest[1] = h;
- break;
- }
- }
- */
- float closestDistSqr = FLT_MAX;
- 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 pt[3];
- dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
- float d = dtVdistSqr(pos, pt);
-
- if (d < closestDistSqr)
- {
- dtVcopy(closest, pt);
- closestDistSqr = d;
- }
- }
-
- return DT_SUCCESS;
+ if (posOverPoly)
+ *posOverPoly = false;
+ }
+ else
+ {
+ if (posOverPoly)
+ *posOverPoly = true;
+ }
+
+ // 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(pos, v[0], v[1], v[2], h))
+ {
+ closest[1] = h;
+ break;
+ }
+ }
+
+ return DT_SUCCESS;
}
/// @par
@@ -405,8 +663,8 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h
{
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 d0 = dtVdist2D(pos, v0);
+ const float d1 = dtVdist2D(pos, v1);
const float u = d0 / (d0+d1);
if (height)
*height = v0[1] + (v1[1] - v0[1]) * u;
@@ -470,9 +728,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
{
dtPolyRef ref = polys[i];
float closestPtPoly[3];
- if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS)
- continue;
- float d = dtVdistSqr(center, closestPtPoly);
+ float diff[3];
+ bool posOverPoly = false;
+ float d = 0;
+ closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+ // If a point is directly over a polygon and closer than
+ // climb height, favor that instead of straight line nearest point.
+ dtVsub(diff, center, closestPtPoly);
+ if (posOverPoly)
+ {
+ const dtMeshTile* tile = 0;
+ const dtPoly* poly = 0;
+ m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly);
+ d = dtAbs(diff[1]) - tile->header->walkableClimb;
+ d = d > 0 ? d*d : 0;
+ }
+ else
+ {
+ d = dtVlenSqr(diff);
+ }
+
if (d < nearestDistanceSqr)
{
if (nearestPt)
@@ -488,43 +764,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
return DT_SUCCESS;
}
-dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
- const dtQueryFilter* filter, float* nearestPt) const
-{
- dtAssert(m_nav);
-
- float bmin[3], bmax[3];
- dtVsub(bmin, center, extents);
- dtVadd(bmax, center, extents);
-
- // Get nearby polygons from proximity grid.
- dtPolyRef polys[128];
- int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);
-
- // 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];
- const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
- float closestPtPoly[3];
- if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS)
- continue;
-
- float d = dtVdistSqr(center, closestPtPoly);
- if (d < nearestDistanceSqr)
- {
- if (nearestPt)
- dtVcopy(nearestPt, closestPtPoly);
- nearestDistanceSqr = d;
- nearest = ref;
- }
- }
-
- return nearest;
-}
-
int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
const dtQueryFilter* filter,
dtPolyRef* polys, const int maxPolys) const
@@ -592,8 +831,15 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
const dtPolyRef base = m_nav->getPolyRefBase(tile);
for (int i = 0; i < tile->header->polyCount; ++i)
{
+ const dtPoly* p = &tile->polys[i];
+ // Do not return off-mesh connection polygons.
+ if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+ continue;
+ // Must pass filter
+ const dtPolyRef ref = base | (dtPolyRef)i;
+ if (!filter->passFilter(ref, tile, p))
+ continue;
// Calc polygon bounds.
- dtPoly* p = &tile->polys[i];
const float* v = &tile->verts[p->verts[0]*3];
dtVcopy(bmin, v);
dtVcopy(bmax, v);
@@ -605,12 +851,8 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
}
if (dtOverlapBounds(qmin,qmax, bmin,bmax))
{
- const dtPolyRef ref = base | (dtPolyRef)i;
- if (filter->passFilter(ref, tile, p))
- {
- if (n < maxPolys)
- polys[n++] = ref;
- }
+ if (n < maxPolys)
+ polys[n++] = ref;
}
}
return n;
@@ -641,18 +883,23 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents
m_nav->calcTileLoc(bmin, &minx, &miny);
m_nav->calcTileLoc(bmax, &maxx, &maxy);
+ static const int MAX_NEIS = 32;
+ const dtMeshTile* neis[MAX_NEIS];
+
int n = 0;
for (int y = miny; y <= maxy; ++y)
{
for (int x = minx; x <= maxx; ++x)
{
- const dtMeshTile* tile = m_nav->getTileAt(x,y);
- if (!tile) continue;
- n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n);
- if (n >= maxPolys)
+ const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
+ for (int j = 0; j < nneis; ++j)
{
- *polyCount = n;
- return DT_SUCCESS;
+ n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n);
+ if (n >= maxPolys)
+ {
+ *polyCount = n;
+ return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
+ }
}
}
}
@@ -715,6 +962,8 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* lastBestNode = startNode;
float lastBestNodeCost = startNode->total;
+ dtStatus status = DT_SUCCESS;
+
while (!m_openList->empty())
{
// Remove node from open list and put it in closed list.
@@ -764,7 +1013,10 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
+ {
+ status |= DT_OUT_OF_NODES;
continue;
+ }
// If the node is visited the first time, calculate node position.
if (neighbourNode->flags == 0)
@@ -817,7 +1069,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
// Add or update the node.
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->id = neighbourRef;
- neighbourNode->flags &= ~DT_NODE_CLOSED;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->cost = cost;
neighbourNode->total = total;
@@ -842,6 +1094,9 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
}
}
+ if (lastBestNode->id != endRef)
+ status |= DT_PARTIAL_RESULT;
+
// Reverse the path.
dtNode* prev = 0;
dtNode* node = lastBestNode;
@@ -860,13 +1115,18 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
do
{
path[n++] = node->id;
+ if (n >= maxPath)
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ break;
+ }
node = m_nodePool->getNodeAtIdx(node->pidx);
}
- while (node && n < maxPath);
+ while (node);
*pathCount = n;
- return DT_SUCCESS;
+ return status;
}
/// @par
@@ -926,7 +1186,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
return m_query.status;
}
-dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
+dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
{
if (!dtStatusInProgress(m_query.status))
return m_query.status;
@@ -952,7 +1212,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
if (bestNode->id == m_query.endRef)
{
m_query.lastBestNode = bestNode;
- m_query.status = DT_SUCCESS;
+ const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+ m_query.status = DT_SUCCESS | details;
+ if (doneIters)
+ *doneIters = iter;
return m_query.status;
}
@@ -965,6 +1228,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
{
// The polygon has disappeared during the sliced query, fail.
m_query.status = DT_FAILURE;
+ if (doneIters)
+ *doneIters = iter;
return m_query.status;
}
@@ -980,6 +1245,8 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
{
// The polygon has disappeared during the sliced query, fail.
m_query.status = DT_FAILURE;
+ if (doneIters)
+ *doneIters = iter;
return m_query.status;
}
}
@@ -1003,7 +1270,10 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
+ {
+ m_query.status |= DT_OUT_OF_NODES;
continue;
+ }
// If the node is visited the first time, calculate node position.
if (neighbourNode->flags == 0)
@@ -1056,7 +1326,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
// Add or update the node.
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->id = neighbourRef;
- neighbourNode->flags &= ~DT_NODE_CLOSED;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->cost = cost;
neighbourNode->total = total;
@@ -1083,7 +1353,13 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
// Exhausted all nodes, but could not find path.
if (m_openList->empty())
- m_query.status = DT_SUCCESS;
+ {
+ const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+ m_query.status = DT_SUCCESS | details;
+ }
+
+ if (doneIters)
+ *doneIters = iter;
return m_query.status;
}
@@ -1110,6 +1386,10 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
{
// Reverse the path.
dtAssert(m_query.lastBestNode);
+
+ if (m_query.lastBestNode->id != m_query.endRef)
+ m_query.status |= DT_PARTIAL_RESULT;
+
dtNode* prev = 0;
dtNode* node = m_query.lastBestNode;
do
@@ -1126,17 +1406,24 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
do
{
path[n++] = node->id;
+ if (n >= maxPath)
+ {
+ m_query.status |= DT_BUFFER_TOO_SMALL;
+ break;
+ }
node = m_nodePool->getNodeAtIdx(node->pidx);
}
- while (node && n < maxPath);
+ while (node);
}
+ const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
*pathCount = n;
- return DT_SUCCESS;
+ return DT_SUCCESS | details;
}
dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
@@ -1149,7 +1436,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
return DT_FAILURE;
}
- if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS)
+ if (dtStatusFailed(m_query.status))
{
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
@@ -1177,7 +1464,9 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
if (!node)
{
- return DT_FAILURE;
+ m_query.status |= DT_PARTIAL_RESULT;
+ dtAssert(m_query.lastBestNode);
+ node = m_query.lastBestNode;
}
// Reverse the path.
@@ -1195,24 +1484,128 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
do
{
path[n++] = node->id;
+ if (n >= maxPath)
+ {
+ m_query.status |= DT_BUFFER_TOO_SMALL;
+ break;
+ }
node = m_nodePool->getNodeAtIdx(node->pidx);
}
- while (node && n < maxPath);
+ while (node);
}
+ const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
+
// Reset query.
memset(&m_query, 0, sizeof(dtQueryData));
*pathCount = n;
- return DT_SUCCESS;
+ return DT_SUCCESS | details;
}
+dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
+ float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+ int* straightPathCount, const int maxStraightPath) const
+{
+ if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
+ {
+ // The vertices are equal, update flags and poly.
+ if (straightPathFlags)
+ straightPathFlags[(*straightPathCount)-1] = flags;
+ if (straightPathRefs)
+ straightPathRefs[(*straightPathCount)-1] = ref;
+ }
+ else
+ {
+ // Append new vertex.
+ dtVcopy(&straightPath[(*straightPathCount)*3], pos);
+ if (straightPathFlags)
+ straightPathFlags[(*straightPathCount)] = flags;
+ if (straightPathRefs)
+ straightPathRefs[(*straightPathCount)] = ref;
+ (*straightPathCount)++;
+ // If reached end of path or there is no space to append more vertices, return.
+ if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath)
+ {
+ return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+ }
+ }
+ return DT_IN_PROGRESS;
+}
+
+dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
+ float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
+ int* straightPathCount, const int maxStraightPath, const int options) const
+{
+ const float* startPos = &straightPath[(*straightPathCount-1)*3];
+ // Append or update last vertex
+ dtStatus stat = 0;
+ for (int i = startIdx; i < endIdx; i++)
+ {
+ // Calculate portal
+ const dtPolyRef from = path[i];
+ const dtMeshTile* fromTile = 0;
+ const dtPoly* fromPoly = 0;
+ if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
+ return DT_FAILURE | DT_INVALID_PARAM;
+
+ const dtPolyRef to = path[i+1];
+ const dtMeshTile* toTile = 0;
+ const dtPoly* toPoly = 0;
+ if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
+ return DT_FAILURE | DT_INVALID_PARAM;
+
+ float left[3], right[3];
+ if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
+ break;
+
+ if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
+ {
+ // Skip intersection if only area crossings are requested.
+ if (fromPoly->getArea() == toPoly->getArea())
+ continue;
+ }
+
+ // Append intersection
+ float s,t;
+ if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
+ {
+ float pt[3];
+ dtVlerp(pt, left,right, t);
+
+ stat = appendVertex(pt, 0, path[i+1],
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
+ }
+ }
+ return DT_IN_PROGRESS;
+}
+
+/// @par
+///
+/// This method peforms what is often called 'string pulling'.
+///
+/// The start position is clamped to the first polygon in the path, and the
+/// end position is clamped to the last. So the start and end positions should
+/// normally be within or very near the first and last polygons respectively.
+///
+/// The returned polygon references represent the reference id of the polygon
+/// that is entered at the associated path position. The reference id associated
+/// with the end point will always be zero. This allows, for example, matching
+/// off-mesh link points to their representative polygons.
+///
+/// If the provided result buffers are too small for the entire result set,
+/// they will be filled as far as possible from the start toward the end
+/// position.
+///
dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
const dtPolyRef* path, const int pathSize,
float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
- int* straightPathCount, const int maxStraightPath) const
+ int* straightPathCount, const int maxStraightPath, const int options) const
{
dtAssert(m_nav);
@@ -1224,29 +1617,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
if (!path[0])
return DT_FAILURE | DT_INVALID_PARAM;
- int n = 0;
+ dtStatus stat = 0;
// TODO: Should this be callers responsibility?
float closestStartPos[3];
if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
return DT_FAILURE | DT_INVALID_PARAM;
+
+ float closestEndPos[3];
+ if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
+ return DT_FAILURE | DT_INVALID_PARAM;
// Add start point.
- dtVcopy(&straightPath[n*3], closestStartPos);
- if (straightPathFlags)
- straightPathFlags[n] = DT_STRAIGHTPATH_START;
- if (straightPathRefs)
- straightPathRefs[n] = path[0];
- n++;
- if (n >= maxStraightPath)
- {
- *straightPathCount = n;
- return DT_SUCCESS;
- }
-
- float closestEndPos[3];
- if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS)
- return DT_FAILURE;
+ stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
if (pathSize > 1)
{
@@ -1274,17 +1661,28 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
// Next portal.
if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
{
- if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS)
- return DT_FAILURE;
+ // Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
+ // Clamp the end point to path[i], and return the path so far.
- dtVcopy(&straightPath[n*3], closestEndPos);
- if (straightPathFlags)
- straightPathFlags[n] = 0;
- if (straightPathRefs)
- straightPathRefs[n] = path[i];
- n++;
+ if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
+ {
+ // This should only happen when the first polygon is invalid.
+ return DT_FAILURE | DT_INVALID_PARAM;
+ }
+
+ // Apeend portals along the current straight path segment.
+ if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+ {
+ stat = appendPortals(apexIndex, i, closestEndPos, path,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath, options);
+ }
+
+ stat = appendVertex(closestEndPos, 0, path[i],
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
- return DT_SUCCESS;
+ return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}
// If starting really close the portal, advance.
@@ -1316,6 +1714,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
}
else
{
+ // Append portals along the current straight path segment.
+ if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+ {
+ stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath, options);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
+ }
+
dtVcopy(portalApex, portalLeft);
apexIndex = leftIndex;
@@ -1326,30 +1734,12 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
dtPolyRef ref = leftPolyRef;
- if (!dtVequal(&straightPath[(n-1)*3], portalApex))
- {
- // Append new vertex.
- dtVcopy(&straightPath[n*3], portalApex);
- if (straightPathFlags)
- straightPathFlags[n] = flags;
- if (straightPathRefs)
- straightPathRefs[n] = ref;
- n++;
- // If reached end of path or there is no space to append more vertices, return.
- if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
- {
- *straightPathCount = n;
- return DT_SUCCESS;
- }
- }
- else
- {
- // The vertices are equal, update flags and poly.
- if (straightPathFlags)
- straightPathFlags[n-1] = flags;
- if (straightPathRefs)
- straightPathRefs[n-1] = ref;
- }
+ // Append or update vertex
+ stat = appendVertex(portalApex, flags, ref,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
dtVcopy(portalLeft, portalApex);
dtVcopy(portalRight, portalApex);
@@ -1375,6 +1765,16 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
}
else
{
+ // Append portals along the current straight path segment.
+ if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+ {
+ stat = appendPortals(apexIndex, rightIndex, portalRight, path,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath, options);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
+ }
+
dtVcopy(portalApex, portalRight);
apexIndex = rightIndex;
@@ -1384,31 +1784,13 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
dtPolyRef ref = rightPolyRef;
-
- if (!dtVequal(&straightPath[(n-1)*3], portalApex))
- {
- // Append new vertex.
- dtVcopy(&straightPath[n*3], portalApex);
- if (straightPathFlags)
- straightPathFlags[n] = flags;
- if (straightPathRefs)
- straightPathRefs[n] = ref;
- n++;
- // If reached end of path or there is no space to append more vertices, return.
- if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
- {
- *straightPathCount = n;
- return DT_SUCCESS;
- }
- }
- else
- {
- // The vertices are equal, update flags and poly.
- if (straightPathFlags)
- straightPathFlags[n-1] = flags;
- if (straightPathRefs)
- straightPathRefs[n-1] = ref;
- }
+
+ // Append or update vertex
+ stat = appendVertex(portalApex, flags, ref,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
dtVcopy(portalLeft, portalApex);
dtVcopy(portalRight, portalApex);
@@ -1422,25 +1804,23 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
}
}
}
+
+ // Append portals along the current straight path segment.
+ if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
+ {
+ stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath, options);
+ if (stat != DT_IN_PROGRESS)
+ return stat;
+ }
}
+
+ stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
+ straightPath, straightPathFlags, straightPathRefs,
+ straightPathCount, maxStraightPath);
- // If the point already exists, remove it and add reappend the actual end location.
- if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos))
- n--;
-
- // Add end point.
- if (n < maxStraightPath)
- {
- dtVcopy(&straightPath[n*3], closestEndPos);
- if (straightPathFlags)
- straightPathFlags[n] = DT_STRAIGHTPATH_END;
- if (straightPathRefs)
- straightPathRefs[n] = 0;
- n++;
- }
-
- *straightPathCount = n;
- return DT_SUCCESS;
+ return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
}
/// @par
@@ -1478,6 +1858,8 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
if (!m_nav->isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
+ dtStatus status = DT_SUCCESS;
+
static const int MAX_STACK = 48;
dtNode* stack[MAX_STACK];
int nstack = 0;
@@ -1641,16 +2023,21 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
do
{
visited[n++] = node->id;
+ if (n >= maxVisitedSize)
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ break;
+ }
node = m_tinyNodePool->getNodeAtIdx(node->pidx);
}
- while (node && n < maxVisitedSize);
+ while (node);
}
dtVcopy(resultPos, bestPos);
*visitedCount = n;
- return DT_SUCCESS;
+ return status;
}
@@ -1753,7 +2140,7 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mi
{
float left[3], right[3];
unsigned char fromType, toType;
- if (!getPortalPoints(from, to, left,right, fromType, toType))
+ if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
return DT_FAILURE | DT_INVALID_PARAM;
mid[0] = (left[0]+right[0])*0.5f;
mid[1] = (left[1]+right[1])*0.5f;
@@ -1834,6 +2221,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
hitNormal[1] = 0;
hitNormal[2] = 0;
+ dtStatus status = DT_SUCCESS;
+
while (curRef)
{
// Cast ray against current polygon.
@@ -1858,7 +2247,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
// Could not hit the polygon, keep the old t and report hit.
if (pathCount)
*pathCount = n;
- return DT_SUCCESS;
+ return status;
}
// Keep track of furthest t so far.
if (tmax > *t)
@@ -1867,6 +2256,8 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
// Store visited polygons.
if (n < maxPath)
path[n++] = curRef;
+ else
+ status |= DT_BUFFER_TOO_SMALL;
// Ray end is completely inside the polygon.
if (segMax == -1)
@@ -1874,7 +2265,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
*t = FLT_MAX;
if (pathCount)
*pathCount = n;
- return DT_SUCCESS;
+ return status;
}
// Follow neighbours.
@@ -1976,7 +2367,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
if (pathCount)
*pathCount = n;
- return DT_SUCCESS;
+ return status;
}
// No hit, advance to neighbour polygon.
@@ -1986,7 +2377,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
if (pathCount)
*pathCount = n;
- return DT_SUCCESS;
+ return status;
}
/// @par
@@ -2045,6 +2436,8 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
+ dtStatus status = DT_SUCCESS;
+
int n = 0;
if (n < maxResult)
{
@@ -2056,6 +2449,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
resultCost[n] = 0;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
const float radiusSqr = dtSqr(radius);
@@ -2111,7 +2508,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
+ {
+ status |= DT_OUT_OF_NODES;
continue;
+ }
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
@@ -2127,7 +2527,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
continue;
neighbourNode->id = neighbourRef;
- neighbourNode->flags &= ~DT_NODE_CLOSED;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
@@ -2147,6 +2547,10 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
resultCost[n] = neighbourNode->total;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
neighbourNode->flags = DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
@@ -2155,7 +2559,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
*resultCount = n;
- return DT_SUCCESS;
+ return status;
}
/// @par
@@ -2212,6 +2616,8 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
startNode->flags = DT_NODE_OPEN;
m_openList->push(startNode);
+ dtStatus status = DT_SUCCESS;
+
int n = 0;
if (n < maxResult)
{
@@ -2223,6 +2629,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
resultCost[n] = 0;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
while (!m_openList->empty())
{
@@ -2278,7 +2688,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
+ {
+ status |= DT_OUT_OF_NODES;
continue;
+ }
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
@@ -2294,7 +2707,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
continue;
neighbourNode->id = neighbourRef;
- neighbourNode->flags &= ~DT_NODE_CLOSED;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
@@ -2314,6 +2727,10 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
resultCost[n] = neighbourNode->total;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
neighbourNode->flags = DT_NODE_OPEN;
m_openList->push(neighbourNode);
}
@@ -2322,7 +2739,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
*resultCount = n;
- return DT_SUCCESS;
+ return status;
}
/// @par
@@ -2378,6 +2795,8 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
float pa[DT_VERTS_PER_POLYGON*3];
float pb[DT_VERTS_PER_POLYGON*3];
+ dtStatus status = DT_SUCCESS;
+
int n = 0;
if (n < maxResult)
{
@@ -2386,6 +2805,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
resultParent[n] = 0;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
while (nstack)
{
@@ -2499,6 +2922,10 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
resultParent[n] = curRef;
++n;
}
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
if (nstack < MAX_STACK)
{
@@ -2509,17 +2936,18 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
*resultCount = n;
- return DT_SUCCESS;
+ return status;
}
struct dtSegInterval
{
+ dtPolyRef ref;
short tmin, tmax;
};
static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
- const short tmin, const short tmax)
+ const short tmin, const short tmax, const dtPolyRef ref)
{
if (nints+1 > maxInts) return;
// Find insertion point.
@@ -2534,6 +2962,7 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
if (nints-idx)
memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
// Store
+ ints[idx].ref = ref;
ints[idx].tmin = tmin;
ints[idx].tmax = tmax;
nints++;
@@ -2551,7 +2980,8 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
/// maximum segments per polygon of the source navigation mesh.
///
dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
- float* segments, int* segmentCount, const int maxSegments) const
+ float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
+ const int maxSegments) const
{
dtAssert(m_nav);
@@ -2567,6 +2997,10 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
dtSegInterval ints[MAX_INTERVAL];
int nints;
+ const bool storePortals = segmentRefs != 0;
+
+ dtStatus status = DT_SUCCESS;
+
for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
{
// Skip non-solid edges.
@@ -2586,54 +3020,95 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
if (filter->passFilter(link->ref, neiTile, neiPoly))
{
- insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax);
+ insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
}
}
}
}
}
- else if (poly->neis[j])
+ else
{
// Internal edge
- const unsigned int idx = (unsigned int)(poly->neis[j]-1);
- const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx;
- if (filter->passFilter(ref, tile, &tile->polys[idx]))
+ dtPolyRef neiRef = 0;
+ if (poly->neis[j])
+ {
+ const unsigned int idx = (unsigned int)(poly->neis[j]-1);
+ neiRef = m_nav->getPolyRefBase(tile) | idx;
+ if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
+ neiRef = 0;
+ }
+
+ // If the edge leads to another polygon and portals are not stored, skip.
+ if (neiRef != 0 && !storePortals)
continue;
+
+ if (n < maxSegments)
+ {
+ const float* vj = &tile->verts[poly->verts[j]*3];
+ const float* vi = &tile->verts[poly->verts[i]*3];
+ float* seg = &segmentVerts[n*6];
+ dtVcopy(seg+0, vj);
+ dtVcopy(seg+3, vi);
+ if (segmentRefs)
+ segmentRefs[n] = neiRef;
+ n++;
+ }
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
+ }
+
+ continue;
}
// Add sentinels
- insertInterval(ints, nints, MAX_INTERVAL, -1, 0);
- insertInterval(ints, nints, MAX_INTERVAL, 255, 256);
+ insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
+ insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
- // Store segment.
+ // Store segments.
const float* vj = &tile->verts[poly->verts[j]*3];
const float* vi = &tile->verts[poly->verts[i]*3];
for (int k = 1; k < nints; ++k)
{
- // Find the space inbetween the opening areas.
- const int imin = ints[k-1].tmax;
- const int imax = ints[k].tmin;
- if (imin == imax) continue;
- if (imin == 0 && imax == 255)
+ // Portal segment.
+ if (storePortals && ints[k].ref)
{
+ const float tmin = ints[k].tmin/255.0f;
+ const float tmax = ints[k].tmax/255.0f;
if (n < maxSegments)
{
- float* seg = &segments[n*6];
+ float* seg = &segmentVerts[n*6];
+ dtVlerp(seg+0, vj,vi, tmin);
+ dtVlerp(seg+3, vj,vi, tmax);
+ if (segmentRefs)
+ segmentRefs[n] = ints[k].ref;
n++;
- dtVcopy(seg+0, vj);
- dtVcopy(seg+3, vi);
+ }
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
}
}
- else
+
+ // Wall segment.
+ const int imin = ints[k-1].tmax;
+ const int imax = ints[k].tmin;
+ if (imin != imax)
{
const float tmin = imin/255.0f;
const float tmax = imax/255.0f;
if (n < maxSegments)
{
- float* seg = &segments[n*6];
- n++;
+ float* seg = &segmentVerts[n*6];
dtVlerp(seg+0, vj,vi, tmin);
dtVlerp(seg+3, vj,vi, tmax);
+ if (segmentRefs)
+ segmentRefs[n] = 0;
+ n++;
+ }
+ else
+ {
+ status |= DT_BUFFER_TOO_SMALL;
}
}
}
@@ -2641,7 +3116,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
*segmentCount = n;
- return DT_SUCCESS;
+ return status;
}
/// @par
@@ -2680,6 +3155,8 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
float radiusSqr = dtSqr(maxRadius);
+ dtStatus status = DT_SUCCESS;
+
while (!m_openList->empty())
{
dtNode* bestNode = m_openList->pop();
@@ -2787,7 +3264,10 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
if (!neighbourNode)
+ {
+ status |= DT_OUT_OF_NODES;
continue;
+ }
if (neighbourNode->flags & DT_NODE_CLOSED)
continue;
@@ -2806,7 +3286,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
continue;
neighbourNode->id = neighbourRef;
- neighbourNode->flags &= ~DT_NODE_CLOSED;
+ neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
neighbourNode->total = total;
@@ -2828,7 +3308,21 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
*hitDist = sqrtf(radiusSqr);
- return DT_SUCCESS;
+ return status;
+}
+
+bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const
+{
+ const dtMeshTile* tile = 0;
+ const dtPoly* poly = 0;
+ dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
+ // If cannot get polygon, assume it does not exists and boundary is invalid.
+ if (dtStatusFailed(status))
+ return false;
+ // If cannot pass filter, assume flags has changed and boundary is invalid.
+ if (!filter->passFilter(ref, tile, poly))
+ return false;
+ return true;
}
/// @par