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