pathdoc.cpp

本程序主要是设计了一个人工势能场机器人运动规划算法。并提出了一个新的斥力场函数。

						jj=5;
					if(m_cBoard[i-jj][j])
						jj=5;
				}
				if(jj==4)
				{
					pt2 = CPoint(px,py);
					ii = 40;
				}
			}
			///////////////////

			//计算两个点与中间点的距离，取小的
			float ds1,ds2;
			ds1=ComDistance(pt1,middle);
			ds2=ComDistance(pt2,middle);
			if(ds1<=ds2)
			{
				newend=pt1;			
			}
			else
			{
				newend=pt2;
			}
			return;
		}
	}
}

//********** 沿墙走 **************
void CPathDoc::ResetArray()
{
	m_iAllow[0]=m_iAllow[1]=m_iAllow[2]=m_iAllow[3]=0;
	//flag0=flag1=flag2=flag3=false;
}

void CPathDoc::CheckCurrentPoint(CPoint now)
{
	//int xi = now.x/MAP_GRIDSIZE;
	//int yj = now.y/MAP_GRIDSIZE;
	int xi = now.x;
	int yj = now.y;

	//ResetArray();
	int i=1;
	for(i;i<3;i++)
	{
		if(m_cBoard[xi+i][yj]==1)
			m_iAllow[0] = 1;
		if(m_cBoard[xi-i][yj]==1)
			m_iAllow[2] = 1;
		if(m_cBoard[xi][yj+i]==1)
			m_iAllow[3] = 1;
		if(m_cBoard[xi][yj-i]==1)
			m_iAllow[1] = 1;
	}
}

void CPathDoc::SetFlag(CPoint now)
{
	for(int i=0;i<4;i++)
	{

	}
}

CPoint CPathDoc::GetNextPoint(CPoint now)
{
	//int xi = now.x/MAP_GRIDSIZE;
	//int yj = now.y/MAP_GRIDSIZE;
	int xi = now.x;
	int yj = now.y;
	//ResetArray();

	if(!m_iAllow[0])
	{
		ResetArray();
		m_iAllow[2] = 1;
		return CPoint(xi+1,yj);
	}
	if(!m_iAllow[1])
	{
		ResetArray();
		m_iAllow[3] = 1;	
		return CPoint(xi,yj-1);
	}
	if(!m_iAllow[2])
	{
		ResetArray();
		m_iAllow[0] = 1;
		return CPoint(xi-1,yj);
	}
	if(!m_iAllow[3])
	{
		ResetArray();
		m_iAllow[1] = 1;
		return CPoint(xi,yj+1);
	}
	return CPoint(1,4);
}

//********************************
// *问题的最终解决*
void CPathDoc::ResetAllFlag()
{
	flagUp=flagDown=true;
	flagRight=flagLeft=true;
}

void CPathDoc::CheckUpDown(CPoint now)
{
	int px = now.x / MAP_GRIDSIZE;
	int py = now.y / MAP_GRIDSIZE;
	int i;
	for(i=1;i<10;i++)
	{
		if(m_cBoard[px][py-i]!=0)
			flagUp=false;
		if(m_cBoard[px][py+i]!=0)
			flagDown=false;
	}
}

void CPathDoc::GetUDEndPoint(CPoint now,CPoint end,CPoint &newend)
{
	//middle 点的产生
	CPoint middle;
	bool flag;
	if(end.x > now.x)
		flag=true;// "+"
	else
		flag=false;// "-"
	if(flagUp && !flagDown)
	//下方有障碍物
	{
		for(int ii=1;ii<20;ii++)
		{
			float px;
			if(flag)
				px = now.x + 6*ii;
			else
				px = now.x - 6*ii;
			float py = now.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.x = px + 4;
				else
					middle.x = px - 4;
				middle.y = py;
				ii=22;
			}
		}
		//////////////////
		for(ii=1;ii<40;ii++)
		{
				float py = middle.y - 6*ii;
				float px = middle.x;
				int i=px/MAP_GRIDSIZE;
				int j=py/MAP_GRIDSIZE;
				for(int jj=1;jj<4;jj++)
				{
					if(m_cBoard[i][j+jj])
						jj=5;
					if(m_cBoard[i][j-jj])
						jj=5;
					if(m_cBoard[i+jj][j])
						jj=5;
					if(m_cBoard[i-jj][j])
						jj=5;
				}
				if(jj==4)
				{
					newend = CPoint(px,py);
					//pt2 = CPoint(px-6,k2*(px+6)+b2);
					ii = 40;
				}
		}
		return;
	}
	if(!flagUp && flagDown)
	//上方有障碍物
	{
		for(int ii=1;ii<20;ii++)
		{
			float px;
			if(flag)
				px = now.x + 6*ii;
			else
				px = now.x - 6*ii;
			float py = now.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.x = px + 4;
				else
					middle.x = px - 4;
				middle.y = py;
				ii=22;
			}
		}
		////////////////////
		for(ii=1;ii<40;ii++)
		{
				float py = middle.y + 6*ii;
				float px = middle.x;
				int i=px/MAP_GRIDSIZE;
				int j=py/MAP_GRIDSIZE;
				for(int jj=1;jj<4;jj++)
				{
					if(m_cBoard[i][j+jj])
						jj=5;
					if(m_cBoard[i][j-jj])
						jj=5;
					if(m_cBoard[i+jj][j])
						jj=5;
					if(m_cBoard[i-jj][j])
						jj=5;
				}
				if(jj==4)
				{
					newend = CPoint(px,py);
					ii = 40;
				}
		}
		return;
	}
	if(flagUp && flagDown)
	//上下都无障碍
	{
		for(int ii=1;ii<20;ii++)
		{
			float px;
			if(flag)
				px = now.x + 6*ii;
			else
				px = now.x - 6*ii;
			float py = now.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.x = px + 4;
				else
					middle.x = px - 4;
				middle.y = py;
				ii=22;
			}
		}
		/////////////////////
		CPoint pt1,pt2;
		for(ii=1;ii<20;ii++)
		{
			float py = middle.y + 6*ii;
			float px = middle.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}

		for(ii=1;ii<20;ii++)
		{
			float py = middle.y - 6*ii;
			float px = middle.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}

		//计算两个点与中间点的距离，取小的
		float ds1,ds2;
		ds1=ComDistance(pt1,now);
		ds2=ComDistance(pt2,now);
		if(ds1<=ds2)
		{
			newend=pt1;
		}
		else
		{
			newend=pt2;
		}
		return;
	}
	if(!flagUp && !flagDown)
	//上下都有障碍
	{
		if(flag)
			middle.x = now.x - 12;
		else
			middle.x = now.x + 12;
		middle.y = now.y;
		////////////////
		CPoint pt1,pt2;
		for(int ii=0;ii<40;ii++)
		{
			float py = middle.y + 8*ii;
			float px = middle.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			if(m_cBoard[i][j])
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}
		///
		for(ii=1;ii<40;ii++)
		{
			float py = pt1.y + 6*ii;
			float px = pt1.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}
		////////////////////

		for(ii=0;ii<40;ii++)
		{
			float py = middle.y - 6*ii;
			float px = middle.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			if(m_cBoard[i][j])
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}
		//
		for(ii=1;ii<40;ii++)
		{
			float py = pt2.y - 6*ii;
			float px = pt2.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}
		///////////////////

		//计算两个点与中间点的距离，取小的
		float ds1,ds2;
		ds1=ComDistance(pt1,middle);
		ds2=ComDistance(pt2,middle);
		if(ds1<=ds2)
		{
			newend=pt1;			
		}
		else
		{
			newend=pt2;
		}
		return;
	}
}

void CPathDoc::CheckRightLeft(CPoint now)
{
	int px = now.x / MAP_GRIDSIZE;
	int py = now.y / MAP_GRIDSIZE;
	int i;
	for(i=1;i<10;i++)
	{
		if(m_cBoard[px-i][py]!=0)
			flagLeft=false;
		if(m_cBoard[px+i][py]!=0)
			flagRight=false;
	}
}

void CPathDoc::GetRLEndPoint(CPoint now,CPoint end,CPoint &newend)
{
	//middle 点的产生
	CPoint middle;
	bool flag;
	if(end.y > now.y)
		flag=true;// "+"
	else
		flag=false;// "-"
	if(flagRight && !flagLeft)
	//左方有障碍物
	{
		for(int ii=1;ii<20;ii++)
		{
			float py;
			if(flag)
				py = now.y + 6*ii;
			else
				py = now.y - 6*ii;
			float px = now.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.y = py + 4;
				else
					middle.y = py - 4;
				middle.x = px;
				ii=22;
			}
		}
		//////////////////

		for(ii=1;ii<40;ii++)
		{
				float px = middle.x + 6*ii;
				float py = middle.y;
				int i=px/MAP_GRIDSIZE;
				int j=py/MAP_GRIDSIZE;
				for(int jj=1;jj<4;jj++)
				{
					if(m_cBoard[i][j+jj])
						jj=5;
					if(m_cBoard[i][j-jj])
						jj=5;
					if(m_cBoard[i+jj][j])
						jj=5;
					if(m_cBoard[i-jj][j])
						jj=5;
				}
				if(jj==4)
				{
					newend = CPoint(px,py);
					ii = 40;
				}
		}
		return;
	}
	if(!flagRight && flagLeft)
	//右方有障碍物
	{
		for(int ii=1;ii<20;ii++)
		{
			float py;
			if(flag)
				py = now.y + 6*ii;
			else
				py = now.y - 6*ii;
			float px = now.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.y = py + 4;
				else
					middle.y = py - 4;
				middle.x = px;
				ii=22;
			}
		}
		////////////////////

		for(ii=1;ii<40;ii++)
		{
				float px = middle.x - 6*ii;
				float py = middle.y;
				int i=px/MAP_GRIDSIZE;
				int j=py/MAP_GRIDSIZE;
				for(int jj=1;jj<4;jj++)
				{
					if(m_cBoard[i][j+jj])
						jj=5;
					if(m_cBoard[i][j-jj])
						jj=5;
					if(m_cBoard[i+jj][j])
						jj=5;
					if(m_cBoard[i-jj][j])
						jj=5;
				}
				if(jj==4)
				{
					newend = CPoint(px,py);
					ii = 40;
				}
		}
		return;
	}
	if(flagRight && flagLeft)
	//左右都无障碍
	{
		for(int ii=1;ii<20;ii++)
		{
			float py;
			if(flag)
				py = now.y + 6*ii;
			else
				py = now.y - 6*ii;
			float px = now.x;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;//得到网格坐标
			if(m_cBoard[i][j]==1)//有障碍
			{
				if(flag)
					middle.y = py + 4;
				else
					middle.y = py - 4;
				middle.x = px;
				ii=22;
			}
		}
		/////////////////////
		CPoint pt1,pt2;
		for(ii=1;ii<40;ii++)
		{
			float px = middle.x + 6*ii;
			float py = middle.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}

		for(ii=1;ii<40;ii++)
		{
			float px = middle.x - 6*ii;
			float py = middle.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}

		//计算两个点与中间点的距离，取小的
		float ds1,ds2;
		if(flagRight && !flagLeft)
		{
			ds1=ComDistance(pt1,middle);
			newend=pt1;
			return;
		}
		if(flagLeft && !flagRight)
		{
			ds2=ComDistance(pt2,middle);
			newend=pt2;
			return;
		}
		ds1=ComDistance(pt1,middle);
		ds2=ComDistance(pt2,middle);
		if(ds1<=ds2)
		{
			newend=pt1;
		}
		else
		{
			newend=pt2;
		}
		return;
	}
	if(!flagRight && !flagLeft)
	//左右都有障碍
	{
		if(flag)
			middle.y = now.y - 10;
		else
			middle.y = middle.y + 10;
		middle.x = now.x;
		////////////////
		CPoint pt1,pt2;
		for(int ii=1;ii<40;ii++)
		{
			float px = middle.x + 6*ii;
			float py = middle.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			if(m_cBoard[i][j])
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}
		///
		for(ii=1;ii<40;ii++)
		{
			float px = pt1.x + 6*ii;
			float py = pt1.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt1 = CPoint(px,py);
				ii = 40;
			}
		}
		////////////////////

		for(ii=1;ii<40;ii++)
		{
			float px = middle.x - 6*ii;
			float py = middle.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			if(m_cBoard[i][j])
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}
		//
		for(ii=1;ii<40;ii++)
		{
			float px = pt2.x - 6*ii;
			float py = pt2.y;
			int i=px/MAP_GRIDSIZE;
			int j=py/MAP_GRIDSIZE;
			for(int jj=1;jj<4;jj++)
			{
				if(m_cBoard[i][j+jj])
					jj=5;
				if(m_cBoard[i][j-jj])
					jj=5;
				if(m_cBoard[i+jj][j])
					jj=5;
				if(m_cBoard[i-jj][j])
					jj=5;
			}
			if(jj==4)
			{
				pt2 = CPoint(px,py);
				ii = 40;
			}
		}
		///////////////////

		//计算两个点与中间点的距离，取小的
		float ds1,ds2;
		ds1=ComDistance(pt1,middle);
		ds2=ComDistance(pt2,middle);
		if(ds1<=ds2)
		{
			newend=pt1;			
		}
		else
		{
			newend=pt2;
		}
		return;
	}
}

//处理倾斜直线情况
void CPathDoc::ResetAllFlag2()
{
	flagLeftUp=flagRightDown=true;
	flagRightUp=flagLeftDown=true;
}

void CPathDoc::CheckAlongLine(float k2,float b2,CPoint now)
{
	if(k2>0)
	{
		int x,y;
		float px,py;
		for(int i=1;i<15;i++)
		{
			px = now.x + 6*i;
			py = k2*px + b2;
			x = px/MAP_GRIDSIZE;
			y = py/MAP_GRIDSIZE;
			if(m_cBoard[x][y])
				flagRightUp = false;
			///////////////////////
			px = now.x - 6*i;
			py = k2*px + b2;
			x = px/MAP_GRIDSIZE;
			y = py/MAP_GRIDSIZE;
			if(m_cBoard[x][y])
				flagLeftDown = false;			
		}
		return;
	}
	if(k2<0)
	{
		int x,y;
		float px,py;
		for(int i=0;i<15;i++)
		{
			px = now.x - 6*i;
			py = k2*px + b2;
			x = px/MAP_GRIDSIZE;
			y = py/MAP_GRIDSIZE;
			if(m_cBoard[x][y])
				flagLeftUp = false;
			///////////////////////
			px = now.x + 6*i;
			py = k2*px + b2;
			x = px/MAP_GRIDSIZE;
			y = py/MAP_GRIDSIZE;
			if(m_cBoard[x][y])
				flagRightDown = false;			
		}
		return;
	}
}

// *end*