mirror of
https://github.com/recastnavigation/recastnavigation.git
synced 2026-08-15 23:59:59 +00:00
Random polygon selection for Detour.
This commit is contained in:
Mikko Mononen
@@ -216,6 +216,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
|
||||
|
||||
Reference in New Issue
Block a user