1 files changed, 112 insertions, 50 deletions

diff --git a/deps/recastnavigation/Detour/Source/DetourNavMesh.cpp b/deps/recastnavigation/Detour/Source/DetourNavMesh.cpp
index b81a2567b2..a655d1d89a 100644
--- a/deps/recastnavigation/Detour/Source/DetourNavMesh.cpp
+++ b/deps/recastnavigation/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;
-	const dtPoly* poly = 0;
-	getTileAndPolyByRefUnsafe(ref, &tile, &poly);
-	
-	// Off-mesh connections don't have detail polygons.
-	if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
+	template<bool onlyBoundary>
+	void closestPointOnDetailEdges(const dtMeshTile* tile, const dtPoly* poly, const float* pos, float* closest)
 	{
-		const float* v0 = &tile->verts[poly->verts[0]*3];
-		const float* v1 = &tile->verts[poly->verts[1]*3];
-		const float d0 = dtVdist(pos, v0);
-		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];
+
+		float dmin = FLT_MAX;
+		float tmin = 0;
+		const float* pmin = 0;
+		const float* pmax = 0;
+
+		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 (!dtDistancePtPolyEdgesSqr(pos, verts, nv, edged, edget))
-	{
-		// Point is outside the polygon, dtClamp to nearest edge.
-		float dmin = edged[0];
-		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;
-	}
+	if (!dtPointInPolygon(pos, verts, nv))
+		return false;
+
+	if (!height)
+		return 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;
-			break;
+			*height = h;
+			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,
@@ -855,7 +917,7 @@ dtStatus dtNavMesh::addTile(unsigned char* data, int dataSize, int flags,
 		
 	// Make sure the location is free.
 	if (getTileAt(header->x, header->y, header->layer))
-		return DT_FAILURE;
+		return DT_FAILURE | DT_ALREADY_OCCUPIED;
 		
 	// Allocate a tile.
 	dtMeshTile* tile = 0;