diff --git a/Detour/Source/DetourNavMesh.cpp b/Detour/Source/DetourNavMesh.cpp
index 1063b10..9d627be 100644
--- a/Detour/Source/DetourNavMesh.cpp
+++ b/Detour/Source/DetourNavMesh.cpp
@@ -711,13 +711,13 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 	// Find nearest polygon amongst the nearby polygons.
 	dtPolyRef nearest = 0;
 	float nearestDistanceSqr = FLT_MAX;
-	bool nearestOverPoly = false;
 	for (int i = 0; i < polyCount; ++i)
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
 		float diff[3];
 		bool posOverPoly = false;
+		float d = 0;
 		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
 
 		// If a point is directly over a polygon and closer than
@@ -725,18 +725,18 @@ dtPolyRef dtNavMesh::findNearestPolyInTile(const dtMeshTile* tile,
 		dtVsub(diff, center, closestPtPoly);
 		if (posOverPoly)
 		{
-			const dtMeshTile* tile = 0;
-			if (dtAbs(diff[1]) > tile->header->walkableClimb)
-				posOverPoly = false;
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
 		}
 		
-		float d = dtVlenSqr(diff);
-		
-		if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly))
+		if (d < nearestDistanceSqr)
 		{
 			dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
-			nearestOverPoly = posOverPoly;
 			nearest = ref;
 		}
 	}
diff --git a/Detour/Source/DetourNavMeshQuery.cpp b/Detour/Source/DetourNavMeshQuery.cpp
index ba67957..cc16946 100644
--- a/Detour/Source/DetourNavMeshQuery.cpp
+++ b/Detour/Source/DetourNavMeshQuery.cpp
@@ -723,13 +723,13 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	// Find nearest polygon amongst the nearby polygons.
 	dtPolyRef nearest = 0;
 	float nearestDistanceSqr = FLT_MAX;
-	bool nearestOverPoly = false;
 	for (int i = 0; i < polyCount; ++i)
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
 		float diff[3];
 		bool posOverPoly = false;
+		float d = 0;
 		closestPointOnPoly(ref, center, closestPtPoly, &posOverPoly);
 
 		// If a point is directly over a polygon and closer than
@@ -740,18 +740,19 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 			const dtMeshTile* tile = 0;
 			const dtPoly* poly = 0;
 			m_nav->getTileAndPolyByRefUnsafe(polys[i], &tile, &poly);
-			if (dtAbs(diff[1]) > tile->header->walkableClimb)
-				posOverPoly = false;
+			d = dtAbs(diff[1]) - tile->header->walkableClimb;
+			d = d > 0 ? d*d : 0;			
+		}
+		else
+		{
+			d = dtVlenSqr(diff);
 		}
 		
-		float d = dtVlenSqr(diff);
-		
-		if (d < nearestDistanceSqr || (!nearestOverPoly && posOverPoly))
+		if (d < nearestDistanceSqr)
 		{
 			if (nearestPt)
 				dtVcopy(nearestPt, closestPtPoly);
 			nearestDistanceSqr = d;
-			nearestOverPoly = posOverPoly;
 			nearest = ref;
 		}
 	}
diff --git a/RecastDemo/Bin/Tests/raycast_test.txt b/RecastDemo/Bin/Tests/raycast_test.txt
index a410aea..3fde0ee 100644
--- a/RecastDemo/Bin/Tests/raycast_test.txt
+++ b/RecastDemo/Bin/Tests/raycast_test.txt
@@ -3,3 +3,4 @@ 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
\ No newline at end of file