/// Returns random location on navmesh within the reach of specified location.
/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
/// The location is not exactly constrained by the circle, but it limits the visited polygons.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] frand Function returning a random number [0..1).
/// @param[out] randomRef The reference id of the random location.
/// @param[out] randomPt The random location. [(x, y, z)]
/// @returns The status flags for the query.
public dtStatus findRandomPointAroundCircle(dtPolyRef startRef, float[] centerPos, float radius,
dtQueryFilter filter, randomFloatGenerator frand,
ref dtPolyRef randomRef,ref float[] randomPt)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_nodePool != null);
Debug.Assert(m_openList != null);
// Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
dtMeshTile startTile = null;
dtPoly startPoly = null;
m_nav.getTileAndPolyByRefUnsafe(startRef, ref startTile, ref 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 = (byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(startNode);
dtStatus status = DT_SUCCESS;
float radiusSqr = dtSqr(radius);
float areaSum = 0.0f;
dtMeshTile randomTile = null;
dtPoly randomPoly = null;
dtPolyRef randomPolyRef = 0;
while (!m_openList.empty())
{
dtNode bestNode = m_openList.pop();
unchecked{
bestNode.flags &= (byte)( ~ dtNodeFlags.DT_NODE_OPEN );
}
bestNode.flags |= (byte)dtNodeFlags.DT_NODE_CLOSED;
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
dtPolyRef bestRef = bestNode.id;
dtMeshTile bestTile = null;
dtPoly bestPoly = null;
m_nav.getTileAndPolyByRefUnsafe(bestRef, ref bestTile, ref bestPoly);
// Place random locations on on ground.
if (bestPoly.getType() == (byte)dtPolyTypes.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(bestTile.verts, bestPoly.verts[0]*3, bestTile.verts, bestPoly.verts[j-1]*3, bestTile.verts, bestPoly.verts[j]*3);
}
// Choose random polygon weighted by area, using reservoi sampling.
areaSum += polyArea;
float u = frand();
if (u*areaSum <= polyArea)
{
randomTile = bestTile;
randomPoly = bestPoly;
randomPolyRef = bestRef;
}
}
// Get parent poly and tile.
dtPolyRef parentRef = 0;
dtMeshTile parentTile = null;
dtPoly parentPoly = null;
if (bestNode.pidx != 0)
parentRef = m_nodePool.getNodeAtIdx(bestNode.pidx).id;
if (parentRef != 0)
m_nav.getTileAndPolyByRefUnsafe(parentRef, ref parentTile, ref parentPoly);
for (uint i = bestPoly.firstLink; i != DT_NULL_LINK; i = bestTile.links[i].next)
{
dtLink link = bestTile.links[i];
dtPolyRef neighbourRef = link.polyRef;
// Skip invalid neighbours and do not follow back to parent.
if (neighbourRef == 0 || neighbourRef == parentRef)
continue;
// Expand to neighbour
dtMeshTile neighbourTile = null;
dtPoly neighbourPoly = null;
m_nav.getTileAndPolyByRefUnsafe(neighbourRef, ref neighbourTile, ref 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 = new float[3];//, vb[3];
float[] vb = new float[3];
if (getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb) == 0)
continue;
// If the circle is not touching the next polygon, skip it.
float tseg = .0f;
float distSqr = dtDistancePtSegSqr2D(centerPos, 0, va, 0, vb, 0, ref tseg);
if (distSqr > radiusSqr)
continue;
dtNode neighbourNode = m_nodePool.getNode(neighbourRef);
if (neighbourNode == null)
{
status |= DT_OUT_OF_NODES;
continue;
}
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_CLOSED) != 0)
continue;
// Cost
if (neighbourNode.flags == 0){
dtVlerp(neighbourNode.pos, va, vb, 0.5f);
}
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 & (byte)dtNodeFlags.DT_NODE_OPEN) != 0) && total >= neighbourNode.total)
continue;
neighbourNode.id = neighbourRef;
unchecked{
neighbourNode.flags = (byte)(neighbourNode.flags & (byte)(~dtNodeFlags.DT_NODE_CLOSED));
}
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
neighbourNode.total = total;
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_OPEN) != 0)
{
m_openList.modify(neighbourNode);
}
else
{
neighbourNode.flags = (byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(neighbourNode);
}
}
}
if (randomPoly == null)
return DT_FAILURE;
// Randomly pick point on polygon.
//float* v = &randomTile.verts[randomPoly.verts[0]*3];
float[] verts = new float[3*DT_VERTS_PER_POLYGON];
float[] areas = new float[DT_VERTS_PER_POLYGON];
dtVcopy(verts, 0*3, randomTile.verts, 0);
for (int j = 1; j < randomPoly.vertCount; ++j)
{
//v = &randomTile.verts[randomPoly.verts[j]*3];
dtVcopy(verts,j*3,randomTile.verts,randomPoly.verts[j]*3);
}
float s = frand();
float t = frand();
float[] pt = new float[3];
dtRandomPointInConvexPoly(verts, randomPoly.vertCount, areas, s, t, pt);
float h = 0.0f;
dtStatus stat = getPolyHeight(randomPolyRef, pt, ref h);
if (dtStatusFailed(status))
return stat;
pt[1] = h;
dtVcopy(randomPt, pt);
randomRef = randomPolyRef;
return DT_SUCCESS;
}
/// Returns random location on navmesh.
/// Polygons are chosen weighted by area. The search runs in linear related to number of polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[in] frand Function returning a random number [0..1).
/// @param[out] randomRef The reference id of the random location.
/// @param[out] randomPt The random location.
/// @returns The status flags for the query.
public dtStatus findRandomPoint(dtQueryFilter filter, randomFloatGenerator frand,
ref dtPolyRef randomRef, ref float[] randomPt)
{
Debug.Assert(m_nav != null);
// Randomly pick one tile. Assume that all tiles cover roughly the same area.
dtMeshTile tile = null;
float tsum = 0.0f;
for (int i = 0; i < m_nav.getMaxTiles(); i++)
{
dtMeshTile curTile = m_nav.getTile(i);
if (curTile == null || curTile.header == null) continue;
// Choose random tile using reservoi sampling.
const float area = 1.0f; // Could be tile area too.
tsum += area;
float u = frand();
if (u*tsum <= area)
tile = curTile;
}
if (tile == null)
return DT_FAILURE;
// Randomly pick one polygon weighted by polygon area.
dtPoly poly = null;
dtPolyRef polyRef = 0;
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
float areaSum = 0.0f;
for (int i = 0; i < tile.header.polyCount; ++i)
{
dtPoly p = tile.polys[i];
// Do not return off-mesh connection polygons.
if (p.getType() != (byte)dtPolyTypes.DT_POLYTYPE_GROUND)
continue;
// Must pass filter
dtPolyRef pRef = polyRefBase | (dtPolyRef)i;
if (!filter.passFilter(pRef, tile, p))
continue;
// Calc area of the polygon.
float polyArea = 0.0f;
for (int j = 2; j < p.vertCount; ++j)
{
//float* va = &tile.verts[p.verts[0]*3];
//float* vb = &tile.verts[p.verts[j-1]*3];
//float* vc = &tile.verts[p.verts[j]*3];
polyArea += Detour.dtTriArea2D(tile.verts, p.verts[0]*3, tile.verts, p.verts[j-1]*3, tile.verts, p.verts[j]*3);
}
// Choose random polygon weighted by area, using reservoi sampling.
areaSum += polyArea;
float u = frand();
if (u*areaSum <= polyArea)
{
poly = p;
polyRef = pRef;
}
}
if (poly == null)
return DT_FAILURE;
// Randomly pick point on polygon.
//const float* v = &tile.verts[poly.verts[0]*3];
int vStart = poly.verts[0]*3;
float[] verts = new float[3*DT_VERTS_PER_POLYGON];
float[] areas = new float[DT_VERTS_PER_POLYGON];
Detour.dtVcopy(verts,0*3,tile.verts,vStart);
for (int j = 1; j < poly.vertCount; ++j)
{
//v = &tile.verts[poly.verts[j]*3];
Detour.dtVcopy(verts,j*3,tile.verts,poly.verts[j]*3);
}
float s = frand();
float t = frand();
float[] pt = new float[3];
dtRandomPointInConvexPoly(verts, poly.vertCount, areas, s, t, pt);
float h = 0.0f;
dtStatus status = getPolyHeight(polyRef, pt, ref h);
if (dtStatusFailed(status))
return status;
pt[1] = h;
Detour.dtVcopy(randomPt, 0 , pt, 0);
randomRef = polyRef;
return DT_SUCCESS;
}
