Random polygon selection for Detour.

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

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.

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];
+}
+

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

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;
 

RecastDemo/Source/NavMeshTesterTool.cpp

@@ -19,6 +19,7 @@
 #define _USE_MATH_DEFINES
 #include <math.h>
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #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)

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 ||