/// @}
/// @name Local Query Functions
///@{
/// Finds the polygon nearest to the specified center point.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] nearestRef The reference id of the nearest polygon.
/// @param[out] nearestPt The nearest point on the polygon. [opt] [(x, y, z)]
/// @returns The status flags for the query.
/// @par
///
/// @note If the search box does not intersect any polygons the search will
/// return #DT_SUCCESS, but @p nearestRef will be zero. So if in doubt, check
/// @p nearestRef before using @p nearestPt.
///
/// @warning This function is not suitable for large area searches. If the search
/// extents overlaps more than 128 polygons it may return an invalid result.
///
public dtStatus findNearestPoly(float[] center, float[] extents,
dtQueryFilter filter,
ref dtPolyRef nearestRef, ref float[] nearestPt)
{
Debug.Assert(m_nav != null);
nearestRef = 0;
// Get nearby polygons from proximity grid.
dtPolyRef[] polys = new dtPolyRef[128];
int polyCount = 0;
if (dtStatusFailed(queryPolygons(center, extents, filter, polys, ref polyCount, 128)))
return DT_FAILURE | DT_INVALID_PARAM;
// Find nearest polygon amongst the nearby polygons.
dtPolyRef nearest = 0;
float nearestDistanceSqr = float.MaxValue;
for (int i = 0; i < polyCount; ++i)
{
dtPolyRef polyRef = polys[i];
float[] closestPtPoly = new float[3];
float[] diff = new float[3];
bool posOverPoly = false;
float d = 0;
closestPointOnPoly(polyRef, center, closestPtPoly, ref 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)
{
dtMeshTile tile = null;
dtPoly poly = null;
m_nav.getTileAndPolyByRefUnsafe(polys[i], ref tile, ref poly);
d = (float)(Math.Abs(diff[1]) - tile.header.walkableClimb);
d = d > 0 ? d*d : 0;
}
else
{
d = dtVlenSqr(diff);
}
if (d < nearestDistanceSqr)
{
//if (nearestPt != null)
dtVcopy(nearestPt, closestPtPoly);
nearestDistanceSqr = d;
nearest = polyRef;
}
}
//if (nearestRef)
nearestRef = nearest;
return DT_SUCCESS;
}
/// Queries polygons within a tile.
public int queryPolygonsInTile(dtMeshTile tile, float[] qmin, float[] qmax,
dtQueryFilter filter,
dtPolyRef[] polys, int polyStart, int maxPolys)
{
Debug.Assert(m_nav != null);
if (tile.bvTree != null)
{
dtBVNode node = tile.bvTree[0];
//dtBVNode* end = &tile.bvTree[tile.header.bvNodeCount];
int endIndex = tile.header.bvNodeCount;
float[] tbmin = tile.header.bmin;
float[] tbmax = tile.header.bmax;
float qfac = tile.header.bvQuantFactor;
// Calculate quantized box
ushort[] bmin = new ushort[3];//, bmax[3];
ushort[] bmax = new ushort[3];
// dtClamp query box to world box.
float minx = dtClamp(qmin[0], tbmin[0], tbmax[0]) - tbmin[0];
float miny = dtClamp(qmin[1], tbmin[1], tbmax[1]) - tbmin[1];
float minz = dtClamp(qmin[2], tbmin[2], tbmax[2]) - tbmin[2];
float maxx = dtClamp(qmax[0], tbmin[0], tbmax[0]) - tbmin[0];
float maxy = dtClamp(qmax[1], tbmin[1], tbmax[1]) - tbmin[1];
float maxz = dtClamp(qmax[2], tbmin[2], tbmax[2]) - tbmin[2];
// Quantize
bmin[0] = (ushort)((int)(qfac * minx) & 0xfffe);
bmin[1] = (ushort)((int)(qfac * miny) & 0xfffe);
bmin[2] = (ushort)((int)(qfac * minz) & 0xfffe);
bmax[0] = (ushort)((int)(qfac * maxx + 1) | 1);
bmax[1] = (ushort)((int)(qfac * maxy + 1) | 1);
bmax[2] = (ushort)((int)(qfac * maxz + 1) | 1);
// Traverse tree
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
int n = 0;
int nodeIndex = 0;
while (nodeIndex < endIndex)
{
node = tile.bvTree[nodeIndex];
bool overlap = dtOverlapQuantBounds(bmin, bmax, node.bmin, node.bmax);
bool isLeafNode = node.i >= 0;
if (isLeafNode && overlap)
{
dtPolyRef polyRef = polyRefBase | (dtPolyRef)node.i;
if (filter.passFilter(polyRef, tile, tile.polys[node.i]))
{
if (n < maxPolys)
polys[polyStart + n++] = polyRef;
}
}
if (overlap || isLeafNode){
nodeIndex++;
}
else
{
int escapeIndex = -node.i;
nodeIndex += escapeIndex;
}
}
return n;
}
else
{
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
int n = 0;
dtPolyRef polyRefBase = m_nav.getPolyRefBase(tile);
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_OFFMESH_CONNECTION)
continue;
// Must pass filter
dtPolyRef polyRef = polyRefBase | (dtPolyRef)i;
if (!filter.passFilter(polyRef, tile, p))
continue;
// Calc polygon bounds.
//const float* v = &tile.verts[p.verts[0]*3];
int vStart = p.verts[0]*3;
dtVcopy(bmin,0, tile.verts,vStart);
dtVcopy(bmax,0, tile.verts,vStart);
for (int j = 1; j < p.vertCount; ++j)
{
//v = &tile.verts[p.verts[j]*3];
vStart = p.verts[j]*3;
dtVmin(bmin,0, tile.verts,vStart);
dtVmax(bmax,0, tile.verts,vStart);
}
if (dtOverlapBounds(qmin,qmax, bmin,bmax))
{
if (n < maxPolys)
polys[polyStart + n++] = polyRef;
}
}
return n;
}
}
/// @}
/// @name Miscellaneous Functions
/// @{
/// Returns true if the polygon reference is valid and passes the filter restrictions.
/// @param[in] ref The polygon reference to check.
/// @param[in] filter The filter to apply.
bool isValidPolyRef(dtPolyRef polyRef,dtQueryFilter filter)
{
dtMeshTile tile = null;
dtPoly poly = null;
dtStatus status = m_nav.getTileAndPolyByRef(polyRef, ref tile, ref 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(polyRef, tile, poly))
return false;
return true;
}
/// 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 the segments for the specified polygon, optionally including portals.
/// @param[in] ref The reference id of the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] segmentVerts The segments. [(ax, ay, az, bx, by, bz) * segmentCount]
/// @param[out] segmentRefs The reference ids of each segment's neighbor polygon.
/// Or zero if the segment is a wall. [opt] [(parentRef) * @p segmentCount]
/// @param[out] segmentCount The number of segments returned.
/// @param[in] maxSegments The maximum number of segments the result arrays can hold.
/// @returns The status flags for the query.
/// @par
///
/// If the @p segmentRefs parameter is provided, then all polygon segments will be returned.
/// Otherwise only the wall segments are returned.
///
/// A segment that is normally a portal will be included in the result set as a
/// wall if the @p filter results in the neighbor polygon becoomming impassable.
///
/// The @p segmentVerts and @p segmentRefs buffers should normally be sized for the
/// maximum segments per polygon of the source navigation mesh.
///
dtStatus getPolyWallSegments(dtPolyRef polyRef, dtQueryFilter filter,
float[] segmentVerts, dtPolyRef[] segmentRefs, ref int segmentCount,
int maxSegments)
{
Debug.Assert(m_nav != null);
segmentCount = 0;
dtMeshTile tile = null;
dtPoly poly = null;
if (dtStatusFailed(m_nav.getTileAndPolyByRef(polyRef, ref tile, ref poly)))
return DT_FAILURE | DT_INVALID_PARAM;
int n = 0;
const int MAX_INTERVAL = 16;
dtSegInterval[] ints = new dtSegInterval[MAX_INTERVAL];
dtcsArrayItemsCreate(ints);
int nints;
bool storePortals = segmentRefs != null;
dtStatus status = DT_SUCCESS;
for (int i = 0, j = (int)poly.vertCount-1; i < (int)poly.vertCount; j = i++) {
// Skip non-solid edges.
nints = 0;
if ((poly.neis[j] & DT_EXT_LINK) != 0) {
// Tile border.
for (uint k = poly.firstLink; k != DT_NULL_LINK; k = tile.links[k].next) {
dtLink link = tile.links[k];
if (link.edge == j) {
if (link.polyRef != 0) {
dtMeshTile neiTile = null;
dtPoly neiPoly = null;
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref neiTile, ref neiPoly);
if (filter.passFilter(link.polyRef, neiTile, neiPoly)) {
insertInterval(ints, ref nints, MAX_INTERVAL, link.bmin, link.bmax, link.polyRef);
}
}
}
}
} else {
// Internal edge
dtPolyRef neiRef = 0;
if (poly.neis[j] != 0) {
uint idx = (uint)(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];
int vjStart = poly.verts[j] * 3;
int viStart = poly.verts[i] * 3;
int segStart = n * 6;
dtVcopy(segmentVerts, segStart, tile.verts, vjStart);
dtVcopy(segmentVerts, segStart + 3, tile.verts, viStart);
if (segmentRefs != null)
segmentRefs[n] = neiRef;
n++;
} else {
status |= DT_BUFFER_TOO_SMALL;
}
continue;
}
// Add sentinels
insertInterval(ints, ref nints, MAX_INTERVAL, -1, 0, 0);
insertInterval(ints, ref nints, MAX_INTERVAL, 255, 256, 0);
// Store segments.
//const float* vj = &tile.verts[poly.verts[j]*3];
//const float* vi = &tile.verts[poly.verts[i]*3];
int vjStart2 = poly.verts[j] * 3;
int viStart2 = poly.verts[i] * 3;
for (int k = 1; k < nints; ++k) {
// Portal segment.
if (storePortals && ints[k].polyRef != 0) {
float tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f;
if (n < maxSegments) {
//float* seg = &segmentVerts[n*6];
int segStart = n * 6;
dtVlerp(segmentVerts, segStart, tile.verts, vjStart2, tile.verts, viStart2, tmin);
dtVlerp(segmentVerts, segStart + 3, tile.verts, vjStart2, tile.verts, viStart2, tmax);
if (segmentRefs != null)
segmentRefs[n] = ints[k].polyRef;
n++;
} else {
status |= DT_BUFFER_TOO_SMALL;
}
}
// Wall segment.
int imin = ints[k - 1].tmax;
int imax = ints[k].tmin;
if (imin != imax) {
float tmin = imin / 255.0f;
float tmax = imax / 255.0f;
if (n < maxSegments) {
//float* seg = &segmentVerts[n*6];
int segStart = n * 6;
dtVlerp(segmentVerts, segStart, tile.verts, vjStart2, tile.verts, viStart2, tmin);
dtVlerp(segmentVerts, segStart + 3, tile.verts, vjStart2, tile.verts, viStart2, tmax);
if (segmentRefs != null)
segmentRefs[n] = 0;
n++;
} else {
status |= DT_BUFFER_TOO_SMALL;
}
}
}
}
segmentCount = n;
return status;
}
/// Finds the distance from the specified position to the nearest polygon wall.
/// @param[in] startRef The reference id of the polygon containing @p centerPos.
/// @param[in] centerPos The center of the search circle. [(x, y, z)]
/// @param[in] maxRadius The radius of the search circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] hitDist The distance to the nearest wall from @p centerPos.
/// @param[out] hitPos The nearest position on the wall that was hit. [(x, y, z)]
/// @param[out] hitNormal The normalized ray formed from the wall point to the
/// source point. [(x, y, z)]
/// @returns The status flags for the query.
/// @par
///
/// @p hitPos is not adjusted using the height detail data.
///
/// @p hitDist will equal the search radius if there is no wall within the
/// radius. In this case the values of @p hitPos and @p hitNormal are
/// undefined.
///
/// The normal will become unpredicable if @p hitDist is a very small number.
///
dtStatus findDistanceToWall(dtPolyRef startRef, float[] centerPos, float maxRadius,
dtQueryFilter filter,
ref float hitDist, float[] hitPos, float[] hitNormal)
{
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;
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);
float radiusSqr = dtSqr(maxRadius);
dtStatus status = DT_SUCCESS;
while (!m_openList.empty())
{
dtNode bestNode = m_openList.pop();
//bestNode.flags &= ~DT_NODE_OPEN;
//bestNode.flags |= DT_NODE_CLOSED;
bestNode.dtcsClearFlag(dtNodeFlags.DT_NODE_OPEN);
bestNode.dtcsSetFlag(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);
// 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);
// Hit test walls.
for (int i = 0, j = (int)bestPoly.vertCount-1; i < (int)bestPoly.vertCount; j = i++)
{
// Skip non-solid edges.
if ((bestPoly.neis[j] & DT_EXT_LINK) != 0)
{
// Tile border.
bool solid = true;
for (uint k = bestPoly.firstLink; k != DT_NULL_LINK; k = bestTile.links[k].next)
{
dtLink link = bestTile.links[k];
if (link.edge == j)
{
if (link.polyRef != 0)
{
dtMeshTile neiTile = null;
dtPoly neiPoly = null;
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref neiTile, ref neiPoly);
if (filter.passFilter(link.polyRef, neiTile, neiPoly))
solid = false;
}
break;
}
}
if (!solid) continue;
}
else if (bestPoly.neis[j] != 0)
{
// Internal edge
uint idx = (uint)(bestPoly.neis[j]-1);
dtPolyRef polyRef = m_nav.getPolyRefBase(bestTile) | idx;
if (filter.passFilter(polyRef, bestTile, bestTile.polys[idx]))
continue;
}
// Calc distance to the edge.
//const float* vj = &bestTile.verts[bestPoly.verts[j]*3];
//const float* vi = &bestTile.verts[bestPoly.verts[i]*3];
int vjStart = bestPoly.verts[j]*3;
int viStart = bestPoly.verts[i]*3;
float tseg = .0f;
float distSqr = dtDistancePtSegSqr2D(centerPos, 0, bestTile.verts,vjStart, bestTile.verts, viStart,ref tseg);
// Edge is too far, skip.
if (distSqr > radiusSqr)
continue;
// Hit wall, update radius.
radiusSqr = distSqr;
// Calculate hit pos.
hitPos[0] = bestTile.verts[vjStart + 0] + (bestTile.verts[viStart + 0] - bestTile.verts[vjStart + 0])*tseg;
hitPos[1] = bestTile.verts[vjStart + 1] + (bestTile.verts[viStart + 1] - bestTile.verts[vjStart + 1])*tseg;
hitPos[2] = bestTile.verts[vjStart + 2] + (bestTile.verts[viStart + 2] - bestTile.verts[vjStart + 2])*tseg;
}
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);
// Skip off-mesh connections.
if (neighbourPoly.getType() == (byte)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Calc distance to the edge.
//const float* va = &bestTile.verts[bestPoly.verts[link.edge]*3];
//const float* vb = &bestTile.verts[bestPoly.verts[(link.edge+1) % bestPoly.vertCount]*3];
int vaStart = bestPoly.verts[link.edge]*3;
int vbStart = bestPoly.verts[(link.edge+1) % bestPoly.vertCount]*3;
float tseg = .0f;
float distSqr = dtDistancePtSegSqr2D(centerPos, 0, bestTile.verts, vaStart, bestTile.verts, vbStart, ref tseg);
// If the circle is not touching the next polygon, skip it.
if (distSqr > radiusSqr)
continue;
if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
dtNode neighbourNode = m_nodePool.getNode(neighbourRef);
if (neighbourNode == null)
{
status |= DT_OUT_OF_NODES;
continue;
}
if (neighbourNode.dtcsTestFlag(dtNodeFlags.DT_NODE_CLOSED))// .flags & DT_NODE_CLOSED)
continue;
// Cost
if (neighbourNode.flags == 0)
{
getEdgeMidPoint(bestRef, bestPoly, bestTile,
neighbourRef, neighbourPoly, neighbourTile, neighbourNode.pos);
}
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.dtcsTestFlag(dtNodeFlags.DT_NODE_OPEN) && total >= neighbourNode.total)
continue;
neighbourNode.id = neighbourRef;
//neighbourNode.flags = (neighbourNode.flags & ~DT_NODE_CLOSED);
neighbourNode.dtcsClearFlag(dtNodeFlags.DT_NODE_CLOSED);
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
neighbourNode.total = total;
if (neighbourNode.dtcsTestFlag(dtNodeFlags.DT_NODE_OPEN))// .flags & DT_NODE_OPEN)
{
m_openList.modify(neighbourNode);
}
else
{
//neighbourNode.flags |= DT_NODE_OPEN;
neighbourNode.dtcsSetFlag(dtNodeFlags.DT_NODE_OPEN);
m_openList.push(neighbourNode);
}
}
}
// Calc hit normal.
dtVsub(hitNormal, centerPos, hitPos);
dtVnormalize(hitNormal);
hitDist = (float) Math.Sqrt(radiusSqr);
return status;
}
/// 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;
}
/// Finds the non-overlapping navigation polygons in the local neighbourhood around the center position.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] centerPos The center of the query circle. [(x, y, z)]
/// @param[in] radius The radius of the query circle.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the circle.
/// @param[out] resultParent The reference ids of the parent polygons for each result.
/// Zero if a result polygon has no parent. [opt]
/// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
/// @par
///
/// This method is optimized for a small search radius and small number of result
/// polygons.
///
/// Candidate polygons are found by searching the navigation graph beginning at
/// the start polygon.
///
/// The same intersection test restrictions that apply to the findPolysAroundCircle
/// mehtod applies to this method.
///
/// The value of the center point is used as the start point for cost calculations.
/// It is not projected onto the surface of the mesh, so its y-value will effect
/// the costs.
///
/// Intersection tests occur in 2D. All polygons and the search circle are
/// projected onto the xz-plane. So the y-value of the center point does not
/// effect intersection tests.
///
/// If the result arrays are is too small to hold the entire result set, they will
/// be filled to capacity.
///
dtStatus findLocalNeighbourhood(dtPolyRef startRef, float[] centerPos, float radius,
dtQueryFilter filter,
dtPolyRef[] resultRef, dtPolyRef[] resultParent,
ref int resultCount, int maxResult)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_tinyNodePool != null);
resultCount = 0;
// Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
const int MAX_STACK = 48;
dtNode[] stack = new dtNode[MAX_STACK];
dtcsArrayItemsCreate(stack);
int nstack = 0;
m_tinyNodePool.clear();
dtNode startNode = m_tinyNodePool.getNode(startRef);
startNode.pidx = 0;
startNode.id = startRef;
startNode.flags = (byte)dtNodeFlags.DT_NODE_CLOSED;
stack[nstack++] = startNode;
float radiusSqr = dtSqr(radius);
float[] pa = new float[DT_VERTS_PER_POLYGON*3];
float[] pb = new float[DT_VERTS_PER_POLYGON*3];
dtStatus status = DT_SUCCESS;
int n = 0;
if (n < maxResult)
{
resultRef[n] = startNode.id;
if (resultParent != null)
resultParent[n] = 0;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
while (nstack != 0)
{
// Pop front.
dtNode curNode = stack[0];
for (int i = 0; i < nstack-1; ++i)
stack[i] = stack[i+1];
nstack--;
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
dtPolyRef curRef = curNode.id;
dtMeshTile curTile = null;
dtPoly curPoly = null;
m_nav.getTileAndPolyByRefUnsafe(curRef, ref curTile, ref curPoly);
for (uint i = curPoly.firstLink; i != DT_NULL_LINK; i = curTile.links[i].next)
{
dtLink link = curTile.links[i];
dtPolyRef neighbourRef = link.polyRef;
// Skip invalid neighbours.
if (neighbourRef == 0)
continue;
// Skip if cannot alloca more nodes.
dtNode neighbourNode = m_tinyNodePool.getNode(neighbourRef);
if (neighbourNode == null)
continue;
// Skip visited.
if (neighbourNode.dtcsTestFlag(dtNodeFlags.DT_NODE_CLOSED))// .flags & DT_NODE_CLOSED)
continue;
// Expand to neighbour
dtMeshTile neighbourTile = null;
dtPoly neighbourPoly = null;
m_nav.getTileAndPolyByRefUnsafe(neighbourRef, ref neighbourTile, ref neighbourPoly);
// Skip off-mesh connections.
if (neighbourPoly.getType() == (byte)dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// 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(curRef, curPoly, curTile, 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;
// Mark node visited, this is done before the overlap test so that
// we will not visit the poly again if the test fails.
//neighbourNode.flags |= DT_NODE_CLOSED;
neighbourNode.dtcsSetFlag(dtNodeFlags.DT_NODE_CLOSED);
neighbourNode.pidx = m_tinyNodePool.getNodeIdx(curNode);
// Check that the polygon does not collide with existing polygons.
// Collect vertices of the neighbour poly.
int npa = neighbourPoly.vertCount;
for (int k = 0; k < npa; ++k){
dtVcopy(pa,k*3, neighbourTile.verts,neighbourPoly.verts[k]*3);
}
bool overlap = false;
for (int j = 0; j < n; ++j)
{
dtPolyRef pastRef = resultRef[j];
// Connected polys do not overlap.
bool connected = false;
for (uint k = curPoly.firstLink; k != DT_NULL_LINK; k = curTile.links[k].next)
{
if (curTile.links[k].polyRef == pastRef)
{
connected = true;
break;
}
}
if (connected)
continue;
// Potentially overlapping.
dtMeshTile pastTile = null;
dtPoly pastPoly = null;
m_nav.getTileAndPolyByRefUnsafe(pastRef, ref pastTile, ref pastPoly);
// Get vertices and test overlap
int npb = pastPoly.vertCount;
for (int k = 0; k < npb; ++k){
dtVcopy(pb,k*3, pastTile.verts,pastPoly.verts[k]*3);
}
if (dtOverlapPolyPoly2D(pa,npa, pb,npb))
{
overlap = true;
break;
}
}
if (overlap)
continue;
// This poly is fine, store and advance to the poly.
if (n < maxResult)
{
resultRef[n] = neighbourRef;
if (resultParent != null)
resultParent[n] = curRef;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
if (nstack < MAX_STACK)
{
stack[nstack++] = neighbourNode;
}
}
}
resultCount = n;
return status;
}
/// Casts a 'walkability' ray along the surface of the navigation mesh from
/// the start position toward the end position.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position within the start polygon representing
/// the start of the ray. [(x, y, z)]
/// @param[in] endPos The position to cast the ray toward. [(x, y, z)]
/// @param[out] t The hit parameter. (FLT_MAX if no wall hit.)
/// @param[out] hitNormal The normal of the nearest wall hit. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] path The reference ids of the visited polygons. [opt]
/// @param[out] pathCount The number of visited polygons. [opt]
/// @param[in] maxPath The maximum number of polygons the @p path array can hold.
/// @returns The status flags for the query.
/// @par
///
/// This method is meant to be used for quick, short distance checks.
///
/// If the path array is too small to hold the result, it will be filled as
/// far as possible from the start postion toward the end position.
///
/// <b>Using the Hit Parameter (t)</b>
///
/// If the hit parameter is a very high value (FLT_MAX), then the ray has hit
/// the end position. In this case the path represents a valid corridor to the
/// end position and the value of @p hitNormal is undefined.
///
/// If the hit parameter is zero, then the start position is on the wall that
/// was hit and the value of @p hitNormal is undefined.
///
/// If 0 < t < 1.0 then the following applies:
///
/// @code
/// distanceToHitBorder = distanceToEndPosition * t
/// hitPoint = startPos + (endPos - startPos) * t
/// @endcode
///
/// <b>Use Case Restriction</b>
///
/// The raycast ignores the y-value of the end position. (2D check.) This
/// places significant limits on how it can be used. For example:
///
/// Consider a scene where there is a main floor with a second floor balcony
/// that hangs over the main floor. So the first floor mesh extends below the
/// balcony mesh. The start position is somewhere on the first floor. The end
/// position is on the balcony.
///
/// The raycast will search toward the end position along the first floor mesh.
/// If it reaches the end position's xz-coordinates it will indicate FLT_MAX
/// (no wall hit), meaning it reached the end position. This is one example of why
/// this method is meant for short distance checks.
///
dtStatus raycast(dtPolyRef startRef, float[] startPos, float[] endPos,
dtQueryFilter filter,
ref float t, float[] hitNormal, dtPolyRef[] path, ref int pathCount, int maxPath)
{
Debug.Assert(m_nav != null);
t = 0;
//if (pathCount)
pathCount = 0;
// Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
dtPolyRef curRef = startRef;
float[] verts = new float[DT_VERTS_PER_POLYGON*3];
int n = 0;
hitNormal[0] = 0;
hitNormal[1] = 0;
hitNormal[2] = 0;
dtStatus status = DT_SUCCESS;
while (curRef != 0)
{
// Cast ray against current polygon.
// The API input has been cheked already, skip checking internal data.
dtMeshTile tile = null;
dtPoly poly = null;
m_nav.getTileAndPolyByRefUnsafe(curRef, ref tile, ref poly);
// Collect vertices.
int nv = 0;
for (int i = 0; i < (int)poly.vertCount; ++i)
{
dtVcopy(verts, nv*3, tile.verts, poly.verts[i]*3);
nv++;
}
float tmin = 0, tmax = 0;
int segMin = 0, segMax = 0;
if (!dtIntersectSegmentPoly2D(startPos, endPos, verts, nv,ref tmin,ref tmax,ref segMin,ref segMax))
{
// Could not hit the polygon, keep the old t and report hit.
pathCount = n;
return status;
}
// Keep track of furthest t so far.
if (tmax > t)
t = tmax;
// 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)
{
t = float.MaxValue;
//if (pathCount)
pathCount = n;
return status;
}
// Follow neighbours.
dtPolyRef nextRef = 0;
for (uint i = poly.firstLink; i != DT_NULL_LINK; i = tile.links[i].next)
{
dtLink link = tile.links[i];
// Find link which contains this edge.
if ((int)link.edge != segMax)
continue;
// Get pointer to the next polygon.
dtMeshTile nextTile = null;
dtPoly nextPoly = null;
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref nextTile, ref nextPoly);
// Skip off-mesh connections.
if (nextPoly.getType() == (byte) dtPolyTypes.DT_POLYTYPE_OFFMESH_CONNECTION)
continue;
// Skip links based on filter.
if (!filter.passFilter(link.polyRef, nextTile, nextPoly))
continue;
// If the link is internal, just return the ref.
if (link.side == 0xff)
{
nextRef = link.polyRef;
break;
}
// If the link is at tile boundary,
// Check if the link spans the whole edge, and accept.
if (link.bmin == 0 && link.bmax == 255)
{
nextRef = link.polyRef;
break;
}
// Check for partial edge links.
int v0 = poly.verts[link.edge];
int v1 = poly.verts[(link.edge+1) % poly.vertCount];
//const float* left = &tile.verts[v0*3];
//const float* right = &tile.verts[v1*3];
int leftStart = v0*3;
int rightStart = v1*3;
// Check that the intersection lies inside the link portal.
if (link.side == 0 || link.side == 4)
{
// Calculate link size.
const float s = 1.0f/255.0f;
float lmin = tile.verts[leftStart + 2] + (tile.verts[rightStart + 2] - tile.verts[leftStart + 2])*(link.bmin*s);
float lmax = tile.verts[leftStart + 2] + (tile.verts[rightStart + 2] - tile.verts[leftStart + 2])*(link.bmax*s);
if (lmin > lmax)
dtSwap(ref lmin,ref lmax);
// Find Z intersection.
float z = startPos[2] + (endPos[2]-startPos[2])*tmax;
if (z >= lmin && z <= lmax)
{
nextRef = link.polyRef;
break;
}
}
else if (link.side == 2 || link.side == 6)
{
// Calculate link size.
const float s = 1.0f/255.0f;
float lmin = tile.verts[leftStart + 0] + (tile.verts[rightStart + 0] - tile.verts[leftStart + 0])*(link.bmin*s);
float lmax = tile.verts[leftStart + 0] + (tile.verts[rightStart + 0] - tile.verts[leftStart + 0])*(link.bmax*s);
if (lmin > lmax)
dtSwap(ref lmin,ref lmax);
// Find X intersection.
float x = startPos[0] + (endPos[0]-startPos[0])*tmax;
if (x >= lmin && x <= lmax)
{
nextRef = link.polyRef;
break;
}
}
}
if (nextRef == 0)
{
// No neighbour, we hit a wall.
// Calculate hit normal.
int a = segMax;
int b = segMax+1 < nv ? segMax+1 : 0;
//const float* va = &verts[a*3];
//const float* vb = &verts[b*3];
int vaStart = a*3;
int vbStart = b*3;
float dx = verts[vbStart + 0] - verts[vaStart + 0];
float dz = verts[vbStart + 2] - verts[vaStart + 2];
hitNormal[0] = dz;
hitNormal[1] = 0;
hitNormal[2] = -dx;
dtVnormalize(hitNormal);
//if (pathCount)
pathCount = n;
return status;
}
// No hit, advance to neighbour polygon.
curRef = nextRef;
}
//if (pathCount)
pathCount = n;
return status;
}
/// Finds the polygons along the naviation graph that touch the specified convex polygon.
/// @param[in] startRef The reference id of the polygon where the search starts.
/// @param[in] verts The vertices describing the convex polygon. (CCW)
/// [(x, y, z) * @p nverts]
/// @param[in] nverts The number of vertices in the polygon.
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultRef The reference ids of the polygons touched by the search polygon. [opt]
/// @param[out] resultParent The reference ids of the parent polygons for each result. Zero if a
/// result polygon has no parent. [opt]
/// @param[out] resultCost The search cost from the centroid point to the polygon. [opt]
/// @param[out] resultCount The number of polygons found.
/// @param[in] maxResult The maximum number of polygons the result arrays can hold.
/// @returns The status flags for the query.
/// @par
///
/// The order of the result set is from least to highest cost.
///
/// At least one result array must be provided.
///
/// A common use case for this method is to perform Dijkstra searches.
/// Candidate polygons are found by searching the graph beginning at the start
/// polygon.
///
/// The same intersection test restrictions that apply to findPolysAroundCircle()
/// method apply to this method.
///
/// The 3D centroid of the search polygon is used as the start position for cost
/// calculations.
///
/// Intersection tests occur in 2D. All polygons are projected onto the
/// xz-plane. So the y-values of the vertices do not effect intersection tests.
///
/// If the result arrays are is too small to hold the entire result set, they will
/// be filled to capacity.
///
dtStatus findPolysAroundShape(dtPolyRef startRef, float[] verts, int nverts,
dtQueryFilter filter,
dtPolyRef[] resultRef,dtPolyRef[] resultParent, float[] resultCost,
ref int resultCount, int maxResult)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_nodePool != null);
Debug.Assert(m_openList != null);
resultCount = 0;
// Validate input
if (startRef == 0 || !m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
m_nodePool.clear();
m_openList.clear();
float[] centerPos = new float[] {0,0,0};
for (int i = 0; i < nverts; ++i){
dtVadd(centerPos,0,centerPos,0,verts,i*3);
}
dtVscale(centerPos,centerPos,1.0f/nverts);
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;
int n = 0;
if (n < maxResult)
{
if (resultRef != null)
resultRef[n] = startNode.id;
if (resultParent != null)
resultParent[n] = 0;
if (resultCost != null)
resultCost[n] = 0;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
while (!m_openList.empty())
{
dtNode bestNode = m_openList.pop();
//bestNode.flags &= ~DT_NODE_OPEN;
//bestNode.flags |= DT_NODE_CLOSED;
bestNode.dtcsClearFlag(dtNodeFlags.DT_NODE_OPEN);
bestNode.dtcsSetFlag(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);
// 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 poly is not touching the edge to the next polygon, skip the connection it.
float tmin = 0, tmax = 0;
int segMin = 0, segMax = 0;
if (dtIntersectSegmentPoly2D(va, vb, verts, nverts,ref tmin,ref tmax,ref segMin,ref segMax))
continue;
if (tmin > 1.0f || tmax < 0.0f)
continue;
dtNode neighbourNode = m_nodePool.getNode(neighbourRef);
if (neighbourNode == null)
{
status |= DT_OUT_OF_NODES;
continue;
}
if (neighbourNode.dtcsTestFlag(dtNodeFlags.DT_NODE_CLOSED))
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.dtcsTestFlag(dtNodeFlags.DT_NODE_OPEN)) && total >= neighbourNode.total)
continue;
neighbourNode.id = neighbourRef;
neighbourNode.dtcsClearFlag(dtNodeFlags.DT_NODE_CLOSED);// = (neighbourNode.flags & ~DT_NODE_CLOSED);
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
neighbourNode.total = total;
if (neighbourNode.dtcsTestFlag(dtNodeFlags.DT_NODE_OPEN))// .flags & DT_NODE_OPEN)
{
m_openList.modify(neighbourNode);
}
else
{
if (n < maxResult)
{
if (resultRef != null)
resultRef[n] = neighbourNode.id;
if (resultParent != null)
resultParent[n] = m_nodePool.getNodeAtIdx(neighbourNode.pidx).id;
if (resultCost != null)
resultCost[n] = neighbourNode.total;
++n;
}
else
{
status |= DT_BUFFER_TOO_SMALL;
}
neighbourNode.flags = (byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(neighbourNode);
}
}
}
resultCount = n;
return status;
}
/// Moves from the start to the end position constrained to the navigation mesh.
/// @param[in] startRef The reference id of the start polygon.
/// @param[in] startPos A position of the mover within the start polygon. [(x, y, x)]
/// @param[in] endPos The desired end position of the mover. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] resultPos The result position of the mover. [(x, y, z)]
/// @param[out] visited The reference ids of the polygons visited during the move.
/// @param[out] visitedCount The number of polygons visited during the move.
/// @param[in] maxVisitedSize The maximum number of polygons the @p visited array can hold.
/// @returns The status flags for the query.
/// @par
///
/// This method is optimized for small delta movement and a small number of
/// polygons. If used for too great a distance, the result set will form an
/// incomplete path.
///
/// @p resultPos will equal the @p endPos if the end is reached.
/// Otherwise the closest reachable position will be returned.
///
/// @p resultPos is not projected onto the surface of the navigation
/// mesh. Use #getPolyHeight if this is needed.
///
/// This method treats the end position in the same manner as
/// the #raycast method. (As a 2D point.) See that method's documentation
/// for details.
///
/// If the @p visited array is too small to hold the entire result set, it will
/// be filled as far as possible from the start position toward the end
/// position.
///
public dtStatus moveAlongSurface(dtPolyRef startRef, float[] startPos, float[] endPos,
dtQueryFilter filter,
float[] resultPos, dtPolyRef[] visited, ref int visitedCount, int maxVisitedSize)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_tinyNodePool != null);
visitedCount = 0;
// Validate input
if (startRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
if (!m_nav.isValidPolyRef(startRef))
return DT_FAILURE | DT_INVALID_PARAM;
dtStatus status = DT_SUCCESS;
const int MAX_STACK = 48;
dtNode[] stack = new dtNode[MAX_STACK];
int nstack = 0;
m_tinyNodePool.clear();
dtNode startNode = m_tinyNodePool.getNode(startRef);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
startNode.id = startRef;
startNode.flags =(byte) dtNodeFlags.DT_NODE_CLOSED;
stack[nstack++] = startNode;
float[] bestPos = new float[3];
float bestDist = float.MaxValue;
dtNode bestNode = null;
dtVcopy(bestPos, startPos);
// Search constraints
float[] searchPos = new float[3];//, searchRadSqr;
float searchRadSqr = .0f;
dtVlerp(searchPos, startPos, endPos, 0.5f);
searchRadSqr = dtSqr(dtVdist(startPos, endPos)/2.0f + 0.001f);
float[] verts = new float[DT_VERTS_PER_POLYGON*3];
while (nstack != 0)
{
// Pop front.
dtNode curNode = stack[0];
for (int i = 0; i < nstack-1; ++i)
stack[i] = stack[i+1];
nstack--;
// Get poly and tile.
// The API input has been cheked already, skip checking internal data.
dtPolyRef curRef = curNode.id;
dtMeshTile curTile = null;
dtPoly curPoly = null;
m_nav.getTileAndPolyByRefUnsafe(curRef, ref curTile, ref curPoly);
// Collect vertices.
int nverts = curPoly.vertCount;
for (int i = 0; i < nverts; ++i){
dtVcopy(verts,i*3, curTile.verts,curPoly.verts[i]*3);
}
// If target is inside the poly, stop search.
if (dtPointInPolygon(endPos, verts, nverts))
{
bestNode = curNode;
dtVcopy(bestPos, endPos);
break;
}
// Find wall edges and find nearest point inside the walls.
for (int i = 0, j = (int)curPoly.vertCount-1; i < (int)curPoly.vertCount; j = i++)
{
// Find links to neighbours.
const int MAX_NEIS = 8;
int nneis = 0;
dtPolyRef[] neis = new dtPolyRef[MAX_NEIS];
if ((curPoly.neis[j] & DT_EXT_LINK) != 0)
{
// Tile border.
for (uint k = curPoly.firstLink; k != DT_NULL_LINK; k = curTile.links[k].next)
{
dtLink link = curTile.links[k];
if (link.edge == j)
{
if (link.polyRef != 0)
{
dtMeshTile neiTile = null;
dtPoly neiPoly = null;
m_nav.getTileAndPolyByRefUnsafe(link.polyRef, ref neiTile, ref neiPoly);
if (filter.passFilter(link.polyRef, neiTile, neiPoly))
{
if (nneis < MAX_NEIS)
neis[nneis++] = link.polyRef;
}
}
}
}
}
else if (curPoly.neis[j] != 0)
{
uint idx = (uint)(curPoly.neis[j]-1);
dtPolyRef polyRef = m_nav.getPolyRefBase(curTile) | idx;
if (filter.passFilter(polyRef, curTile, curTile.polys[idx]))
{
// Internal edge, encode id.
neis[nneis++] = polyRef;
}
}
if (nneis == 0)
{
// Wall edge, calc distance.
//const float* vj = &verts[j*3];
//const float* vi = &verts[i*3];
int vjStart = j*3;
int viStart = i*3;
float tseg = .0f;
float distSqr = dtDistancePtSegSqr2D(endPos, 0, verts, vjStart, verts, viStart, ref tseg);
if (distSqr < bestDist)
{
// Update nearest distance.
dtVlerp(bestPos,0, verts, vjStart,verts, viStart, tseg);
bestDist = distSqr;
bestNode = curNode;
}
}
else
{
for (int k = 0; k < nneis; ++k)
{
// Skip if no node can be allocated.
dtNode neighbourNode = m_tinyNodePool.getNode(neis[k]);
if (neighbourNode == null)
continue;
// Skip if already visited.
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_CLOSED) != 0)
continue;
// Skip the link if it is too far from search constraint.
// TODO: Maybe should use getPortalPoints(), but this one is way faster.
int vjStart = j*3;
int viStart = i*3;
float tseg = .0f;
float distSqr = dtDistancePtSegSqr2D(searchPos, 0,verts, vjStart,verts, viStart, ref tseg);
if (distSqr > searchRadSqr){
continue;
}
// Mark as the node as visited and push to queue.
if (nstack < MAX_STACK)
{
neighbourNode.pidx = m_tinyNodePool.getNodeIdx(curNode);
neighbourNode.dtcsSetFlag(dtNodeFlags.DT_NODE_CLOSED);
stack[nstack++] = neighbourNode;
}
}
}
}
}
int n = 0;
if (bestNode != null)
{
// Reverse the path.
dtNode prev = null;
dtNode node = bestNode;
do
{
dtNode next = m_tinyNodePool.getNodeAtIdx(node.pidx);
node.pidx = m_tinyNodePool.getNodeIdx(prev);
prev = node;
node = next;
}
while (node != null);
// Store result
node = prev;
do
{
visited[n++] = node.id;
if (n >= maxVisitedSize)
{
status |= DT_BUFFER_TOO_SMALL;
break;
}
node = m_tinyNodePool.getNodeAtIdx(node.pidx);
}
while (node != null);
}
dtVcopy(resultPos, bestPos);
visitedCount = n;
return status;
}
public void dtcsClear() {
status = 0;
lastBestNode = null;
lastBestNodeCost = .0f;
startRef = 0;
endRef = 0;
for (int i = 0; i < 3; ++i) {
startPos[i] = .0f;
endPos[i] = .0f;
}
filter = null;
}
///@}
/// @name Sliced Pathfinding Functions
/// Common use case:
/// -# Call initSlicedFindPath() to initialize the sliced path query.
/// -# Call updateSlicedFindPath() until it returns complete.
/// -# Call finalizeSlicedFindPath() to get the path.
///@{
/// Intializes a sliced path query.
/// @param[in] startRef The refrence id of the start polygon.
/// @param[in] endRef The reference id of the end polygon.
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @returns The status flags for the query.
/// @par
///
/// @warning Calling any non-slice methods before calling finalizeSlicedFindPath()
/// or finalizeSlicedFindPathPartial() may result in corrupted data!
///
/// The @p filter pointer is stored and used for the duration of the sliced
/// path query.
///
public dtStatus initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef,
float[] startPos, float[] endPos,
dtQueryFilter filter)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_nodePool != null);
Debug.Assert(m_openList != null);
// Init path state.
//memset(&m_query, 0, sizeof(dtQueryData));
m_query.status = DT_FAILURE;
m_query.startRef = startRef;
m_query.endRef = endRef;
dtVcopy(m_query.startPos, startPos);
dtVcopy(m_query.endPos, endPos);
m_query.filter = filter;
if (startRef == 0 || endRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
// Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
return DT_FAILURE | DT_INVALID_PARAM;
if (startRef == endRef)
{
m_query.status = DT_SUCCESS;
return DT_SUCCESS;
}
m_nodePool.clear();
m_openList.clear();
dtNode startNode = m_nodePool.getNode(startRef);
dtVcopy(startNode.pos, startPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = dtVdist(startPos, endPos) * H_SCALE;
startNode.id = startRef;
startNode.flags = (byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(startNode);
m_query.status = DT_IN_PROGRESS;
m_query.lastBestNode = startNode;
m_query.lastBestNodeCost = startNode.total;
return m_query.status;
}
/// Finds a path from the start polygon to the end polygon.
/// @param[in] startRef The refrence id of the start polygon.
/// @param[in] endRef The reference id of the end polygon.
/// @param[in] startPos A position within the start polygon. [(x, y, z)]
/// @param[in] endPos A position within the end polygon. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] path An ordered list of polygon references representing the path. (Start to end.)
/// [(polyRef) * @p pathCount]
/// @param[out] pathCount The number of polygons returned in the @p path array.
/// @param[in] maxPath The maximum number of polygons the @p path array can hold. [Limit: >= 1]
/// @par
///
/// If the end polygon cannot be reached through the navigation graph,
/// the last polygon in the path will be the nearest the end polygon.
///
/// If the path array is to small to hold the full result, it will be filled as
/// far as possible from the start polygon toward the end polygon.
///
/// The start and end positions are used to calculate traversal costs.
/// (The y-values impact the result.)
///
public dtStatus findPath(dtPolyRef startRef, dtPolyRef endRef,
float[] startPos, float[] endPos,
dtQueryFilter filter,
dtPolyRef[] path, ref int pathCount, int maxPath)
{
Debug.Assert(m_nav != null);
Debug.Assert(m_nodePool != null);
Debug.Assert(m_openList != null);
pathCount = 0;
if (startRef == 0 || endRef == 0)
return DT_FAILURE | DT_INVALID_PARAM;
if (maxPath == 0)
return DT_FAILURE | DT_INVALID_PARAM;
// Validate input
if (!m_nav.isValidPolyRef(startRef) || !m_nav.isValidPolyRef(endRef))
return DT_FAILURE | DT_INVALID_PARAM;
if (startRef == endRef)
{
path[0] = startRef;
pathCount = 1;
return DT_SUCCESS;
}
m_nodePool.clear();
m_openList.clear();
dtNode startNode = m_nodePool.getNode(startRef);
dtVcopy(startNode.pos, startPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = dtVdist(startPos, endPos) * H_SCALE;
startNode.id = startRef;
startNode.flags =(byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(startNode);
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.
dtNode bestNode = m_openList.pop();
unchecked{
bestNode.flags &= (byte)( ~ dtNodeFlags.DT_NODE_OPEN );
}
bestNode.flags |= (byte)dtNodeFlags.DT_NODE_CLOSED;
// Reached the goal, stop searching.
if (bestNode.id == endRef)
{
lastBestNode = bestNode;
break;
}
// Get current 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);
// 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)
{
dtPolyRef neighbourRef = bestTile.links[i].polyRef;
// Skip invalid ids and do not expand back to where we came from.
if (neighbourRef == 0 || neighbourRef == parentRef)
continue;
// Get neighbour poly and tile.
// The API input has been cheked already, skip checking internal data.
dtMeshTile neighbourTile = null;
dtPoly neighbourPoly = null;
m_nav.getTileAndPolyByRefUnsafe(neighbourRef, ref neighbourTile, ref neighbourPoly);
if (!filter.passFilter(neighbourRef, neighbourTile, neighbourPoly))
continue;
dtNode neighbourNode = m_nodePool.getNode(neighbourRef);
if (neighbourNode == null)
{
status |= DT_OUT_OF_NODES;
continue;
}
// If the node is visited the first time, calculate node position.
if (neighbourNode.flags == 0)
{
getEdgeMidPoint(bestRef, bestPoly, bestTile,
neighbourRef, neighbourPoly, neighbourTile,
neighbourNode.pos);
}
// Calculate cost and heuristic.
float cost = 0;
float heuristic = 0;
// Special case for last node.
if (neighbourRef == endRef)
{
// Cost
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
float endCost = filter.getCost(neighbourNode.pos, endPos,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly,
0, null, null);
cost = bestNode.cost + curCost + endCost;
heuristic = 0;
}
else
{
// Cost
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos,
parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly,
neighbourRef, neighbourTile, neighbourPoly);
cost = bestNode.cost + curCost;
heuristic = dtVdist(neighbourNode.pos, endPos)*H_SCALE;
}
float total = cost + heuristic;
// 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;
// The node is already visited and process, and the new result is worse, skip.
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_CLOSED) != 0 && total >= neighbourNode.total)
continue;
// Add or update the node.
neighbourNode.pidx = m_nodePool.getNodeIdx(bestNode);
neighbourNode.id = neighbourRef;
unchecked{
neighbourNode.flags = (byte)(neighbourNode.flags & (byte) ~dtNodeFlags.DT_NODE_CLOSED);
}
neighbourNode.cost = cost;
neighbourNode.total = total;
if ((neighbourNode.flags & (byte)dtNodeFlags.DT_NODE_OPEN) != 0)
{
// Already in open, update node location.
m_openList.modify(neighbourNode);
}
else
{
// Put the node in open list.
neighbourNode.flags |= (byte)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(neighbourNode);
}
// Update nearest node to target so far.
if (heuristic < lastBestNodeCost)
{
lastBestNodeCost = heuristic;
lastBestNode = neighbourNode;
}
}
}
if (lastBestNode.id != endRef)
status |= DT_PARTIAL_RESULT;
// Reverse the path.
dtNode prev = null;
dtNode node = lastBestNode;
do
{
dtNode next = m_nodePool.getNodeAtIdx(node.pidx);
node.pidx = m_nodePool.getNodeIdx(prev);
prev = node;
node = next;
}
while (node != null);
// Store path
node = prev;
int n = 0;
do
{
path[n++] = node.id;
if (n >= maxPath)
{
status |= DT_BUFFER_TOO_SMALL;
break;
}
node = m_nodePool.getNodeAtIdx(node.pidx);
}
while (node != null);
pathCount = n;
return status;
}
/// Finds polygons that overlap the search box.
/// @param[in] center The center of the search box. [(x, y, z)]
/// @param[in] extents The search distance along each axis. [(x, y, z)]
/// @param[in] filter The polygon filter to apply to the query.
/// @param[out] polys The reference ids of the polygons that overlap the query box.
/// @param[out] polyCount The number of polygons in the search result.
/// @param[in] maxPolys The maximum number of polygons the search result can hold.
/// @returns The status flags for the query.
/// @par
///
/// If no polygons are found, the function will return #DT_SUCCESS with a
/// @p polyCount of zero.
///
/// If @p polys is too small to hold the entire result set, then the array will
/// be filled to capacity. The method of choosing which polygons from the
/// full set are included in the partial result set is undefined.
///
public dtStatus queryPolygons(float[] center, float[] extents,
dtQueryFilter filter,
dtPolyRef[] polys, ref int polyCount, int maxPolys)
{
Debug.Assert(m_nav != null);
float[] bmin = new float[3];//, bmax[3];
float[] bmax = new float[3];
dtVsub(bmin, center, extents);
dtVadd(bmax, center, extents);
// Find tiles the query touches.
int minx = 0, miny = 0, maxx = 0 , maxy = 0;
m_nav.calcTileLoc(bmin, ref minx, ref miny);
m_nav.calcTileLoc(bmax, ref maxx, ref maxy);
int MAX_NEIS = 32;
dtMeshTile[] neis = new dtMeshTile[MAX_NEIS];
int n = 0;
for (int y = miny; y <= maxy; ++y)
{
for (int x = minx; x <= maxx; ++x)
{
int nneis = m_nav.getTilesAt(x,y,neis,MAX_NEIS);
for (int j = 0; j < nneis; ++j)
{
n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys, n, maxPolys-n);
if (n >= maxPolys)
{
polyCount = n;
return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
}
}
}
}
polyCount = n;
return DT_SUCCESS;
}
