bugfix: raycasts sometimes fail when the origin is near a boundary of a triangle with a slope. The fix of considers polygons to have a thickness for the purpose of finding closest nodes.

Detour/Include/DetourNavMesh.h

@@ -94,6 +94,10 @@ static const unsigned int DT_OFFMESH_CON_BIDIR = 1;
 /// @ingroup detour
 static const int DT_MAX_AREAS = 64;
 
+/// Polygon thickness relative to agent height (for correct scaling)
+/// We define the polygon to have a slight thickness so that things slightly above and below will be considered on the polygon.
+static const float SCALE_POLY_THICK = 1.f / 6.f;
+
 /// Tile flags used for various functions and fields.
 /// For an example, see dtNavMesh::addTile().
 enum dtTileFlags
@@ -612,8 +616,10 @@ private:
 	/// Find nearest polygon within a tile.
 	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,
+	/// find the closest point on polygon.
+	/// @return whether the point is straight above the closest polygon
+	bool closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
 								  const float* pos, float* closest) const;
 	
 	dtNavMeshParams m_params;			///< Current initialization params. TODO: do not store this info twice.

Detour/Include/DetourNavMeshQuery.h

@@ -378,8 +378,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]	inside2D		true if the closest point is above/below 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* inside2D=0) const;
 	
 	/// Returns a point on the boundary closest to the source point if the source point is outside the 
 	/// polygon's xz-bounds.
@@ -431,8 +432,9 @@ private:
 	/// 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.
+	/// finds the closest point on a polygon.
-	void closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest) const;
+	/// returns whether the point is inside the polygon (in 2D)
+	bool 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,7 +617,7 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
 	}
 }
 
-void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
+bool dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip,
 										 const float* pos, float* closest) const
 {
 	const dtPoly* poly = &tile->polys[ip];
@@ -630,7 +630,7 @@ 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);
-		return;
+		return false;
 	}
 	
 	const dtPolyDetail* pd = &tile->detailMeshes[ip];
@@ -644,7 +644,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
 		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
 	
 	dtVcopy(closest, pos);
-	if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
+	bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
+	if (!inside)
 	{
 		// Point is outside the polygon, dtClamp to nearest edge.
 		float dmin = FLT_MAX;
@@ -681,6 +682,8 @@ void dtNavMesh::closestPointOnPolyInTile(const dtMeshTile* tile, unsigned int ip
 			break;
 		}
 	}
+
+	return inside;
 }
 
 dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
@@ -694,6 +697,9 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 	// Get nearby polygons from proximity grid.
 	dtPolyRef polys[128];
 	int polyCount = queryPolygonsInTile(tile, bmin, bmax, polys, 128);
+
+	// tile thickness is scaled relative to an agent height
+	const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK;
 	
 	// Find nearest polygon amongst the nearby polygons.
 	dtPolyRef nearest = 0;
@@ -701,15 +707,31 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 	for (int i = 0; i < polyCount; ++i)
 	{
 		dtPolyRef ref = polys[i];
-		float closestPtPoly[3];
+		float d, closestPtPoly[3], offset[3];
-		closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly);
+		bool inside2D = closestPointOnPolyInTile(tile, decodePolyIdPoly(ref), center, closestPtPoly);
-		float d = dtVdistSqr(center, closestPtPoly);
+
+		dtVsub(offset, center, closestPtPoly);
+
+		if (inside2D)
+		{
+			// right above the polygon is considered on the polygon
+			d = dtAbs(offset[1]) - polyThick;
+			d = d > 0 ? d*d : 0;
+		}
+		else
+			d = dtVlenSqr(offset);
+
 		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
 				dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
 			nearest = ref;
+			if (d == 0)
+			{
+				nearestDistanceSqr = 0;
+				break;
+			}
 		}
 	}
 	

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* inside2D) const
 {
 	dtAssert(m_nav);
 	const dtMeshTile* tile = 0;
@@ -511,12 +511,14 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
 	if (!tile)
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	closestPointOnPolyInTile(tile, poly, pos, closest);
+	bool in2D = closestPointOnPolyInTile(tile, poly, pos, closest);
+	if (inside2D)
+		*inside2D = in2D;
 	
 	return DT_SUCCESS;
 }
 
-void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
+bool dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
 											  const float* pos, float* closest) const
 {
 	// Off-mesh connections don't have detail polygons.
@@ -528,7 +530,7 @@ 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;
+		return false;
 	}
 
 	const unsigned int ip = (unsigned int)(poly - tile->polys);
@@ -543,7 +545,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
 		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
 	
 	dtVcopy(closest, pos);
-	if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
+	bool inside = dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget);
+	if (!inside)
 	{
 		// Point is outside the polygon, dtClamp to nearest edge.
 		float dmin = FLT_MAX;
@@ -581,6 +584,8 @@ void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPo
 		}
 	}
 
+	return inside;
+
 /*	float closestDistSqr = FLT_MAX;
 	for (int j = 0; j < pd->triCount; ++j)
 	{
@@ -739,6 +744,15 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	int polyCount = 0;
 	if (dtStatusFailed(queryPolygons(center, extents, filter, polys, &polyCount, 128)))
 		return DT_FAILURE | DT_INVALID_PARAM;
+
+	if (polyCount == 0)
+		return DT_SUCCESS;
+
+	// tile thickness is scaled relative to an agent height
+	const dtMeshTile* tile0;
+	const dtPoly* poly0;
+	m_nav->getTileAndPolyByRefUnsafe(polys[0], &tile0, &poly0);
+	const float polyThick = tile0->header->walkableHeight * SCALE_POLY_THICK;
 	
 	// Find nearest polygon amongst the nearby polygons.
 	dtPolyRef nearest = 0;
@@ -746,15 +760,32 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	for (int i = 0; i < polyCount; ++i)
 	{
 		dtPolyRef ref = polys[i];
-		float closestPtPoly[3];
+		float d, closestPtPoly[3], offset[3];
-		closestPointOnPoly(ref, center, closestPtPoly);
+		bool inside2d;
-		float d = dtVdistSqr(center, closestPtPoly);
+		closestPointOnPoly(ref, center, closestPtPoly, &inside2d);
+		
+		dtVsub(offset, center, closestPtPoly);
+
+		if (inside2d)
+		{
+			// right above the polygon is considered on the polygon
+			d = dtAbs(offset[1]) - polyThick;
+			d = d > 0 ? d*d : 0;
+		}
+		else
+			d = dtVlenSqr(offset);
+
 		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
 				dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
 			nearest = ref;
+			if (d == 0)
+			{
+				nearestDistanceSqr = 0;
+				break;
+			}
 		}
 	}
 	
@@ -768,7 +799,7 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl
 												const dtQueryFilter* filter, float* nearestPt) const
 {
 	dtAssert(m_nav);
-	
+
 	float bmin[3], bmax[3];
 	dtVsub(bmin, center, extents);
 	dtVadd(bmax, center, extents);
@@ -777,6 +808,9 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl
 	dtPolyRef polys[128];
 	int polyCount = queryPolygonsInTile(tile, bmin, bmax, filter, polys, 128);
 	
+	// tile thickness is scaled relative to an agent height
+	const float polyThick = tile->header->walkableHeight * SCALE_POLY_THICK;
+
 	// Find nearest polygon amongst the nearby polygons.
 	dtPolyRef nearest = 0;
 	float nearestDistanceSqr = FLT_MAX;
@@ -784,16 +818,31 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl
 	{
 		dtPolyRef ref = polys[i];
 		const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
-		float closestPtPoly[3];
+		float d, closestPtPoly[3], offset[3];
-		closestPointOnPolyInTile(tile, poly, center, closestPtPoly);
+		bool inside2d = closestPointOnPolyInTile(tile, poly, center, closestPtPoly);
-			
+		
-		float d = dtVdistSqr(center, closestPtPoly);
+		dtVsub(offset, center, closestPtPoly);
+
+		if (inside2d)
+		{
+			// right above the polygon is considered on the polygon
+			d = dtAbs(offset[1]) - polyThick;
+			d = d > 0 ? d*d : 0;
+		}
+		else
+			d = dtVlenSqr(offset);
+
 		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
 				dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
 			nearest = ref;
+			if (d == 0)
+			{
+				nearestDistanceSqr = 0;
+				break;
+			}
 		}
 	}