public dtNode pop()
{
dtNode result = m_heap[0];
m_size--;
trickleDown(0, m_heap[m_size]);
return(result);
}
public uint getNodeIdx(dtNode node)
{
if (node == null)
{
return(0);
}
return((uint)(System.Array.IndexOf(m_nodes, node)) + 1);
}
public void modify(dtNode node)
{
for (int i = 0; i < m_size; ++i)
{
if (m_heap[i] == node)
{
bubbleUp(i, node);
return;
}
}
}
public void modify(dtNode node)
{
for (int i = 0; i < m_size; ++i)
{
if (m_heap[i] == node)
{
bubbleUp(i, node);
return;
}
}
}
/*int getMemUsed()
{
return sizeof(*this) +sizeof(dtNode*)*(m_capacity+1);
}*/
private void bubbleUp(int i, dtNode node)
{
int parent = (i-1)/2;
// note: (index > 0) means there is a parent
while ((i > 0) && (m_heap[parent].total > node.total))
{
m_heap[i] = m_heap[parent];
i = parent;
parent = (i-1)/2;
}
m_heap[i] = node;
}
public void bubbleUp(int i, dtNode node)
{
int parent = (i - 1) / 2;
// note: (index > 0) means there is a parent
while ((i > 0) && (m_heap[parent].total > node.total))
{
m_heap[i] = m_heap[parent];
i = parent;
parent = (i - 1) / 2;
}
m_heap[i] = node;
}
private void trickleDown(int i, dtNode node)
{
int child = (i*2)+1;
while (child < m_size)
{
if (((child+1) < m_size) &&
(m_heap[child].total > m_heap[child+1].total))
{
child++;
}
m_heap[i] = m_heap[child];
i = child;
child = (i*2)+1;
}
bubbleUp(i, node);
}
public void trickleDown(int i, dtNode node)
{
int child = (i * 2) + 1;
while (child < m_size)
{
if (((child + 1) < m_size) &&
(m_heap[child].total > m_heap[child + 1].total))
{
child++;
}
m_heap[i] = m_heap[child];
i = child;
child = (i * 2) + 1;
}
bubbleUp(i, node);
}
public dtNodePool(int maxNodes, int hashSize)
{
m_maxNodes = maxNodes;
m_hashSize = hashSize;
m_nodes = new dtNode[m_maxNodes];
m_next = new ushort[m_maxNodes];
m_first = new ushort[m_hashSize];
for( int i=0; i<m_maxNodes; ++i )
{
m_next[i] = 0xff;
m_nodes[i] = new dtNode();
m_nodes[i].index = (uint)i;
}
for( int i=0; i<m_hashSize; ++i )
{
m_first[i] = 0xff;
}
}
public dtNode getNode(dtPolyRef id, byte state = 0)
{
uint bucket = (uint)(dtHashRef(id) & (m_hashSize - 1));
dtNodeIndex i = m_first[bucket];
dtNode node = null;
while (i != DT_NULL_IDX)
{
if (m_nodes[i].id == id && m_nodes[i].state == state)
{
return(m_nodes[i]);
}
i = m_next[i];
}
if (m_nodeCount >= m_maxNodes)
{
return(null);
}
i = (dtNodeIndex)m_nodeCount;
m_nodeCount++;
// Init node
node = m_nodes[i];
node.pidx = 0;
node.cost = 0;
node.total = 0;
node.id = id;
node.state = state;
node.flags = 0;
m_next[i] = m_first[bucket];
m_first[bucket] = i;
return(node);
}
/// 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;
}
/// 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;
}
// Finds path from start polygon to end polygon.
// If target polygon canno be reached through the navigation graph,
// the last node on the array is nearest node to the end polygon.
// Params:
// startRef - (in) ref to path start polygon.
// endRef - (in) ref to path end polygon.
// path - (out) array holding the search result.
// maxPathSize - (in) The max number of polygons the path array can hold.
// Returns: Number of polygons in search result array.
public int findPath(ushort startRef, ushort endRef, Vector3 startPos, Vector3 endPos, ushort[] path, int maxPathSize)
{
if (null == m_header)
return 0;
if ( 0 == startRef || 0 == endRef)
return 0;
if (0 == maxPathSize)
return 0;
if (startRef == endRef)
{
path[0] = startRef;
return 1;
}
m_nodePool.clear();
m_openList.clear();
const float H_SCALE = 1.1f; // Heuristic scale.
dtNode startNode = m_nodePool.getNode(startRef);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = vdist(startPos, endPos) * H_SCALE;
startNode.id = startRef;
startNode.flags = (uint)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(startNode);
dtNode lastBestNode = startNode;
float lastBestNodeCost = startNode.total;
while (!m_openList.empty())
{
dtNode bestNode = m_openList.pop();
if (bestNode.id == endRef)
{
lastBestNode = bestNode;
break;
}
dtStatPoly poly = getPoly((int)bestNode.id-1);
for (int i = 0; i < (int)poly.nv; ++i)
{
ushort neighbour = poly.n[i];
if ( 0!=neighbour)
{
// Skip parent node.
if ( 0!=bestNode.pidx && m_nodePool.getNodeAtIdx(bestNode.pidx).id == neighbour)
continue;
dtNode parent = bestNode;
dtNode newNode = new dtNode();
newNode.pidx = m_nodePool.getNodeIdx(parent);
newNode.id = neighbour;
// Calculate cost.
//float p0[3], p1[3];
Vector3 p0 = new Vector3();
Vector3 p1 = new Vector3();
if (0==parent.pidx)
p0 = startPos;
else
getEdgeMidPoint(m_nodePool.getNodeAtIdx(parent.pidx).id, parent.id, ref p0);
getEdgeMidPoint(parent.id, newNode.id, ref p1);
newNode.cost = parent.cost + vdist(p0,p1);
// Special case for last node.
if (newNode.id == endRef)
newNode.cost += vdist(p1, endPos);
// Heuristic
float h = vdist(p1,endPos)*H_SCALE;
newNode.total = newNode.cost + h;
dtNode actualNode = m_nodePool.getNode(newNode.id);
if (null == actualNode)
continue;
if (!( (0!=(actualNode.flags & (uint)dtNodeFlags.DT_NODE_OPEN)) && newNode.total > actualNode.total) &&
!( (0!=(actualNode.flags & (uint)dtNodeFlags.DT_NODE_CLOSED)) && newNode.total > actualNode.total))
{
actualNode.flags &= ~ ((uint)dtNodeFlags.DT_NODE_CLOSED);
actualNode.pidx = newNode.pidx;
actualNode.cost = newNode.cost;
actualNode.total = newNode.total;
if (h < lastBestNodeCost)
{
lastBestNodeCost = h;
lastBestNode = actualNode;
}
if ( 0 != (actualNode.flags & (uint)dtNodeFlags.DT_NODE_OPEN) )
{
m_openList.modify(actualNode);
}
else
{
actualNode.flags |= (uint)dtNodeFlags.DT_NODE_OPEN;
m_openList.push(actualNode);
}
}
}
}
bestNode.flags |= (uint)dtNodeFlags.DT_NODE_CLOSED;
}
// 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 ( null != node );
// Store path
node = prev;
int n = 0;
do
{
path[n++] = (ushort)node.id;
node = m_nodePool.getNodeAtIdx(node.pidx);
}
while ( null != node && n < maxPathSize);
return n;
}
public uint getNodeIdx(dtNode node)
{
if (null == node)
return 0;
return node.index+1;
}
public void push(dtNode node)
{
m_size++;
bubbleUp(m_size - 1, node);
}
public void push(dtNode node) {
m_size++;
bubbleUp(m_size - 1, node);
}
public uint getNodeIdx(dtNode node) {
if (node == null)
return 0;
return (uint)(System.Array.IndexOf(m_nodes, node)) + 1;
}
