From cd3a68dd9bae21495af4178c8b9ceced60ccfdd5 Mon Sep 17 00:00:00 2001 From: Mikko Mononen Date: Mon, 27 Feb 2012 07:17:51 +0000 Subject: [PATCH] Random polygon selection for Detour. --- Detour/Include/DetourCommon.h | 3 + Detour/Include/DetourNavMeshQuery.h | 24 +++ Detour/Source/DetourCommon.cpp | 41 ++++ Detour/Source/DetourNavMeshQuery.cpp | 275 ++++++++++++++++++++++++ RecastDemo/Include/NavMeshTesterTool.h | 6 + RecastDemo/Source/NavMeshTesterTool.cpp | 93 +++++++- RecastDemo/Source/Sample_SoloMesh.cpp | 3 + 7 files changed, 444 insertions(+), 1 deletion(-) diff --git a/Detour/Include/DetourCommon.h b/Detour/Include/DetourCommon.h index efdb4cb..34f46d8 100644 --- a/Detour/Include/DetourCommon.h +++ b/Detour/Include/DetourCommon.h @@ -473,6 +473,9 @@ inline void dtSwapEndian(float* v) dtSwapByte(x+0, x+3); dtSwapByte(x+1, x+2); } +void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, + const float s, const float t, float* out); + /// @} #endif // DETOURCOMMON_H diff --git a/Detour/Include/DetourNavMeshQuery.h b/Detour/Include/DetourNavMeshQuery.h index 9bf9fc5..b856b70 100644 --- a/Detour/Include/DetourNavMeshQuery.h +++ b/Detour/Include/DetourNavMeshQuery.h @@ -348,6 +348,30 @@ public: dtStatus getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter, float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount, const int maxSegments) const; + + /// 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. + dtStatus findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; + + /// 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. + dtStatus findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float maxRadius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const; /// Finds the closest point on the specified polygon. /// @param[in] ref The reference id of the polygon. diff --git a/Detour/Source/DetourCommon.cpp b/Detour/Source/DetourCommon.cpp index 493e011..e003bf6 100644 --- a/Detour/Source/DetourCommon.cpp +++ b/Detour/Source/DetourCommon.cpp @@ -333,3 +333,44 @@ bool dtOverlapPolyPoly2D(const float* polya, const int npolya, return true; } +// Returns a random point in a convex polygon. +// Adapted from Graphics Gems article. +void dtRandomPointInConvexPoly(const float* pts, const int npts, float* areas, + const float s, const float t, float* out) +{ + // Calc triangle araes + float areasum = 0.0f; + for (int i = 2; i < npts; i++) { + areas[i] = dtTriArea2D(&pts[0], &pts[(i-1)*3], &pts[i*3]); + areasum += dtMax(0.001f, areas[i]); + } + // Find sub triangle weighted by area. + const float thr = s*areasum; + float acc = 0.0f; + float u = 0.0f; + int tri = 0; + for (int i = 2; i < npts; i++) { + const float dacc = areas[i]; + if (thr >= acc && thr < (acc+dacc)) + { + u = (thr - acc) / dacc; + tri = i; + break; + } + acc += dacc; + } + + float v = dtSqrt(t); + + const float a = 1 - v; + const float b = (1 - u) * v; + const float c = u * v; + const float* pa = &pts[0]; + const float* pb = &pts[(tri-1)*3]; + const float* pc = &pts[tri*3]; + + out[0] = a*pa[0] + b*pb[0] + c*pc[0]; + out[1] = a*pa[1] + b*pb[1] + c*pc[1]; + out[2] = a*pa[2] + b*pb[2] + c*pc[2]; +} + diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp index 24ade0a..1606b90 100644 --- a/Detour/Source/DetourNavMeshQuery.cpp +++ b/Detour/Source/DetourNavMeshQuery.cpp @@ -216,6 +216,281 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) return DT_SUCCESS; } +dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + + // Randomly pick one tile. Assume that all tiles cover roughly the same area. + const dtMeshTile* tile = 0; + float tsum = 0.0f; + for (int i = 0; i < m_nav->getMaxTiles(); i++) + { + const dtMeshTile* t = m_nav->getTile(i); + if (!t || !t->header) continue; + + // Choose random tile using reservoi sampling. + const float area = 1.0f; // Could be tile area too. + tsum += area; + const float u = frand(); + if (u*tsum <= area) + tile = t; + } + if (!tile) + return DT_FAILURE; + + // Randomly pick one polygon weighted by polygon area. + const dtPoly* poly = 0; + dtPolyRef polyRef = 0; + const dtPolyRef base = m_nav->getPolyRefBase(tile); + + float areaSum = 0.0f; + for (int i = 0; i < tile->header->polyCount; ++i) + { + const dtPoly* p = &tile->polys[i]; + // Do not return off-mesh connection polygons. + if (p->getType() != DT_POLYTYPE_GROUND) + continue; + // Must pass filter + const dtPolyRef ref = base | (dtPolyRef)i; + if (!filter->passFilter(ref, tile, p)) + continue; + + // Calc area of the polygon. + float polyArea = 0.0f; + for (int j = 2; j < p->vertCount; ++j) + { + const float* va = &tile->verts[p->verts[0]*3]; + const float* vb = &tile->verts[p->verts[j-1]*3]; + const float* vc = &tile->verts[p->verts[j]*3]; + polyArea += dtTriArea2D(va,vb,vc); + } + + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + poly = p; + polyRef = ref; + } + } + + if (!poly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &tile->verts[poly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < poly->vertCount; ++j) + { + v = &tile->verts[poly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus status = getPolyHeight(polyRef, pt, &h); + if (dtStatusFailed(status)) + return status; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = polyRef; + + return DT_SUCCESS; +} + +dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius, + const dtQueryFilter* filter, float (*frand)(), + dtPolyRef* randomRef, float* randomPt) const +{ + dtAssert(m_nav); + dtAssert(m_nodePool); + dtAssert(m_openList); + + // Validate input + if (!startRef || !m_nav->isValidPolyRef(startRef)) + return DT_FAILURE | DT_INVALID_PARAM; + + const dtMeshTile* startTile = 0; + const dtPoly* startPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &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 = DT_NODE_OPEN; + m_openList->push(startNode); + + dtStatus status = DT_SUCCESS; + + const float radiusSqr = dtSqr(radius); + float areaSum = 0.0f; + + const dtMeshTile* randomTile = 0; + const dtPoly* randomPoly = 0; + dtPolyRef randomPolyRef = 0; + + while (!m_openList->empty()) + { + dtNode* bestNode = m_openList->pop(); + bestNode->flags &= ~DT_NODE_OPEN; + bestNode->flags |= DT_NODE_CLOSED; + + // Get poly and tile. + // The API input has been cheked already, skip checking internal data. + const dtPolyRef bestRef = bestNode->id; + const dtMeshTile* bestTile = 0; + const dtPoly* bestPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly); + + // Place random locations on on ground. + if (bestPoly->getType() == 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(va,vb,vc); + } + // Choose random polygon weighted by area, using reservoi sampling. + areaSum += polyArea; + const float u = frand(); + if (u*areaSum <= polyArea) + { + randomTile = bestTile; + randomPoly = bestPoly; + randomPolyRef = bestRef; + } + } + + + // Get parent poly and tile. + dtPolyRef parentRef = 0; + const dtMeshTile* parentTile = 0; + const dtPoly* parentPoly = 0; + if (bestNode->pidx) + parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id; + if (parentRef) + m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly); + + for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next) + { + const dtLink* link = &bestTile->links[i]; + dtPolyRef neighbourRef = link->ref; + // Skip invalid neighbours and do not follow back to parent. + if (!neighbourRef || neighbourRef == parentRef) + continue; + + // Expand to neighbour + const dtMeshTile* neighbourTile = 0; + const dtPoly* neighbourPoly = 0; + m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &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[3], vb[3]; + if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb)) + continue; + + // If the circle is not touching the next polygon, skip it. + float tseg; + float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg); + if (distSqr > radiusSqr) + continue; + + dtNode* neighbourNode = m_nodePool->getNode(neighbourRef); + if (!neighbourNode) + { + status |= DT_OUT_OF_NODES; + continue; + } + + if (neighbourNode->flags & DT_NODE_CLOSED) + continue; + + // Cost + if (neighbourNode->flags == 0) + dtVlerp(neighbourNode->pos, va, vb, 0.5f); + + const 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 & DT_NODE_OPEN) && total >= neighbourNode->total) + continue; + + neighbourNode->id = neighbourRef; + neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED); + neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode); + neighbourNode->total = total; + + if (neighbourNode->flags & DT_NODE_OPEN) + { + m_openList->modify(neighbourNode); + } + else + { + neighbourNode->flags = DT_NODE_OPEN; + m_openList->push(neighbourNode); + } + } + } + + if (!randomPoly) + return DT_FAILURE; + + // Randomly pick point on polygon. + const float* v = &randomTile->verts[randomPoly->verts[0]*3]; + float verts[3*DT_VERTS_PER_POLYGON]; + float areas[DT_VERTS_PER_POLYGON]; + dtVcopy(&verts[0*3],v); + for (int j = 1; j < randomPoly->vertCount; ++j) + { + v = &randomTile->verts[randomPoly->verts[j]*3]; + dtVcopy(&verts[j*3],v); + } + + const float s = frand(); + const float t = frand(); + + float pt[3]; + dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt); + + float h = 0.0f; + dtStatus stat = getPolyHeight(randomPolyRef, pt, &h); + if (dtStatusFailed(status)) + return stat; + pt[1] = h; + + dtVcopy(randomPt, pt); + *randomRef = randomPolyRef; + + return DT_SUCCESS; +} + + ////////////////////////////////////////////////////////////////////////////////////////// /// @par diff --git a/RecastDemo/Include/NavMeshTesterTool.h b/RecastDemo/Include/NavMeshTesterTool.h index 51f16f7..8f1c224 100644 --- a/RecastDemo/Include/NavMeshTesterTool.h +++ b/RecastDemo/Include/NavMeshTesterTool.h @@ -64,6 +64,11 @@ class NavMeshTesterTool : public SampleTool float m_smoothPath[MAX_SMOOTH*3]; int m_nsmoothPath; float m_queryPoly[4*3]; + + static const int MAX_RAND_POINTS = 64; + float m_randPoints[MAX_RAND_POINTS*3]; + int m_nrandPoints; + bool m_randPointsInCircle; float m_spos[3]; float m_epos[3]; @@ -72,6 +77,7 @@ class NavMeshTesterTool : public SampleTool bool m_hitResult; float m_distanceToWall; float m_neighbourhoodRadius; + float m_randomRadius; bool m_sposSet; bool m_eposSet; diff --git a/RecastDemo/Source/NavMeshTesterTool.cpp b/RecastDemo/Source/NavMeshTesterTool.cpp index 1a9a8c1..639f552 100644 --- a/RecastDemo/Source/NavMeshTesterTool.cpp +++ b/RecastDemo/Source/NavMeshTesterTool.cpp @@ -19,6 +19,7 @@ #define _USE_MATH_DEFINES #include #include +#include #include #include "SDL.h" #include "SDL_opengl.h" @@ -39,6 +40,13 @@ // Uncomment this to dump all the requests in stdout. #define DUMP_REQS +// Returns a random number [0..1) +static float frand() +{ +// return ((float)(rand() & 0xffff)/(float)0xffff); + return (float)rand()/(float)RAND_MAX; +} + inline bool inRange(const float* v1, const float* v2, const float r, const float h) { const float dx = v2[0] - v1[0]; @@ -152,6 +160,8 @@ NavMeshTesterTool::NavMeshTesterTool() : m_npolys(0), m_nstraightPath(0), m_nsmoothPath(0), + m_nrandPoints(0), + m_randPointsInCircle(false), m_hitResult(false), m_distanceToWall(0), m_sposSet(false), @@ -167,6 +177,7 @@ NavMeshTesterTool::NavMeshTesterTool() : m_polyPickExt[2] = 2; m_neighbourhoodRadius = 2.5f; + m_randomRadius = 5.0f; } NavMeshTesterTool::~NavMeshTesterTool() @@ -192,6 +203,7 @@ void NavMeshTesterTool::init(Sample* sample) } m_neighbourhoodRadius = sample->getAgentRadius() * 20.0f; + m_randomRadius = sample->getAgentRadius() * 30.0f; } void NavMeshTesterTool::handleMenu() @@ -248,6 +260,69 @@ void NavMeshTesterTool::handleMenu() m_toolMode = TOOLMODE_FIND_LOCAL_NEIGHBOURHOOD; recalc(); } + + imguiSeparator(); + + if (imguiButton("Set Random Start")) + { + dtStatus status = m_navQuery->findRandomPoint(&m_filter, frand, &m_startRef, m_spos); + if (dtStatusSucceed(status)) + { + m_sposSet = true; + recalc(); + } + } + if (imguiButton("Set Random End", m_sposSet)) + { + if (m_sposSet) + { + dtStatus status = m_navQuery->findRandomPointAroundCircle(m_startRef, m_spos, m_randomRadius, &m_filter, frand, &m_endRef, m_epos); + if (dtStatusSucceed(status)) + { + m_eposSet = true; + recalc(); + } + } + } + + imguiSeparator(); + + if (imguiButton("Make Random Points")) + { + m_randPointsInCircle = false; + m_nrandPoints = 0; + for (int i = 0; i < MAX_RAND_POINTS; i++) + { + float pt[3]; + dtPolyRef ref; + dtStatus status = m_navQuery->findRandomPoint(&m_filter, frand, &ref, pt); + if (dtStatusSucceed(status)) + { + dtVcopy(&m_randPoints[m_nrandPoints*3], pt); + m_nrandPoints++; + } + } + } + if (imguiButton("Make Random Points Around", m_sposSet)) + { + if (m_sposSet) + { + m_nrandPoints = 0; + m_randPointsInCircle = true; + for (int i = 0; i < MAX_RAND_POINTS; i++) + { + float pt[3]; + dtPolyRef ref; + dtStatus status = m_navQuery->findRandomPointAroundCircle(m_startRef, m_spos, m_randomRadius, &m_filter, frand, &ref, pt); + if (dtStatusSucceed(status)) + { + dtVcopy(&m_randPoints[m_nrandPoints*3], pt); + m_nrandPoints++; + } + } + } + } + imguiSeparator(); @@ -1184,7 +1259,23 @@ void NavMeshTesterTool::handleRender() duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_neighbourhoodRadius, duRGBA(64,16,0,220), 2.0f); dd.depthMask(true); } - } + } + + if (m_nrandPoints > 0) + { + dd.begin(DU_DRAW_POINTS, 6.0f); + for (int i = 0; i < m_nrandPoints; i++) + { + const float* p = &m_randPoints[i*3]; + dd.vertex(p[0],p[1]+0.1,p[2], duRGBA(220,32,16,192)); + } + dd.end(); + + if (m_randPointsInCircle && m_sposSet) + { + duDebugDrawCircle(&dd, m_spos[0], m_spos[1]+agentHeight/2, m_spos[2], m_randomRadius, duRGBA(64,16,0,220), 2.0f); + } + } } void NavMeshTesterTool::handleRenderOverlay(double* proj, double* model, int* view) diff --git a/RecastDemo/Source/Sample_SoloMesh.cpp b/RecastDemo/Source/Sample_SoloMesh.cpp index 83a78a0..73fb61d 100644 --- a/RecastDemo/Source/Sample_SoloMesh.cpp +++ b/RecastDemo/Source/Sample_SoloMesh.cpp @@ -238,6 +238,9 @@ void Sample_SoloMesh::handleRender() const float* bmin = m_geom->getMeshBoundsMin(); const float* bmax = m_geom->getMeshBoundsMax(); duDebugDrawBoxWire(&dd, bmin[0],bmin[1],bmin[2], bmax[0],bmax[1],bmax[2], duRGBA(255,255,255,128), 1.0f); + dd.begin(DU_DRAW_POINTS, 5.0f); + dd.vertex(bmin[0],bmin[1],bmin[2],duRGBA(255,255,255,128)); + dd.end(); if (m_navMesh && m_navQuery && (m_drawMode == DRAWMODE_NAVMESH ||