[Code] Fix Recast path to keep away from the wall/corners

[Game2Mesh]

There is an issue when you are using recast to make your navigation system:The path is too close to the wall or corner,it makes the toon stuck regularly unless you makesure the toon's move precisely.

Another way to fix this problem is set the agent radius bigger,but it's not the best solution.If you do that,some small door or small path will be blocked,even it's not blocking in game world.

Here is my solution:

Before:


After:



Now is coding time:

Add function declaration in NavMeshTesterTool.h:

void FixPath();
float DistanceToWall(float* pos,float* hitPos,float* hitNormal);

Add this code in NavMeshTesterTool.cpp:

float Fix_GetXZArea(float* verts, int nv)
{
	float area = 0;
	for(int i=0; i<nv-1; i++)
		area+=(verts[i*3]*verts[i*3+5]-verts[i*3+3]*verts[i*3+2]);
	area += (verts[(nv-1)*3]*verts[2] - verts[0]*verts[(nv-1)*3+2]);
	return area*0.5f;
}


//dtPointInPolygon will return false when the point is too close to the edge,so we rewite the test function.
bool Fix_PointIsInPoly(float* pos,float* verts,int nv,float err)
{
	//poly area
	float area = abs(Fix_GetXZArea(verts,nv));

	//calculate each area of triangles
	float TestTri[9];
	memcpy(TestTri,pos,sizeof(float)*3);
	float area1 = 0;
	for(int i=0;i<nv-1;++i)
	{
		memcpy(&TestTri[3],&verts[i*3],sizeof(float)*3);
		memcpy(&TestTri[6],&verts[i*3+3],sizeof(float)*3);
		area1+= abs(Fix_GetXZArea(TestTri,3));
		if(area1-err>area)
			return false;
	}

	//last one
	memcpy(&TestTri[3],verts,sizeof(float)*3);
	memcpy(&TestTri[6],&verts[nv*3-3],sizeof(float)*3);
	area1+= abs(Fix_GetXZArea(TestTri,3));

	return abs(area1-area)<err;
}


float NavMeshTesterTool::DistanceToWall(float* pos,float* hitPos,float* hitNormal)
{
	float distanceToWall=0;
	dtPolyRef ref;
	if(dtStatusSucceed(m_navQuery->findNearestPoly(pos, m_polyPickExt, &m_filter, &ref, 0))==false || ref ==0)
		return -1;

	const dtMeshTile* tile = 0;
	const dtPoly* poly = 0;
	if (dtStatusFailed(m_navMesh->getTileAndPolyByRef(ref, &tile, &poly)))
		return -1;
	
	// Collect vertices.
	float verts[DT_VERTS_PER_POLYGON*3];
	int nv = 0;
	for (int i = 0; i < (int)poly->vertCount; ++i)
	{
		dtVcopy(&verts[nv*3], &tile->verts[poly->verts[i]*3]);
		nv++;
	}		
	
	bool inside = Fix_PointIsInPoly(pos, verts, nv,0.05f);
	if(inside == false)
		return -1;

	if(dtStatusSucceed(m_navQuery->findDistanceToWall(ref, pos, 100.0f, &m_filter, &distanceToWall, hitPos, hitNormal))==false)
		return -1;

	return distanceToWall;
}

#define MIN_WALL_DISTANCE 1.5f    //set this value bigger to make the path point far way from wall

//Try to fix the path,
void NavMeshTesterTool::FixPath()
{
	float hitPos[3];
	float hitNormal[3];
	float TestPos[3];
	float distanceToWall=0;
	float up[3]={0,1,0};
	float origDis = 0;

	for(int i=1;i<m_nstraightPath-1;++i)
	{
		dtPolyRef pt;
		float* pCurPoi=&m_straightPath[i*3];
		distanceToWall = DistanceToWall(pCurPoi,hitPos,hitNormal);
		
		if(distanceToWall<MIN_WALL_DISTANCE && distanceToWall>=0)
		{
			float vec[3];
			dtVsub(vec,&m_straightPath[i*3-3],&m_straightPath[i*3]);
			//distanceToWall is 0 means the point is in the edge.so we can't get the hitpos.
			if(distanceToWall == 0)
			{
				//test left side
				dtVcross(TestPos,vec,up);
				dtVadd(TestPos,TestPos,pCurPoi);
				float ft = MIN_WALL_DISTANCE/dtVdist(TestPos,pCurPoi);
				dtVlerp(TestPos,pCurPoi,TestPos,ft);
				distanceToWall = DistanceToWall(TestPos,hitPos,hitNormal);
				if(abs(MIN_WALL_DISTANCE - distanceToWall)>0.1f)
				{
					//test right side
					dtVcross(TestPos,up,vec);
					dtVadd(TestPos,TestPos,pCurPoi);
					ft = MIN_WALL_DISTANCE/dtVdist(TestPos,pCurPoi);
					dtVlerp(TestPos,pCurPoi,TestPos,ft);
					distanceToWall = DistanceToWall(TestPos,hitPos,hitNormal);
				}

				//if test point is better than the orig point,replace it.
				if(abs(distanceToWall-MIN_WALL_DISTANCE)<0.1f)
					dtVcopy(pCurPoi,TestPos);
			}
			else
			{
				//ok,we get the hitpos,just make a ray
				float ft = MIN_WALL_DISTANCE/distanceToWall;
				dtVlerp(TestPos,hitPos,pCurPoi,ft);
				distanceToWall = DistanceToWall(TestPos,hitPos,hitNormal);

				if(abs(distanceToWall-MIN_WALL_DISTANCE)<0.1f)
					dtVcopy(pCurPoi,TestPos);
			}
		}
	}
}

Last thing:find NavMeshTesterTool::recalc(),search "m_toolMode == TOOLMODE_PATHFIND_STRAIGHT" and add one line:

m_navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys,
							m_straightPath, m_straightPathFlags,
							m_straightPathPolys, &m_nstraightPath, MAX_POLYS, m_straightPathOptions);
				
FixPath(); <----Call this function after call findStraightPath.

OK,it's done.Have fun

[telcy007]

Very nice work! It's a better way then my solution to expand the moving vector