/// 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; }
/// 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; } }
/// 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; }
/// @} /// @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; }
/// 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; }
/// 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; }
/// 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 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; }
/// 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; }
/// 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; }
/// 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; }