Core/MMAPS: Fix invisible wall issue in MMAPS.

Fix invisible wall issues in MMAPS caused by tile connections not being extracted correctly.
This reverts aa645683b8b25bfb35cb977678daf5c56c1531e6 and restores an old version of recastnavigation, most likely https://github.com/memononen/recastnavigation/commit/1be9de72d87dcfdecf6bea30f5461745f7d7afe6 .

Re-extracting MMAPS is required.

1 files changed, 242 insertions, 777 deletions

diff --git a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
index e6557cf707e..ec0c30460a9 100644
--- a/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
+++ b/dep/recastnavigation/Detour/DetourNavMeshQuery.cpp
@@ -81,7 +81,7 @@ float dtQueryFilter::getCost(const float* pa, const float* pb,
 							 const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
 							 const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const
 {
-	return dtVdist(pa, pb) * m_areaCost[curPoly->getArea()];
+	return dtVdist(pa, pb) * m_areaCost[curPoly->area];
 }
 #else
 inline bool dtQueryFilter::passFilter(const dtPolyRef /*ref*/,
@@ -217,281 +217,6 @@ dtStatus dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes)
 	return DT_SUCCESS;
 }
 
-dtStatus dtNavMeshQuery::findRandomPoint(const dtQueryFilter* filter, float (*frand)(),
-										 dtPolyRef* randomRef, float* randomPt) const
-{
-	dtAssert(m_nav);
-	
-	// Randomly pick one tile. Assume that all tiles cover roughly the same area.
-	const dtMeshTile* tile = 0;
-	float tsum = 0.0f;
-	for (int i = 0; i < m_nav->getMaxTiles(); i++)
-	{
-		const dtMeshTile* t = m_nav->getTile(i);
-		if (!t || !t->header) continue;
-		
-		// Choose random tile using reservoi sampling.
-		const float area = 1.0f; // Could be tile area too.
-		tsum += area;
-		const float u = frand();
-		if (u*tsum <= area)
-			tile = t;
-	}
-	if (!tile)
-		return DT_FAILURE;
-
-	// Randomly pick one polygon weighted by polygon area.
-	const dtPoly* poly = 0;
-	dtPolyRef polyRef = 0;
-	const dtPolyRef base = m_nav->getPolyRefBase(tile);
-
-	float areaSum = 0.0f;
-	for (int i = 0; i < tile->header->polyCount; ++i)
-	{
-		const dtPoly* p = &tile->polys[i];
-		// Do not return off-mesh connection polygons.
-		if (p->getType() != DT_POLYTYPE_GROUND)
-			continue;
-		// Must pass filter
-		const dtPolyRef ref = base | (dtPolyRef)i;
-		if (!filter->passFilter(ref, tile, p))
-			continue;
-
-		// Calc area of the polygon.
-		float polyArea = 0.0f;
-		for (int j = 2; j < p->vertCount; ++j)
-		{
-			const float* va = &tile->verts[p->verts[0]*3];
-			const float* vb = &tile->verts[p->verts[j-1]*3];
-			const float* vc = &tile->verts[p->verts[j]*3];
-			polyArea += dtTriArea2D(va,vb,vc);
-		}
-
-		// Choose random polygon weighted by area, using reservoi sampling.
-		areaSum += polyArea;
-		const float u = frand();
-		if (u*areaSum <= polyArea)
-		{
-			poly = p;
-			polyRef = ref;
-		}
-	}
-	
-	if (!poly)
-		return DT_FAILURE;
-
-	// Randomly pick point on polygon.
-	const float* v = &tile->verts[poly->verts[0]*3];
-	float verts[3*DT_VERTS_PER_POLYGON];
-	float areas[DT_VERTS_PER_POLYGON];
-	dtVcopy(&verts[0*3],v);
-	for (int j = 1; j < poly->vertCount; ++j)
-	{
-		v = &tile->verts[poly->verts[j]*3];
-		dtVcopy(&verts[j*3],v);
-	}
-	
-	const float s = frand();
-	const float t = frand();
-	
-	float pt[3];
-	dtRandomPointInConvexPoly(verts, poly->vertCount, areas, s, t, pt);
-	
-	float h = 0.0f;
-	dtStatus status = getPolyHeight(polyRef, pt, &h);
-	if (dtStatusFailed(status))
-		return status;
-	pt[1] = h;
-	
-	dtVcopy(randomPt, pt);
-	*randomRef = polyRef;
-
-	return DT_SUCCESS;
-}
-
-dtStatus dtNavMeshQuery::findRandomPointAroundCircle(dtPolyRef startRef, const float* centerPos, const float radius,
-													 const dtQueryFilter* filter, float (*frand)(),
-													 dtPolyRef* randomRef, float* randomPt) const
-{
-	dtAssert(m_nav);
-	dtAssert(m_nodePool);
-	dtAssert(m_openList);
-	
-	// Validate input
-	if (!startRef || !m_nav->isValidPolyRef(startRef))
-		return DT_FAILURE | DT_INVALID_PARAM;
-	
-	const dtMeshTile* startTile = 0;
-	const dtPoly* startPoly = 0;
-	m_nav->getTileAndPolyByRefUnsafe(startRef, &startTile, &startPoly);
-	if (!filter->passFilter(startRef, startTile, startPoly))
-		return DT_FAILURE | DT_INVALID_PARAM;
-	
-	m_nodePool->clear();
-	m_openList->clear();
-	
-	dtNode* startNode = m_nodePool->getNode(startRef);
-	dtVcopy(startNode->pos, centerPos);
-	startNode->pidx = 0;
-	startNode->cost = 0;
-	startNode->total = 0;
-	startNode->id = startRef;
-	startNode->flags = DT_NODE_OPEN;
-	m_openList->push(startNode);
-	
-	dtStatus status = DT_SUCCESS;
-	
-	const float radiusSqr = dtSqr(radius);
-	float areaSum = 0.0f;
-
-	const dtMeshTile* randomTile = 0;
-	const dtPoly* randomPoly = 0;
-	dtPolyRef randomPolyRef = 0;
-
-	while (!m_openList->empty())
-	{
-		dtNode* bestNode = m_openList->pop();
-		bestNode->flags &= ~DT_NODE_OPEN;
-		bestNode->flags |= DT_NODE_CLOSED;
-		
-		// Get poly and tile.
-		// The API input has been cheked already, skip checking internal data.
-		const dtPolyRef bestRef = bestNode->id;
-		const dtMeshTile* bestTile = 0;
-		const dtPoly* bestPoly = 0;
-		m_nav->getTileAndPolyByRefUnsafe(bestRef, &bestTile, &bestPoly);
-
-		// Place random locations on on ground.
-		if (bestPoly->getType() == DT_POLYTYPE_GROUND)
-		{
-			// Calc area of the polygon.
-			float polyArea = 0.0f;
-			for (int j = 2; j < bestPoly->vertCount; ++j)
-			{
-				const float* va = &bestTile->verts[bestPoly->verts[0]*3];
-				const float* vb = &bestTile->verts[bestPoly->verts[j-1]*3];
-				const float* vc = &bestTile->verts[bestPoly->verts[j]*3];
-				polyArea += dtTriArea2D(va,vb,vc);
-			}
-			// Choose random polygon weighted by area, using reservoi sampling.
-			areaSum += polyArea;
-			const float u = frand();
-			if (u*areaSum <= polyArea)
-			{
-				randomTile = bestTile;
-				randomPoly = bestPoly;
-				randomPolyRef = bestRef;
-			}
-		}
-		
-		
-		// Get parent poly and tile.
-		dtPolyRef parentRef = 0;
-		const dtMeshTile* parentTile = 0;
-		const dtPoly* parentPoly = 0;
-		if (bestNode->pidx)
-			parentRef = m_nodePool->getNodeAtIdx(bestNode->pidx)->id;
-		if (parentRef)
-			m_nav->getTileAndPolyByRefUnsafe(parentRef, &parentTile, &parentPoly);
-		
-		for (unsigned int i = bestPoly->firstLink; i != DT_NULL_LINK; i = bestTile->links[i].next)
-		{
-			const dtLink* link = &bestTile->links[i];
-			dtPolyRef neighbourRef = link->ref;
-			// Skip invalid neighbours and do not follow back to parent.
-			if (!neighbourRef || neighbourRef == parentRef)
-				continue;
-			
-			// Expand to neighbour
-			const dtMeshTile* neighbourTile = 0;
-			const dtPoly* neighbourPoly = 0;
-			m_nav->getTileAndPolyByRefUnsafe(neighbourRef, &neighbourTile, &neighbourPoly);
-			
-			// Do not advance if the polygon is excluded by the filter.
-			if (!filter->passFilter(neighbourRef, neighbourTile, neighbourPoly))
-				continue;
-			
-			// Find edge and calc distance to the edge.
-			float va[3], vb[3];
-			if (!getPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, va, vb))
-				continue;
-			
-			// If the circle is not touching the next polygon, skip it.
-			float tseg;
-			float distSqr = dtDistancePtSegSqr2D(centerPos, va, vb, tseg);
-			if (distSqr > radiusSqr)
-				continue;
-			
-			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
-			if (!neighbourNode)
-			{
-				status |= DT_OUT_OF_NODES;
-				continue;
-			}
-			
-			if (neighbourNode->flags & DT_NODE_CLOSED)
-				continue;
-			
-			// Cost
-			if (neighbourNode->flags == 0)
-				dtVlerp(neighbourNode->pos, va, vb, 0.5f);
-			
-			const float total = bestNode->total + dtVdist(bestNode->pos, neighbourNode->pos);
-			
-			// The node is already in open list and the new result is worse, skip.
-			if ((neighbourNode->flags & DT_NODE_OPEN) && total >= neighbourNode->total)
-				continue;
-			
-			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
-			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
-			neighbourNode->total = total;
-			
-			if (neighbourNode->flags & DT_NODE_OPEN)
-			{
-				m_openList->modify(neighbourNode);
-			}
-			else
-			{
-				neighbourNode->flags = DT_NODE_OPEN;
-				m_openList->push(neighbourNode);
-			}
-		}
-	}
-	
-	if (!randomPoly)
-		return DT_FAILURE;
-	
-	// Randomly pick point on polygon.
-	const float* v = &randomTile->verts[randomPoly->verts[0]*3];
-	float verts[3*DT_VERTS_PER_POLYGON];
-	float areas[DT_VERTS_PER_POLYGON];
-	dtVcopy(&verts[0*3],v);
-	for (int j = 1; j < randomPoly->vertCount; ++j)
-	{
-		v = &randomTile->verts[randomPoly->verts[j]*3];
-		dtVcopy(&verts[j*3],v);
-	}
-	
-	const float s = frand();
-	const float t = frand();
-	
-	float pt[3];
-	dtRandomPointInConvexPoly(verts, randomPoly->vertCount, areas, s, t, pt);
-	
-	float h = 0.0f;
-	dtStatus stat = getPolyHeight(randomPolyRef, pt, &h);
-	if (dtStatusFailed(status))
-		return stat;
-	pt[1] = h;
-	
-	dtVcopy(randomPt, pt);
-	*randomRef = randomPolyRef;
-	
-	return DT_SUCCESS;
-}
-
-
 //////////////////////////////////////////////////////////////////////////////////////////
 
 /// @par
@@ -516,99 +241,92 @@ dtStatus dtNavMeshQuery::closestPointOnPoly(dtPolyRef ref, const float* pos, flo
     if (poly->getType() == DT_POLYTYPE_OFFMESH_CONNECTION) 
         return DT_FAILURE;
 
-	closestPointOnPolyInTile(tile, poly, pos, closest);
-	
+	if (closestPointOnPolyInTile(tile, poly, pos, closest) != DT_SUCCESS)
+		return DT_FAILURE;
 	return DT_SUCCESS;
 }
 
-void dtNavMeshQuery::closestPointOnPolyInTile(const dtMeshTile* tile, const dtPoly* poly,
-											  const float* pos, float* closest) const
+dtStatus 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)
-	{
-		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);
-		return;
-	}
-
-	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 = FLT_MAX;
-		int imin = -1;
-		for (int i = 0; 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]);
-	}
-
-	// Find height at the location.
-	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))
-		{
-			closest[1] = h;
-			break;
-		}
-	}
-
-/*	float closestDistSqr = FLT_MAX;
-	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;
-		}
-	}*/
+    const unsigned int ip = (unsigned int)(poly - tile->polys);
+    const dtPolyDetail* pd = &tile->detailMeshes[ip];
+
+    // TODO: The commented out version finds 'cylinder distance' instead of 'sphere distance' to the navmesh.
+    // Test and enable.
+    /*
+    // 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 = FLT_MAX;
+    int imin = -1;
+    for (int i = 0; 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]);
+    }
+
+    // Find height at the location.
+    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))
+    {
+    closest[1] = h;
+    break;
+    }
+    }
+    */
+    float closestDistSqr = FLT_MAX;
+    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;
+        }
+    }
+
+    return DT_SUCCESS;
 }
 
 /// @par
@@ -752,7 +470,8 @@ dtStatus dtNavMeshQuery::findNearestPoly(const float* center, const float* exten
 	{
 		dtPolyRef ref = polys[i];
 		float closestPtPoly[3];
-		closestPointOnPoly(ref, center, closestPtPoly);
+		if (closestPointOnPoly(ref, center, closestPtPoly) != DT_SUCCESS)
+			continue;
 		float d = dtVdistSqr(center, closestPtPoly);
 		if (d < nearestDistanceSqr)
 		{
@@ -790,7 +509,8 @@ dtPolyRef dtNavMeshQuery::findNearestPolyInTile(const dtMeshTile* tile, const fl
 		dtPolyRef ref = polys[i];
 		const dtPoly* poly = &tile->polys[m_nav->decodePolyIdPoly(ref)];
 		float closestPtPoly[3];
-		closestPointOnPolyInTile(tile, poly, center, closestPtPoly);
+		if (closestPointOnPolyInTile(tile, poly, center, closestPtPoly) != DT_SUCCESS)
+			continue;
 			
 		float d = dtVdistSqr(center, closestPtPoly);
 		if (d < nearestDistanceSqr)
@@ -872,15 +592,8 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
 		const dtPolyRef base = m_nav->getPolyRefBase(tile);
 		for (int i = 0; i < tile->header->polyCount; ++i)
 		{
-			const dtPoly* p = &tile->polys[i];
-			// Do not return off-mesh connection polygons.
-			if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
-				continue;
-			// Must pass filter
-			const dtPolyRef ref = base | (dtPolyRef)i;
-			if (!filter->passFilter(ref, tile, p))
-				continue;
 			// Calc polygon bounds.
+			dtPoly* p = &tile->polys[i];
 			const float* v = &tile->verts[p->verts[0]*3];
 			dtVcopy(bmin, v);
 			dtVcopy(bmax, v);
@@ -892,8 +605,12 @@ int dtNavMeshQuery::queryPolygonsInTile(const dtMeshTile* tile, const float* qmi
 			}
 			if (dtOverlapBounds(qmin,qmax, bmin,bmax))
 			{
-				if (n < maxPolys)
-					polys[n++] = ref;
+				const dtPolyRef ref = base | (dtPolyRef)i;
+				if (filter->passFilter(ref, tile, p))
+				{
+					if (n < maxPolys)
+						polys[n++] = ref;
+				}
 			}
 		}
 		return n;
@@ -924,23 +641,18 @@ dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* extents
 	m_nav->calcTileLoc(bmin, &minx, &miny);
 	m_nav->calcTileLoc(bmax, &maxx, &maxy);
 
-	static const int MAX_NEIS = 32;
-	const dtMeshTile* neis[MAX_NEIS];
-	
 	int n = 0;
 	for (int y = miny; y <= maxy; ++y)
 	{
 		for (int x = minx; x <= maxx; ++x)
 		{
-			const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
-			for (int j = 0; j < nneis; ++j)
+			const dtMeshTile* tile = m_nav->getTileAt(x,y);
+			if (!tile) continue;
+			n += queryPolygonsInTile(tile, bmin, bmax, filter, polys+n, maxPolys-n);
+			if (n >= maxPolys)
 			{
-				n += queryPolygonsInTile(neis[j], bmin, bmax, filter, polys+n, maxPolys-n);
-				if (n >= maxPolys)
-				{
-					*polyCount = n;
-					return DT_SUCCESS | DT_BUFFER_TOO_SMALL;
-				}
+				*polyCount = n;
+				return DT_SUCCESS;
 			}
 		}
 	}
@@ -1003,8 +715,6 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	dtNode* lastBestNode = startNode;
 	float lastBestNodeCost = startNode->total;
 	
-	dtStatus status = DT_SUCCESS;
-	
 	while (!m_openList->empty())
 	{
 		// Remove node from open list and put it in closed list.
@@ -1054,10 +764,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
-			{
-				status |= DT_OUT_OF_NODES;
 				continue;
-			}
 			
 			// If the node is visited the first time, calculate node position.
 			if (neighbourNode->flags == 0)
@@ -1110,7 +817,7 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 			// Add or update the node.
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags &= ~DT_NODE_CLOSED;
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
 			
@@ -1135,9 +842,6 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 		}
 	}
 	
-	if (lastBestNode->id != endRef)
-		status |= DT_PARTIAL_RESULT;
-	
 	// Reverse the path.
 	dtNode* prev = 0;
 	dtNode* node = lastBestNode;
@@ -1156,18 +860,13 @@ dtStatus dtNavMeshQuery::findPath(dtPolyRef startRef, dtPolyRef endRef,
 	do
 	{
 		path[n++] = node->id;
-		if (n >= maxPath)
-		{
-			status |= DT_BUFFER_TOO_SMALL;
-			break;
-		}
 		node = m_nodePool->getNodeAtIdx(node->pidx);
 	}
-	while (node);
+	while (node && n < maxPath);
 	
 	*pathCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -1227,7 +926,7 @@ dtStatus dtNavMeshQuery::initSlicedFindPath(dtPolyRef startRef, dtPolyRef endRef
 	return m_query.status;
 }
 	
-dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
+dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter)
 {
 	if (!dtStatusInProgress(m_query.status))
 		return m_query.status;
@@ -1253,10 +952,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 		if (bestNode->id == m_query.endRef)
 		{
 			m_query.lastBestNode = bestNode;
-			const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
-			m_query.status = DT_SUCCESS | details;
-			if (doneIters)
-				*doneIters = iter;
+			m_query.status = DT_SUCCESS;
 			return m_query.status;
 		}
 		
@@ -1269,8 +965,6 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 		{
 			// The polygon has disappeared during the sliced query, fail.
 			m_query.status = DT_FAILURE;
-			if (doneIters)
-				*doneIters = iter;
 			return m_query.status;
 		}
 		
@@ -1286,8 +980,6 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			{
 				// The polygon has disappeared during the sliced query, fail.
 				m_query.status = DT_FAILURE;
-				if (doneIters)
-					*doneIters = iter;
 				return m_query.status;
 			}
 		}
@@ -1311,10 +1003,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
-			{
-				m_query.status |= DT_OUT_OF_NODES;
 				continue;
-			}
 			
 			// If the node is visited the first time, calculate node position.
 			if (neighbourNode->flags == 0)
@@ -1367,7 +1056,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 			// Add or update the node.
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags &= ~DT_NODE_CLOSED;
 			neighbourNode->cost = cost;
 			neighbourNode->total = total;
 			
@@ -1394,13 +1083,7 @@ dtStatus dtNavMeshQuery::updateSlicedFindPath(const int maxIter, int* doneIters)
 	
 	// Exhausted all nodes, but could not find path.
 	if (m_openList->empty())
-	{
-		const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
-		m_query.status = DT_SUCCESS | details;
-	}
-
-	if (doneIters)
-		*doneIters = iter;
+		m_query.status = DT_SUCCESS;
 
 	return m_query.status;
 }
@@ -1427,10 +1110,6 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 	{
 		// Reverse the path.
 		dtAssert(m_query.lastBestNode);
-		
-		if (m_query.lastBestNode->id != m_query.endRef)
-			m_query.status |= DT_PARTIAL_RESULT;
-		
 		dtNode* prev = 0;
 		dtNode* node = m_query.lastBestNode;
 		do
@@ -1447,24 +1126,17 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPath(dtPolyRef* path, int* pathCount,
 		do
 		{
 			path[n++] = node->id;
-			if (n >= maxPath)
-			{
-				m_query.status |= DT_BUFFER_TOO_SMALL;
-				break;
-			}
 			node = m_nodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node);
+		while (node && n < maxPath);
 	}
 	
-	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
-
 	// Reset query.
 	memset(&m_query, 0, sizeof(dtQueryData));
 	
 	*pathCount = n;
 	
-	return DT_SUCCESS | details;
+	return DT_SUCCESS;
 }
 
 dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing, const int existingSize,
@@ -1477,7 +1149,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		return DT_FAILURE;
 	}
 	
-	if (dtStatusFailed(m_query.status))
+	if (m_query.status != DT_SUCCESS && m_query.status != DT_IN_PROGRESS)
 	{
 		// Reset query.
 		memset(&m_query, 0, sizeof(dtQueryData));
@@ -1505,9 +1177,7 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		
 		if (!node)
 		{
-			m_query.status |= DT_PARTIAL_RESULT;
-			dtAssert(m_query.lastBestNode);
-			node = m_query.lastBestNode;
+			return DT_FAILURE;
 		}
 		
 		// Reverse the path.
@@ -1525,128 +1195,24 @@ dtStatus dtNavMeshQuery::finalizeSlicedFindPathPartial(const dtPolyRef* existing
 		do
 		{
 			path[n++] = node->id;
-			if (n >= maxPath)
-			{
-				m_query.status |= DT_BUFFER_TOO_SMALL;
-				break;
-			}
 			node = m_nodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node);
+		while (node && n < maxPath);
 	}
 	
-	const dtStatus details = m_query.status & DT_STATUS_DETAIL_MASK;
-
 	// Reset query.
 	memset(&m_query, 0, sizeof(dtQueryData));
 	
 	*pathCount = n;
 	
-	return DT_SUCCESS | details;
-}
-
-
-dtStatus dtNavMeshQuery::appendVertex(const float* pos, const unsigned char flags, const dtPolyRef ref,
-									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
-									  int* straightPathCount, const int maxStraightPath) const
-{
-	if ((*straightPathCount) > 0 && dtVequal(&straightPath[((*straightPathCount)-1)*3], pos))
-	{
-		// The vertices are equal, update flags and poly.
-		if (straightPathFlags)
-			straightPathFlags[(*straightPathCount)-1] = flags;
-		if (straightPathRefs)
-			straightPathRefs[(*straightPathCount)-1] = ref;
-	}
-	else
-	{
-		// Append new vertex.
-		dtVcopy(&straightPath[(*straightPathCount)*3], pos);
-		if (straightPathFlags)
-			straightPathFlags[(*straightPathCount)] = flags;
-		if (straightPathRefs)
-			straightPathRefs[(*straightPathCount)] = ref;
-		(*straightPathCount)++;
-		// If reached end of path or there is no space to append more vertices, return.
-		if (flags == DT_STRAIGHTPATH_END || (*straightPathCount) >= maxStraightPath)
-		{
-			return DT_SUCCESS | (((*straightPathCount) >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
-		}
-	}
-	return DT_IN_PROGRESS;
+	return DT_SUCCESS;
 }
 
-dtStatus dtNavMeshQuery::appendPortals(const int startIdx, const int endIdx, const float* endPos, const dtPolyRef* path,
-									  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
-									  int* straightPathCount, const int maxStraightPath, const int options) const
-{
-	const float* startPos = &straightPath[(*straightPathCount-1)*3];
-	// Append or update last vertex
-	dtStatus stat = 0;
-	for (int i = startIdx; i < endIdx; i++)
-	{
-		// Calculate portal
-		const dtPolyRef from = path[i];
-		const dtMeshTile* fromTile = 0;
-		const dtPoly* fromPoly = 0;
-		if (dtStatusFailed(m_nav->getTileAndPolyByRef(from, &fromTile, &fromPoly)))
-			return DT_FAILURE | DT_INVALID_PARAM;
-		
-		const dtPolyRef to = path[i+1];
-		const dtMeshTile* toTile = 0;
-		const dtPoly* toPoly = 0;
-		if (dtStatusFailed(m_nav->getTileAndPolyByRef(to, &toTile, &toPoly)))
-			return DT_FAILURE | DT_INVALID_PARAM;
-		
-		float left[3], right[3];
-		if (dtStatusFailed(getPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, left, right)))
-			break;
-	
-		if (options & DT_STRAIGHTPATH_AREA_CROSSINGS)
-		{
-			// Skip intersection if only area crossings are requested.
-			if (fromPoly->getArea() == toPoly->getArea())
-				continue;
-		}
-		
-		// Append intersection
-		float s,t;
-		if (dtIntersectSegSeg2D(startPos, endPos, left, right, s, t))
-		{
-			float pt[3];
-			dtVlerp(pt, left,right, t);
-
-			stat = appendVertex(pt, 0, path[i+1],
-								straightPath, straightPathFlags, straightPathRefs,
-								straightPathCount, maxStraightPath);
-			if (stat != DT_IN_PROGRESS)
-				return stat;
-		}
-	}
-	return DT_IN_PROGRESS;
-}
 
-/// @par
-/// 
-/// This method peforms what is often called 'string pulling'.
-///
-/// The start position is clamped to the first polygon in the path, and the 
-/// end position is clamped to the last. So the start and end positions should 
-/// normally be within or very near the first and last polygons respectively.
-///
-/// The returned polygon references represent the reference id of the polygon 
-/// that is entered at the associated path position. The reference id associated 
-/// with the end point will always be zero.  This allows, for example, matching 
-/// off-mesh link points to their representative polygons.
-///
-/// If the provided result buffers are too small for the entire result set, 
-/// they will be filled as far as possible from the start toward the end 
-/// position.
-///
 dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* endPos,
 										  const dtPolyRef* path, const int pathSize,
 										  float* straightPath, unsigned char* straightPathFlags, dtPolyRef* straightPathRefs,
-										  int* straightPathCount, const int maxStraightPath, const int options) const
+										  int* straightPathCount, const int maxStraightPath) const
 {
 	dtAssert(m_nav);
 	
@@ -1658,23 +1224,29 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 	if (!path[0])
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	dtStatus stat = 0;
+	int n = 0;
 	
 	// TODO: Should this be callers responsibility?
 	float closestStartPos[3];
 	if (dtStatusFailed(closestPointOnPolyBoundary(path[0], startPos, closestStartPos)))
 		return DT_FAILURE | DT_INVALID_PARAM;
-
-	float closestEndPos[3];
-	if (dtStatusFailed(closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos)))
-		return DT_FAILURE | DT_INVALID_PARAM;
 	
 	// Add start point.
-	stat = appendVertex(closestStartPos, DT_STRAIGHTPATH_START, path[0],
-						straightPath, straightPathFlags, straightPathRefs,
-						straightPathCount, maxStraightPath);
-	if (stat != DT_IN_PROGRESS)
-		return stat;
+	dtVcopy(&straightPath[n*3], closestStartPos);
+	if (straightPathFlags)
+		straightPathFlags[n] = DT_STRAIGHTPATH_START;
+	if (straightPathRefs)
+		straightPathRefs[n] = path[0];
+	n++;
+	if (n >= maxStraightPath)
+	{
+		*straightPathCount = n;
+		return DT_SUCCESS;
+	}
+	
+	float closestEndPos[3];
+	if (closestPointOnPolyBoundary(path[pathSize-1], endPos, closestEndPos) != DT_SUCCESS)
+		return DT_FAILURE;
 	
 	if (pathSize > 1)
 	{
@@ -1702,28 +1274,17 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				// Next portal.
 				if (dtStatusFailed(getPortalPoints(path[i], path[i+1], left, right, fromType, toType)))
 				{
-					// Failed to get portal points, in practice this means that path[i+1] is invalid polygon.
-					// Clamp the end point to path[i], and return the path so far.
+					if (closestPointOnPolyBoundary(path[i], endPos, closestEndPos) != DT_SUCCESS)
+						return DT_FAILURE;
 					
-					if (dtStatusFailed(closestPointOnPolyBoundary(path[i], endPos, closestEndPos)))
-					{
-						// This should only happen when the first polygon is invalid.
-						return DT_FAILURE | DT_INVALID_PARAM;
-					}
-
-					// Apeend portals along the current straight path segment.
-					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
-					{
-						stat = appendPortals(apexIndex, i, closestEndPos, path,
-											 straightPath, straightPathFlags, straightPathRefs,
-											 straightPathCount, maxStraightPath, options);
-					}
-
-					stat = appendVertex(closestEndPos, 0, path[i],
-										straightPath, straightPathFlags, straightPathRefs,
-										straightPathCount, maxStraightPath);
+					dtVcopy(&straightPath[n*3], closestEndPos);
+					if (straightPathFlags)
+						straightPathFlags[n] = 0;
+					if (straightPathRefs)
+						straightPathRefs[n] = path[i];
+					n++;
 					
-					return DT_SUCCESS | DT_PARTIAL_RESULT | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+					return DT_SUCCESS;
 				}
 				
 				// If starting really close the portal, advance.
@@ -1755,16 +1316,6 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 				else
 				{
-					// Append portals along the current straight path segment.
-					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
-					{
-						stat = appendPortals(apexIndex, leftIndex, portalLeft, path,
-											 straightPath, straightPathFlags, straightPathRefs,
-											 straightPathCount, maxStraightPath, options);
-						if (stat != DT_IN_PROGRESS)
-							return stat;					
-					}
-				
 					dtVcopy(portalApex, portalLeft);
 					apexIndex = leftIndex;
 					
@@ -1775,12 +1326,30 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
 					dtPolyRef ref = leftPolyRef;
 					
-					// Append or update vertex
-					stat = appendVertex(portalApex, flags, ref,
-										straightPath, straightPathFlags, straightPathRefs,
-										straightPathCount, maxStraightPath);
-					if (stat != DT_IN_PROGRESS)
-						return stat;
+					if (!dtVequal(&straightPath[(n-1)*3], portalApex))
+					{
+						// Append new vertex.
+						dtVcopy(&straightPath[n*3], portalApex);
+						if (straightPathFlags)
+							straightPathFlags[n] = flags;
+						if (straightPathRefs)
+							straightPathRefs[n] = ref;
+						n++;
+						// If reached end of path or there is no space to append more vertices, return.
+						if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
+						{
+							*straightPathCount = n;
+							return DT_SUCCESS;
+						}
+					}
+					else
+					{
+						// The vertices are equal, update flags and poly.
+						if (straightPathFlags)
+							straightPathFlags[n-1] = flags;
+						if (straightPathRefs)
+							straightPathRefs[n-1] = ref;
+					}
 					
 					dtVcopy(portalLeft, portalApex);
 					dtVcopy(portalRight, portalApex);
@@ -1806,16 +1375,6 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 				else
 				{
-					// Append portals along the current straight path segment.
-					if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
-					{
-						stat = appendPortals(apexIndex, rightIndex, portalRight, path,
-											 straightPath, straightPathFlags, straightPathRefs,
-											 straightPathCount, maxStraightPath, options);
-						if (stat != DT_IN_PROGRESS)
-							return stat;
-					}
-
 					dtVcopy(portalApex, portalRight);
 					apexIndex = rightIndex;
 					
@@ -1825,13 +1384,31 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 					else if (rightPolyType == DT_POLYTYPE_OFFMESH_CONNECTION)
 						flags = DT_STRAIGHTPATH_OFFMESH_CONNECTION;
 					dtPolyRef ref = rightPolyRef;
-
-					// Append or update vertex
-					stat = appendVertex(portalApex, flags, ref,
-										straightPath, straightPathFlags, straightPathRefs,
-										straightPathCount, maxStraightPath);
-					if (stat != DT_IN_PROGRESS)
-						return stat;
+					
+					if (!dtVequal(&straightPath[(n-1)*3], portalApex))
+					{
+						// Append new vertex.
+						dtVcopy(&straightPath[n*3], portalApex);
+						if (straightPathFlags)
+							straightPathFlags[n] = flags;
+						if (straightPathRefs)
+							straightPathRefs[n] = ref;
+						n++;
+						// If reached end of path or there is no space to append more vertices, return.
+						if (flags == DT_STRAIGHTPATH_END || n >= maxStraightPath)
+						{
+							*straightPathCount = n;
+							return DT_SUCCESS;
+						}
+					}
+					else
+					{
+						// The vertices are equal, update flags and poly.
+						if (straightPathFlags)
+							straightPathFlags[n-1] = flags;
+						if (straightPathRefs)
+							straightPathRefs[n-1] = ref;
+					}
 					
 					dtVcopy(portalLeft, portalApex);
 					dtVcopy(portalRight, portalApex);
@@ -1845,23 +1422,25 @@ dtStatus dtNavMeshQuery::findStraightPath(const float* startPos, const float* en
 				}
 			}
 		}
-
-		// Append portals along the current straight path segment.
-		if (options & (DT_STRAIGHTPATH_AREA_CROSSINGS | DT_STRAIGHTPATH_ALL_CROSSINGS))
-		{
-			stat = appendPortals(apexIndex, pathSize-1, closestEndPos, path,
-								 straightPath, straightPathFlags, straightPathRefs,
-								 straightPathCount, maxStraightPath, options);
-			if (stat != DT_IN_PROGRESS)
-				return stat;
-		}
 	}
-
-	stat = appendVertex(closestEndPos, DT_STRAIGHTPATH_END, 0,
-						straightPath, straightPathFlags, straightPathRefs,
-						straightPathCount, maxStraightPath);
 	
-	return DT_SUCCESS | ((*straightPathCount >= maxStraightPath) ? DT_BUFFER_TOO_SMALL : 0);
+	// If the point already exists, remove it and add reappend the actual end location.  
+	if (n > 0 && dtVequal(&straightPath[(n-1)*3], closestEndPos))
+		n--;
+	
+	// Add end point.
+	if (n < maxStraightPath)
+	{
+		dtVcopy(&straightPath[n*3], closestEndPos);
+		if (straightPathFlags)
+			straightPathFlags[n] = DT_STRAIGHTPATH_END;
+		if (straightPathRefs)
+			straightPathRefs[n] = 0;
+		n++;
+	}
+	
+	*straightPathCount = n;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -1899,8 +1478,6 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
 	if (!m_nav->isValidPolyRef(startRef))
 		return DT_FAILURE | DT_INVALID_PARAM;
 	
-	dtStatus status = DT_SUCCESS;
-	
 	static const int MAX_STACK = 48;
 	dtNode* stack[MAX_STACK];
 	int nstack = 0;
@@ -2064,21 +1641,16 @@ dtStatus dtNavMeshQuery::moveAlongSurface(dtPolyRef startRef, const float* start
 		do
 		{
 			visited[n++] = node->id;
-			if (n >= maxVisitedSize)
-			{
-				status |= DT_BUFFER_TOO_SMALL;
-				break;
-			}
 			node = m_tinyNodePool->getNodeAtIdx(node->pidx);
 		}
-		while (node);
+		while (node && n < maxVisitedSize);
 	}
 	
 	dtVcopy(resultPos, bestPos);
 	
 	*visitedCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 
@@ -2181,7 +1753,7 @@ dtStatus dtNavMeshQuery::getEdgeMidPoint(dtPolyRef from, dtPolyRef to, float* mi
 {
 	float left[3], right[3];
 	unsigned char fromType, toType;
-	if (dtStatusFailed(getPortalPoints(from, to, left,right, fromType, toType)))
+	if (!getPortalPoints(from, to, left,right, fromType, toType))
 		return DT_FAILURE | DT_INVALID_PARAM;
 	mid[0] = (left[0]+right[0])*0.5f;
 	mid[1] = (left[1]+right[1])*0.5f;
@@ -2262,8 +1834,6 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 	hitNormal[1] = 0;
 	hitNormal[2] = 0;
 	
-	dtStatus status = DT_SUCCESS;
-	
 	while (curRef)
 	{
 		// Cast ray against current polygon.
@@ -2288,7 +1858,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			// Could not hit the polygon, keep the old t and report hit.
 			if (pathCount)
 				*pathCount = n;
-			return status;
+			return DT_SUCCESS;
 		}
 		// Keep track of furthest t so far.
 		if (tmax > *t)
@@ -2297,8 +1867,6 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 		// Store visited polygons.
 		if (n < maxPath)
 			path[n++] = curRef;
-		else
-			status |= DT_BUFFER_TOO_SMALL;
 		
 		// Ray end is completely inside the polygon.
 		if (segMax == -1)
@@ -2306,7 +1874,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			*t = FLT_MAX;
 			if (pathCount)
 				*pathCount = n;
-			return status;
+			return DT_SUCCESS;
 		}
 		
 		// Follow neighbours.
@@ -2408,7 +1976,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 			
 			if (pathCount)
 				*pathCount = n;
-			return status;
+			return DT_SUCCESS;
 		}
 		
 		// No hit, advance to neighbour polygon.
@@ -2418,7 +1986,7 @@ dtStatus dtNavMeshQuery::raycast(dtPolyRef startRef, const float* startPos, cons
 	if (pathCount)
 		*pathCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -2477,8 +2045,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
 	
-	dtStatus status = DT_SUCCESS;
-	
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2490,10 +2056,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 			resultCost[n] = 0;
 		++n;
 	}
-	else
-	{
-		status |= DT_BUFFER_TOO_SMALL;
-	}
 	
 	const float radiusSqr = dtSqr(radius);
 	
@@ -2549,10 +2111,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
-			{
-				status |= DT_OUT_OF_NODES;
 				continue;
-			}
 				
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -2568,7 +2127,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags &= ~DT_NODE_CLOSED;
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 			
@@ -2588,10 +2147,6 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 						resultCost[n] = neighbourNode->total;
 					++n;
 				}
-				else
-				{
-					status |= DT_BUFFER_TOO_SMALL;
-				}
 				neighbourNode->flags = DT_NODE_OPEN;
 				m_openList->push(neighbourNode);
 			}
@@ -2600,7 +2155,7 @@ dtStatus dtNavMeshQuery::findPolysAroundCircle(dtPolyRef startRef, const float*
 	
 	*resultCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -2657,8 +2212,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 	startNode->flags = DT_NODE_OPEN;
 	m_openList->push(startNode);
 	
-	dtStatus status = DT_SUCCESS;
-
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2670,10 +2223,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 			resultCost[n] = 0;
 		++n;
 	}
-	else
-	{
-		status |= DT_BUFFER_TOO_SMALL;
-	}
 	
 	while (!m_openList->empty())
 	{
@@ -2729,10 +2278,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 			
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
-			{
-				status |= DT_OUT_OF_NODES;
 				continue;
-			}
 			
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -2748,7 +2294,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags &= ~DT_NODE_CLOSED;
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 			
@@ -2768,10 +2314,6 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 						resultCost[n] = neighbourNode->total;
 					++n;
 				}
-				else
-				{
-					status |= DT_BUFFER_TOO_SMALL;
-				}
 				neighbourNode->flags = DT_NODE_OPEN;
 				m_openList->push(neighbourNode);
 			}
@@ -2780,7 +2322,7 @@ dtStatus dtNavMeshQuery::findPolysAroundShape(dtPolyRef startRef, const float* v
 	
 	*resultCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -2836,8 +2378,6 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 	float pa[DT_VERTS_PER_POLYGON*3];
 	float pb[DT_VERTS_PER_POLYGON*3];
 	
-	dtStatus status = DT_SUCCESS;
-	
 	int n = 0;
 	if (n < maxResult)
 	{
@@ -2846,10 +2386,6 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 			resultParent[n] = 0;
 		++n;
 	}
-	else
-	{
-		status |= DT_BUFFER_TOO_SMALL;
-	}
 	
 	while (nstack)
 	{
@@ -2963,10 +2499,6 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 					resultParent[n] = curRef;
 				++n;
 			}
-			else
-			{
-				status |= DT_BUFFER_TOO_SMALL;
-			}
 			
 			if (nstack < MAX_STACK)
 			{
@@ -2977,18 +2509,17 @@ dtStatus dtNavMeshQuery::findLocalNeighbourhood(dtPolyRef startRef, const float*
 	
 	*resultCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 
 struct dtSegInterval
 {
-	dtPolyRef ref;
 	short tmin, tmax;
 };
 
 static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
-						   const short tmin, const short tmax, const dtPolyRef ref)
+						   const short tmin, const short tmax)
 {
 	if (nints+1 > maxInts) return;
 	// Find insertion point.
@@ -3003,7 +2534,6 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
 	if (nints-idx)
 		memmove(ints+idx+1, ints+idx, sizeof(dtSegInterval)*(nints-idx));
 	// Store
-	ints[idx].ref = ref;
 	ints[idx].tmin = tmin;
 	ints[idx].tmax = tmax;
 	nints++;
@@ -3021,8 +2551,7 @@ static void insertInterval(dtSegInterval* ints, int& nints, const int maxInts,
 /// maximum segments per polygon of the source navigation mesh.
 /// 
 dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter* filter,
-											 float* segmentVerts, dtPolyRef* segmentRefs, int* segmentCount,
-											 const int maxSegments) const
+											 float* segments, int* segmentCount, const int maxSegments) const
 {
 	dtAssert(m_nav);
 	
@@ -3038,10 +2567,6 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 	dtSegInterval ints[MAX_INTERVAL];
 	int nints;
 	
-	const bool storePortals = segmentRefs != 0;
-	
-	dtStatus status = DT_SUCCESS;
-	
 	for (int i = 0, j = (int)poly->vertCount-1; i < (int)poly->vertCount; j = i++)
 	{
 		// Skip non-solid edges.
@@ -3061,95 +2586,54 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 						m_nav->getTileAndPolyByRefUnsafe(link->ref, &neiTile, &neiPoly);
 						if (filter->passFilter(link->ref, neiTile, neiPoly))
 						{
-							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax, link->ref);
+							insertInterval(ints, nints, MAX_INTERVAL, link->bmin, link->bmax);
 						}
 					}
 				}
 			}
 		}
-		else
+		else if (poly->neis[j])
 		{
 			// Internal edge
-			dtPolyRef neiRef = 0;
-			if (poly->neis[j])
-			{
-				const unsigned int idx = (unsigned int)(poly->neis[j]-1);
-				neiRef = m_nav->getPolyRefBase(tile) | idx;
-				if (!filter->passFilter(neiRef, tile, &tile->polys[idx]))
-					neiRef = 0;
-			}
-
-			// If the edge leads to another polygon and portals are not stored, skip.
-			if (neiRef != 0 && !storePortals)
+			const unsigned int idx = (unsigned int)(poly->neis[j]-1);
+			const dtPolyRef ref = m_nav->getPolyRefBase(tile) | idx;
+			if (filter->passFilter(ref, tile, &tile->polys[idx]))
 				continue;
-			
-			if (n < maxSegments)
-			{
-				const float* vj = &tile->verts[poly->verts[j]*3];
-				const float* vi = &tile->verts[poly->verts[i]*3];
-				float* seg = &segmentVerts[n*6];
-				dtVcopy(seg+0, vj);
-				dtVcopy(seg+3, vi);
-				if (segmentRefs)
-					segmentRefs[n] = neiRef;
-				n++;
-			}
-			else
-			{
-				status |= DT_BUFFER_TOO_SMALL;
-			}
-			
-			continue;
 		}
 		
 		// Add sentinels
-		insertInterval(ints, nints, MAX_INTERVAL, -1, 0, 0);
-		insertInterval(ints, nints, MAX_INTERVAL, 255, 256, 0);
+		insertInterval(ints, nints, MAX_INTERVAL, -1, 0);
+		insertInterval(ints, nints, MAX_INTERVAL, 255, 256);
 		
-		// Store segments.
+		// Store segment.
 		const float* vj = &tile->verts[poly->verts[j]*3];
 		const float* vi = &tile->verts[poly->verts[i]*3];
 		for (int k = 1; k < nints; ++k)
 		{
-			// Portal segment.
-			if (storePortals && ints[k].ref)
+			// Find the space inbetween the opening areas.
+			const int imin = ints[k-1].tmax;
+			const int imax = ints[k].tmin;
+			if (imin == imax) continue;
+			if (imin == 0 && imax == 255)
 			{
-				const float tmin = ints[k].tmin/255.0f; 
-				const float tmax = ints[k].tmax/255.0f; 
 				if (n < maxSegments)
 				{
-					float* seg = &segmentVerts[n*6];
-					dtVlerp(seg+0, vj,vi, tmin);
-					dtVlerp(seg+3, vj,vi, tmax);
-					if (segmentRefs)
-						segmentRefs[n] = ints[k].ref;
+					float* seg = &segments[n*6];
 					n++;
-				}
-				else
-				{
-					status |= DT_BUFFER_TOO_SMALL;
+					dtVcopy(seg+0, vj);
+					dtVcopy(seg+3, vi);
 				}
 			}
-
-			// Wall segment.
-			const int imin = ints[k-1].tmax;
-			const int imax = ints[k].tmin;
-			if (imin != imax)
+			else
 			{
 				const float tmin = imin/255.0f; 
 				const float tmax = imax/255.0f; 
 				if (n < maxSegments)
 				{
-					float* seg = &segmentVerts[n*6];
+					float* seg = &segments[n*6];
+					n++;
 					dtVlerp(seg+0, vj,vi, tmin);
 					dtVlerp(seg+3, vj,vi, tmax);
-					if (segmentRefs)
-						segmentRefs[n] = 0;
-					n++;
-				}
-				else
-				{
-					status |= DT_BUFFER_TOO_SMALL;
 				}
 			}
 		}
@@ -3157,7 +2641,7 @@ dtStatus dtNavMeshQuery::getPolyWallSegments(dtPolyRef ref, const dtQueryFilter*
 	
 	*segmentCount = n;
 	
-	return status;
+	return DT_SUCCESS;
 }
 
 /// @par
@@ -3196,8 +2680,6 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 	
 	float radiusSqr = dtSqr(maxRadius);
 	
-	dtStatus status = DT_SUCCESS;
-	
 	while (!m_openList->empty())
 	{
 		dtNode* bestNode = m_openList->pop();
@@ -3305,10 +2787,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 
 			dtNode* neighbourNode = m_nodePool->getNode(neighbourRef);
 			if (!neighbourNode)
-			{
-				status |= DT_OUT_OF_NODES;
 				continue;
-			}
 			
 			if (neighbourNode->flags & DT_NODE_CLOSED)
 				continue;
@@ -3327,7 +2806,7 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 				continue;
 			
 			neighbourNode->id = neighbourRef;
-			neighbourNode->flags = (neighbourNode->flags & ~DT_NODE_CLOSED);
+			neighbourNode->flags &= ~DT_NODE_CLOSED;
 			neighbourNode->pidx = m_nodePool->getNodeIdx(bestNode);
 			neighbourNode->total = total;
 				
@@ -3347,23 +2826,9 @@ dtStatus dtNavMeshQuery::findDistanceToWall(dtPolyRef startRef, const float* cen
 	dtVsub(hitNormal, centerPos, hitPos);
 	dtVnormalize(hitNormal);
 	
-	*hitDist = dtSqrt(radiusSqr);
+	*hitDist = sqrtf(radiusSqr);
 	
-	return status;
-}
-
-bool dtNavMeshQuery::isValidPolyRef(dtPolyRef ref, const dtQueryFilter* filter) const
-{
-	const dtMeshTile* tile = 0;
-	const dtPoly* poly = 0;
-	dtStatus status = m_nav->getTileAndPolyByRef(ref, &tile, &poly);
-	// If cannot get polygon, assume it does not exists and boundary is invalid.
-	if (dtStatusFailed(status))
-		return false;
-	// If cannot pass filter, assume flags has changed and boundary is invalid.
-	if (!filter->passFilter(ref, tile, poly))
-		return false;
-	return true;
+	return DT_SUCCESS;
 }
 
 /// @par