⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 pathdoc.cpp

📁 本程序主要是设计了一个人工势能场机器人运动规划算法。并提出了一个新的斥力场函数。
💻 CPP
📖 第 1 页 / 共 2 页
字号:
🔍
12 
				}
				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;
	}
}
// *end*
12

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -