Make closestPointOnPoly and getPolyHeight more robust

* Make dtClosestHeightPointTriangle use no epsilon (except for checking
denominator)
* When point is outside poly, directly find closest height from boundary
detail edges instead of first interpolating and then finding height from
detail tris.
* When point is inside poly, if all dtClosestHeightPointTriangle queries
failed, then find height from detail edges. This should only happen if
the point is right on top of an internal detail tri edge.

Detour/Include/DetourNavMesh.h

@@ -651,6 +651,8 @@ private:
 	/// Find nearest polygon within a tile.
 	dtPolyRef findNearestPolyInTile(const dtMeshTile* tile, const float* center,
 									const float* halfExtents, float* nearestPt) const;
+	/// Returns whether position is over the poly and the height at the position if so.
+	bool getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const;
 	/// Returns closest point on polygon.
 	void closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const;
 	

Detour/Source/DetourCommon.cpp

@@ -203,14 +203,18 @@ void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const floa
 
 bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h)
 {
+	const float EPS = 1e-6f;
 	float v0[3], v1[3], v2[3];
 
-	dtVsub(v0, c,a);
+	dtVsub(v0, c, a);
-	dtVsub(v1, b,a);
+	dtVsub(v1, b, a);
-	dtVsub(v2, p,a);
+	dtVsub(v2, p, a);
 
 	// Compute scaled barycentric coordinates
 	float denom = v0[0] * v1[2] - v0[2] * v1[0];
+	if (fabsf(denom) < EPS)
+		return false;
+
 	float u = v1[2] * v2[0] - v1[0] * v2[2];
 	float v = v0[0] * v2[2] - v0[2] * v2[0];
 
@@ -220,13 +224,9 @@ bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b
 		v = -v;
 	}
 
-	// The (sloppy) epsilon is needed to allow to get height of points which
-	// are interpolated along the edges of the triangles.
-	float epsilon = - 1e-4f * denom;
-
 	// If point lies inside the triangle, return interpolated ycoord.
-	if (u >= epsilon && v >= epsilon && (u+v) <= denom - epsilon) {
+	if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) {
-		h = a[1] + (v0[1]*u + v1[1]*v) / denom;
+		h = a[1] + (v0[1] * u + v1[1] * v) / denom;
 		return true;
 	}
 	return false;

Detour/Source/DetourNavMesh.cpp

@@ -616,63 +616,84 @@ void dtNavMesh::baseOffMeshLinks(dtMeshTile* tile)
 	}
 }
 
-void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
+namespace
 {
-	const dtMeshTile* tile = 0;
+	template<bool onlyBoundary>
-	const dtPoly* poly = 0;
+	void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest)
-	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
-	
-	// Off-mesh connections don't have detail polygons.
-	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 	{
-		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const unsigned int ip = (unsigned int)(poly - tile->polys);
-		const float* v1 = &tile->verts[poly->verts[1]*3];
+		const dtPolyDetail* pd = &tile->detailMeshes[ip];
-		const float d0 = dtVdist(pos, v0);
+
-		const float d1 = dtVdist(pos, v1);
+		float dmin = FLT_MAX;
-		const float u = d0 / (d0+d1);
+		float tmin = 0;
-		dtVlerp(closest, v0, v1, u);
+		const float* pmin = 0;
-		if (posOverPoly)
+		const float* pmax = 0;
-			*posOverPoly = false;
+
-		return;
+		for (int i = 0; i < pd->triCount; i++)
+		{
+			const unsigned char* tris = &tile->detailTris[(pd->triBase + i) * 4];
+			const int ANY_BOUNDARY_EDGE =
+				(DT_DETAIL_EDGE_BOUNDARY << 0) |
+				(DT_DETAIL_EDGE_BOUNDARY << 2) |
+				(DT_DETAIL_EDGE_BOUNDARY << 4);
+			if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0)
+				continue;
+
+			const float* v[3];
+			for (int j = 0; j < 3; ++j)
+			{
+				if (tris[j] < poly->vertCount)
+					v[j] = &tile->verts[poly->verts[tris[j]] * 3];
+				else
+					v[j] = &tile->detailVerts[(pd->vertBase + (tris[j] - poly->vertCount)) * 3];
+			}
+
+			for (int k = 0, j = 2; k < 3; j = k++)
+			{
+				if ((dtGetDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0 &&
+					(onlyBoundary || tris[j] < tris[k]))
+				{
+					// Only looking at boundary edges and this is internal, or
+					// this is an inner edge that we will see again or have already seen.
+					continue;
+				}
+
+				float t;
+				float d = dtDistancePtSegSqr2D(pos, v[j], v[k], t);
+				if (d < dmin)
+				{
+					dmin = d;
+					tmin = t;
+					pmin = v[j];
+					pmax = v[k];
+				}
+			}
+		}
+
+		dtVlerp(closest, pmin, pmax, tmin);
 	}
-	
+}
+
+bool dtNavMesh::getPolyHeight(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* height) const
+{
+	// Off-mesh connections do not have detail polys and getting height
+	// over them does not make sense.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+		return false;
+
 	const unsigned int ip = (unsigned int)(poly - tile->polys);
 	const dtPolyDetail* pd = &tile->detailMeshes[ip];
 	
-	// Clamp point to be inside the polygon.
 	float verts[DT_VERTS_PER_POLYGON*3];	
-	float edged[DT_VERTS_PER_POLYGON];
-	float edget[DT_VERTS_PER_POLYGON];
 	const int nv = poly->vertCount;
 	for (int i = 0; i < nv; ++i)
 		dtVcopy(&verts[i*3], &tile->verts[poly->verts[i]*3]);
 	
-	dtVcopy(closest, pos);
+	if (!dtPointInPolygon(pos, verts, nv))
-	if (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
+		return false;
-	{
+
-		// Point is outside the polygon, dtClamp to nearest edge.
+	if (!height)
-		float dmin = edged[0];
+		return true;
-		int imin = 0;
-		for (int i = 1; i < nv; ++i)
-		{
-			if (edged[i] < dmin)
-			{
-				dmin = edged[i];
-				imin = i;
-			}
-		}
-		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.
 	for (int j = 0; j < pd->triCount; ++j)
@@ -687,12 +708,53 @@ void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* close
 				v[k] = &tile->detailVerts[(pd->vertBase+(t[k]-poly->vertCount))*3];
 		}
 		float h;
-		if (dtClosestHeightPointTriangle(closest, v[0], v[1], v[2], h))
+		if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
 		{
-			closest[1] = h;
+			*height = h;
-			break;
+			return true;
 		}
 	}
+
+	// If all triangle checks failed above (can happen with degenerate triangles
+	// or larger floating point values) the point is on an edge, so just select
+	// closest. This should almost never happen so the extra iteration here is
+	// ok.
+	float closest[3];
+	closestPointOnDetailEdges<false>(tile, poly, pos, closest);
+	*height = closest[1];
+	return true;
+}
+
+void dtNavMesh::closestPointOnPoly(dtPolyRef ref, const float* pos, float* closest, bool* posOverPoly) const
+{
+	const dtMeshTile* tile = 0;
+	const dtPoly* poly = 0;
+	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
+
+	dtVcopy(closest, pos);
+	if (getPolyHeight(tile, poly, pos, &closest[1]))
+	{
+		if (posOverPoly)
+			*posOverPoly = true;
+		return;
+	}
+
+	if (posOverPoly)
+		*posOverPoly = false;
+
+	// Off-mesh connections don't have detail polygons.
+	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	{
+		const float* v0 = &tile->verts[poly->verts[0]*3];
+		const float* v1 = &tile->verts[poly->verts[1]*3];
+		float t;
+		dtDistancePtSegSqr2D(pos, v0, v1, t);
+		dtVlerp(closest, v0, v1, t);
+		return;
+	}
+
+	// Outside poly that is not an offmesh connection.
+	closestPointOnDetailEdges<true>(tile, poly, pos, closest);
 }
 
 dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,

Detour/Source/DetourNavMeshQuery.cpp

@@ -588,7 +588,7 @@ dtStatus dtNavMeshQuery::closestPointOnPolyBoundary(dtPolyRef ref, const float*
 
 /// @par
 ///
-/// Will return #DT_FAILURE if the provided position is outside the xz-bounds 
+/// Will return #DT_FAILURE | DT_INVALID_PARAM if the provided position is outside the xz-bounds 
 /// of the polygon.
 /// 
 dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* height) const
@@ -602,44 +602,25 @@ dtStatus dtNavMeshQuery::getPolyHeight(dtPolyRef ref, const float* pos, float* h
 
 	if (!pos || !dtVisfinite2D(pos))
 		return DT_FAILURE | DT_INVALID_PARAM;
-	
+
+	// We used to return success for offmesh connections, but the
+	// getPolyHeight in DetourNavMesh does not do this, so special
+	// case it here.
 	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
 	{
 		const float* v0 = &tile->verts[poly->verts[0]*3];
 		const float* v1 = &tile->verts[poly->verts[1]*3];
-		const float d0 = dtVdist2D(pos, v0);
+		float t;
-		const float d1 = dtVdist2D(pos, v1);
+		dtDistancePtSegSqr2D(pos, v0, v1, t);
-		const float u = d0 / (d0+d1);
 		if (height)
-			*height = v0[1] + (v1[1] - v0[1]) * u;
+			*height = v0[1] + (v1[1] - v0[1])*t;
+
 		return DT_SUCCESS;
 	}
-	else
+
-	{
+	return m_nav->getPolyHeight(tile, poly, pos, height)
-		const unsigned int ip = (unsigned int)(poly - tile->polys);
+		? DT_SUCCESS
-		const dtPolyDetail* pd = &tile->detailMeshes[ip];
+		: DT_FAILURE | DT_INVALID_PARAM;
-		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 h;
-			if (dtClosestHeightPointTriangle(pos, v[0], v[1], v[2], h))
-			{
-				if (height)
-					*height = h;
-				return DT_SUCCESS;
-			}
-		}
-	}
-	
-	return DT_FAILURE | DT_INVALID_PARAM;
 }
 
 class dtFindNearestPolyQuery : public dtPolyQuery