Merge branch 'master' of https://github.com/axelrodR/recastnavigation

Detour/Include/DetourNavMesh.h

@@ -613,8 +613,7 @@ private:
 	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
 									const float* extents, float* nearestPt) const;
 	/// Returns closest point on polygon.
-	void closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
+	void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
-								  const float* pos, float* closest) const;
 	
 	dtNavMeshParams m_params;			///< Current initialization params. TODO: do not store this info twice.
 	float m_orig[3];					///< Origin of the tile (0,0)

Detour/Include/DetourNavMeshQuery.h

@@ -381,8 +381,9 @@ public:
 	///  @param[in]		ref			The reference id of the polygon.
 	///  @param[in]		pos			The position to check. [(x, y, z)]
 	///  @param[out]	closest		The closest point on the polygon. [(x, y, z)]
+	///  @param[out]	posOverPoly	True of the position is over the polygon.
 	/// @returns The status flags for the query.
-	dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const;
+	dtStatus closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
 	
 	/// Returns a point on the boundary closest to the source point if the source point is outside the 
 	/// polygon's xz-bounds.
@@ -431,11 +432,6 @@ private:
 	/// Queries polygons within a tile.
 	int queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax, const dtQueryFilter* filter,
 							dtPolyRef* polys, const int maxPolys) const;
-	/// Find nearest polygon within a tile.
-	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
-									const dtQueryFilter* filter, float* nearestPt) const;
-	/// Returns closest point on polygon.
-	void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const;
 
 	/// Returns portal points between two polygons.
 	dtStatus getPortalPoints(dtPolyRef from, dtPolyRef to, float* left, float* right,

Detour/Source/DetourNavMesh.cpp

@@ -617,10 +617,12 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
 	}
 }
 
-void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
+void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
-										 const float* pos, float* closest) const
 {
-	const dtPoly* poly = &tile->polys[ip];
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
+	
 	// Off-mesh connections don't have detail polygons.
 	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 	{
@@ -630,9 +632,12 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
 		const float d1 = dtVdist(pos, v1);
 		const float u = d0 / (d0+d1);
 		dtVlerp(closest, v0, v1, u);
+		if (posOverPoly)
+			*posOverPoly = false;
 		return;
 	}
 	
+	const unsigned int ip = (unsigned int)(poly - tile->polys);
 	const dtPolyDetail* pd = &tile->detailMeshes[ip];
 	
 	// Clamp point to be inside the polygon.
@@ -660,6 +665,14 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
 		const float* va = &verts[imin*3];
 		const float* vb = &verts[((imin+1)%nv)*3];
 		dtVlerp(closest, va, vb, edget[imin]);
+		
+		if (posOverPoly)
+			*posOverPoly = false;
+	}
+	else
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
 	}
 	
 	// Find height at the location.
@@ -702,11 +715,26 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
-		closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly);
+		float diff[3];
-		float d = dtVdistSqr(center, closestPtPoly);
+		bool posOverPoly = false;
+		float d = 0;
+		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+		// If a point is directly over a polygon and closer than
+		// climb height, favor that instead of straight line nearest point.
+		dtVsub(diff, center, closestPtPoly);
+		if (posOverPoly)
+		{
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
+		}
+		
 		if (d < nearestDistanceSqr)
 		{
-			if (nearestPt)
 			dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
 			nearest = ref;

Detour/Source/DetourNavMeshQuery.cpp

@@ -501,7 +501,7 @@ dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const f
 ///
 /// See closestPointOnPolyBoundary() for a limited but faster option.
 ///
-dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest) const
+dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
 {
 	dtAssert(m_nav);
 	const dtMeshTile* tile = 0;
@@ -511,14 +511,6 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
 	if (!tile)
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	closestPointOnPolyInTile(tile, poly, pos, closest);
-	
-	return DT_SUCCESS;
-}
-
-void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
-											  const float* pos, float* closest) const
-{
 	// Off-mesh connections don't have detail polygons.
 	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 	{
@@ -528,7 +520,9 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
 		const float d1 = dtVdist(pos, v1);
 		const float u = d0 / (d0+d1);
 		dtVlerp(closest, v0, v1, u);
-		return;
+		if (posOverPoly)
+			*posOverPoly = false;
+		return DT_SUCCESS;
 	}
 
 	const unsigned int ip = (unsigned int)(poly - tile->polys);
@@ -559,6 +553,14 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
 		const float* va = &verts[imin*3];
 		const float* vb = &verts[((imin+1)%nv)*3];
 		dtVlerp(closest, va, vb, edget[imin]);
+
+		if (posOverPoly)
+			*posOverPoly = false;
+	}
+	else
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
 	}
 
 	// Find height at the location.
@@ -581,29 +583,7 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
 		}
 	}
 	
-/*	float closestDistSqr = FLT_MAX;
+	return DT_SUCCESS;
-	for (int j = 0; j < pd->triCount; ++j)
-	{
-		const unsigned char* t = &tile->detailTris[(pd->triBase+j)*4];
-		const float* v[3];
-		for (int k = 0; k < 3; ++k)
-		{
-			if (t[k] < poly->vertCount)
-				v[k] = &tile->verts[poly->verts[t[k]]*3];
-			else
-				v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
-		}
-
-		float pt[3];
-		dtClosestPtPointTriangle(pt, pos, v[0], v[1], v[2]);
-		float d = dtVdistSqr(pos, pt);
-		
-		if (d < closestDistSqr)
-		{
-			dtVcopy(closest, pt);
-			closestDistSqr = d;
-		}
-	}*/
 }
 
 /// @par
@@ -747,8 +727,27 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
-		closestPointOnPoly(ref, center, closestPtPoly);
+		float diff[3];
-		float d = dtVdistSqr(center, closestPtPoly);
+		bool posOverPoly = false;
+		float d = 0;
+		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
+
+		// If a point is directly over a polygon and closer than
+		// climb height, favor that instead of straight line nearest point.
+		dtVsub(diff, center, closestPtPoly);
+		if (posOverPoly)
+		{
+			const dtMeshTile* tile = 0;
+			const dtPoly* poly = 0;
+			m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly);
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
+		}
+		
 		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
@@ -764,42 +763,6 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	return DT_SUCCESS;
 }
 
-dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const float* center, const float* extents,
-												const dtQueryFilter* filter, float* nearestPt) const
-{
-	dtAssert(m_nav);
-	
-	float bmin[3], bmax[3];
-	dtVsub(bmin, center, extents);
-	dtVadd(bmax, center, extents);
-	
-	// Get nearby polygons from proximity grid.
-	dtPolyRef polys[128];
-	int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);
-	
-	// Find nearest polygon amongst the nearby polygons.
-	dtPolyRef nearest = 0;
-	float nearestDistanceSqr = FLT_MAX;
-	for (int i = 0; i < polyCount; ++i)
-	{
-		dtPolyRef ref = polys[i];
-		const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
-		float closestPtPoly[3];
-		closestPointOnPolyInTile(tile, poly, center, closestPtPoly);
-			
-		float d = dtVdistSqr(center, closestPtPoly);
-		if (d < nearestDistanceSqr)
-		{
-			if (nearestPt)
-				dtVcopy(nearestPt, closestPtPoly);
-			nearestDistanceSqr = d;
-			nearest = ref;
-		}
-	}
-	
-	return nearest;
-}
-
 int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
 										const dtQueryFilter* filter,
 										dtPolyRef* polys, const int maxPolys) const

DetourCrowd/Source/DetourCrowd.cpp

@@ -711,7 +711,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 				if (reqPath[reqPathCount-1] != ag->targetRef)
 				{
 					// Partial path, constrain target position inside the last polygon.
-					status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos);
+					status = m_navquery->closestPointOnPoly(reqPath[reqPathCount-1], ag->targetPos, reqPos, 0);
 					if (dtStatusFailed(status))
 						reqPathCount = 0;
 				}
@@ -855,7 +855,7 @@ void dtCrowd::updateMoveRequest(const float /*dt*/)
 					{
 						// Partial path, constrain target position inside the last polygon.
 						float nearest[3];
-						status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest);
+						status = m_navquery->closestPointOnPoly(res[nres-1], targetPos, nearest, 0);
 						if (dtStatusSucceed(status))
 							dtVcopy(targetPos, nearest);
 						else
@@ -949,7 +949,7 @@ void dtCrowd::checkPathValidity(dtCrowdAgent** agents, const int nagents, const
 			float nearest[3];
 			dtVcopy(nearest, agentPos);
 			agentRef = 0;
-			dtStatus status = m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
+			m_navquery->findNearestPoly(ag->npos, m_ext, &m_filter, &agentRef, nearest);
 			dtVcopy(agentPos, nearest);
 
 			if (!agentRef)

RecastDemo/Bin/Tests/raycast_test.txt

@@ -0,0 +1,6 @@
+s Tile Mesh
+f nav_test.obj
+rc  45.133884 -0.533207 -3.775568  47.078232 7.797605 14.293253  0xffef 0x0
+rc  52.979847 -2.778793 -2.914886  50.628868 -2.350212 13.917850  0xffef 0x0
+rc  45.209217 2.024442 1.838851  46.888412 7.797606 15.772338  0xffef 0x0
+rc  45.388317 -0.562073 -3.673226  46.651001 7.797606 15.513507  0xffef 0x0

RecastDemo/Include/TestCase.h

@@ -26,6 +26,7 @@ class TestCase
 	enum TestType
 	{
 		TEST_PATHFIND,
+		TEST_RAYCAST,
 	};
 	
 	struct Test
@@ -39,6 +40,7 @@ class TestCase
 		
 		TestType type;
 		float spos[3], epos[3];
+		float nspos[3], nepos[3];
 		float radius;
 		int includeFlags, excludeFlags;
 		bool expand;

RecastDemo/Source/NavMeshTesterTool.cpp

@@ -506,8 +506,8 @@ void NavMeshTesterTool::handleToggle()
 		if (m_pathIterPolyCount)
 		{
 			// Iterate over the path to find smooth path on the detail mesh surface.
-			m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos);
+			m_navQuery->closestPointOnPoly(m_startRef, m_spos, m_iterPos, 0);
-			m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos);
+			m_navQuery->closestPointOnPoly(m_pathIterPolys[m_pathIterPolyCount-1], m_epos, m_targetPos, 0);
 			
 			m_nsmoothPath = 0;
 			
@@ -650,7 +650,7 @@ void NavMeshTesterTool::handleUpdate(const float /*dt*/)
 				float epos[3];
 				dtVcopy(epos, m_epos);
 				if (m_polys[m_npolys-1] != m_endRef)
-				m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
+				m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);
 
 				m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
 											 m_straightPath, m_straightPathFlags,
@@ -715,8 +715,8 @@ void NavMeshTesterTool::recalc()
 				int npolys = m_npolys;
 				
 				float iterPos[3], targetPos[3];
-				m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos);
+				m_navQuery->closestPointOnPoly(m_startRef, m_spos, iterPos, 0);
-				m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos);
+				m_navQuery->closestPointOnPoly(polys[npolys-1], m_epos, targetPos, 0);
 				
 				static const float STEP_SIZE = 0.5f;
 				static const float SLOP = 0.01f;
@@ -855,7 +855,7 @@ void NavMeshTesterTool::recalc()
 				float epos[3];
 				dtVcopy(epos, m_epos);
 				if (m_polys[m_npolys-1] != m_endRef)
-					m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos);
+					m_navQuery->closestPointOnPoly(m_polys[m_npolys-1], m_epos, epos, 0);
 				
 				m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
 											 m_straightPath, m_straightPathFlags,

RecastDemo/Source/TestCase.cpp

@@ -141,6 +141,20 @@ bool TestCase::load(const char* filePath)
 				   &test->epos[0], &test->epos[1], &test->epos[2],
 				   &test->includeFlags, &test->excludeFlags);
 		}
+		else if (row[0] == 'r' && row[1] == 'c')
+		{
+			// Pathfind test.
+			Test* test = new Test;
+			memset(test, 0, sizeof(Test));
+			test->type = TEST_RAYCAST;
+			test->expand = false;
+			test->next = m_tests;
+			m_tests = test;
+			sscanf(row+2, "%f %f %f %f %f %f %x %x",
+				   &test->spos[0], &test->spos[1], &test->spos[2],
+				   &test->epos[0], &test->epos[1], &test->epos[2],
+				   &test->includeFlags, &test->excludeFlags);
+		}
 	}
 	
 	delete [] buf;
@@ -187,8 +201,8 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
 		TimeVal findNearestPolyStart = getPerfTime();
 		
 		dtPolyRef startRef, endRef;
-		navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, 0);
+		navquery->findNearestPoly(iter->spos, polyPickExt, &filter, &startRef, iter->nspos);
-		navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, 0);
+		navquery->findNearestPoly(iter->epos, polyPickExt, &filter, &endRef, iter->nepos);
 
 		TimeVal findNearestPolyEnd = getPerfTime();
 		iter->findNearestPolyTime += getPerfDeltaTimeUsec(findNearestPolyStart, findNearestPolyEnd);
@@ -196,6 +210,8 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
 		if (!startRef || ! endRef)
 			continue;
 	
+		if (iter->type == TEST_PATHFIND)
+		{
 			// Find path
 			TimeVal findPathStart = getPerfTime();
 
@@ -227,6 +243,51 @@ void TestCase::doTests(dtNavMesh* navmesh, dtNavMeshQuery* navquery)
 				memcpy(iter->straight, straight, sizeof(float)*3*iter->nstraight);
 			}
 		}
+		else if (iter->type == TEST_RAYCAST)
+		{
+			float t = 0;
+			float hitNormal[3], hitPos[3];
+			
+			iter->straight = new float[2*3];
+			iter->nstraight = 2;
+			
+			iter->straight[0] = iter->spos[0];
+			iter->straight[1] = iter->spos[1];
+			iter->straight[2] = iter->spos[2];
+			
+			TimeVal findPathStart = getPerfTime();
+			
+			navquery->raycast(startRef, iter->spos, iter->epos, &filter, &t, hitNormal, polys, &iter->npolys, MAX_POLYS);
+
+			TimeVal findPathEnd = getPerfTime();
+			iter->findPathTime += getPerfDeltaTimeUsec(findPathStart, findPathEnd);
+
+			if (t > 1)
+			{
+				// No hit
+				dtVcopy(hitPos, iter->epos);
+			}
+			else
+			{
+				// Hit
+				dtVlerp(hitPos, iter->spos, iter->epos, t);
+			}
+			// Adjust height.
+			if (iter->npolys > 0)
+			{
+				float h = 0;
+				navquery->getPolyHeight(polys[iter->npolys-1], hitPos, &h);
+				hitPos[1] = h;
+			}
+			dtVcopy(&iter->straight[3], hitPos);
+
+			if (iter->npolys)
+			{
+				iter->polys = new dtPolyRef[iter->npolys];
+				memcpy(iter->polys, polys, sizeof(dtPolyRef)*iter->npolys);
+			}
+		}
+	}
 
 
 	printf("Test Results:\n");
@@ -260,6 +321,32 @@ void TestCase::handleRender()
 		glVertex3f(iter->epos[0],iter->epos[1]-0.3f,iter->epos[2]);
 		glVertex3f(iter->epos[0],iter->epos[1]+0.3f,iter->epos[2]);
 
+		if (iter->expand)
+		{
+			const float s = 0.1f;
+			glColor4ub(255,32,0,128);
+			glVertex3f(iter->spos[0]-s,iter->spos[1],iter->spos[2]);
+			glVertex3f(iter->spos[0]+s,iter->spos[1],iter->spos[2]);
+			glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]-s);
+			glVertex3f(iter->spos[0],iter->spos[1],iter->spos[2]+s);
+			glColor4ub(255,192,0,255);
+			glVertex3f(iter->nspos[0]-s,iter->nspos[1],iter->nspos[2]);
+			glVertex3f(iter->nspos[0]+s,iter->nspos[1],iter->nspos[2]);
+			glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]-s);
+			glVertex3f(iter->nspos[0],iter->nspos[1],iter->nspos[2]+s);
+			
+			glColor4ub(255,32,0,128);
+			glVertex3f(iter->epos[0]-s,iter->epos[1],iter->epos[2]);
+			glVertex3f(iter->epos[0]+s,iter->epos[1],iter->epos[2]);
+			glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]-s);
+			glVertex3f(iter->epos[0],iter->epos[1],iter->epos[2]+s);
+			glColor4ub(255,192,0,255);
+			glVertex3f(iter->nepos[0]-s,iter->nepos[1],iter->nepos[2]);
+			glVertex3f(iter->nepos[0]+s,iter->nepos[1],iter->nepos[2]);
+			glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]-s);
+			glVertex3f(iter->nepos[0],iter->nepos[1],iter->nepos[2]+s);
+		}
+		
 		if (iter->expand)
 			glColor4ub(255,192,0,255);
 		else

RecastDemo/Source/main.cpp

@@ -817,7 +817,6 @@ int main(int /*argc*/, char** /*argv*/)
 					{
 						delete geom;
 						geom = 0;
-						
 						showLog = true;
 						logScroll = 0;
 						ctx.dumpLog("Geom load log %s:", meshName);
@@ -827,6 +826,10 @@ int main(int /*argc*/, char** /*argv*/)
 						sample->handleMeshChanged(geom);
 					}
 
+					// This will ensure that tile & poly bits are updated in tiled sample.
+					if (sample)
+						sample->handleSettings();
+
 					ctx.resetLog();
 					if (sample && !sample->handleBuild())
 					{
@@ -860,7 +863,7 @@ int main(int /*argc*/, char** /*argv*/)
 						}
 						rx = 45;
 						ry = -45;
-						glFogf(GL_FOG_START, camr*0.1f);
+						glFogf(GL_FOG_START, camr*0.2f);
 						glFogf(GL_FOG_END, camr*1.25f);
 					}