📄 pathdoc.cpp

📁 本程序主要是设计了一个人工势能场机器人运动规划算法。并提出了一个新的斥力场函数。
💻 CPP
📖 第 1 页 / 共 2 页
字号:
12 下一页
// PathDoc.cpp : implementation of the CPathDoc class
//

#include "stdafx.h"
#include "Path.h"

#include "PathDoc.h"
#include "math.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CPathDoc

IMPLEMENT_DYNCREATE(CPathDoc, CDocument)

BEGIN_MESSAGE_MAP(CPathDoc, CDocument)
	//{{AFX_MSG_MAP(CPathDoc)
		// NOTE - the ClassWizard will add and remove mapping macros here.
		//    DO NOT EDIT what you see in these blocks of generated code!
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CPathDoc construction/destruction

CPathDoc::CPathDoc()
{
	// TODO: add one-time construction code here
	memset(m_cBoard, 0, sizeof(m_cBoard));
	m_uBrushType = 0;
	m_Start.x = m_Start.y = -1;
	m_End.x = m_End.y = -1;

	m_cStart.x = m_cStart.y = -1;
	m_cNow = m_cStart;
	m_cEnd.x = m_cEnd.y = -1;
	m_bDrawRoute = false;

	m_cObsArray.RemoveAll();
	m_cPathArray.RemoveAll();
	m_cEndArray.RemoveAll();

	flagUp=flagDown=flagLeft=flagRight=true;
	KillFlag = false;
}

CPathDoc::~CPathDoc()
{
}

BOOL CPathDoc::OnNewDocument()
{
	if (!CDocument::OnNewDocument())
		return FALSE;

	// TODO: add reinitialization code here
	// (SDI documents will reuse this document)
	memset(m_cBoard, 0, sizeof(m_cBoard));
	m_uBrushType = 0;
	m_cStart.x = m_cStart.y = -1;
	m_cNow = m_cStart;
	m_cEnd.x = m_cEnd.y = -1;
	m_bDrawRoute = false;

	m_cObsArray.RemoveAll();
	m_cAddArray.RemoveAll();
	m_cPathArray.RemoveAll();
	m_cEndArray.RemoveAll();

	return TRUE;
}



/////////////////////////////////////////////////////////////////////////////
// CPathDoc serialization

void CPathDoc::Serialize(CArchive& ar)
{
	int x = MAP_BOARDX, y = MAP_BOARDY;
	if (ar.IsStoring())
	{
		ar << x;
		ar << y;
		ar << m_Start;
		ar << m_End;
		ar << m_cStart;
		ar << m_cEnd;

		for (int i=0; i<x; i++) {
			for (int j=0; j<y; j++) {
				ar << m_cBoard[i][j];
			}
		}
	}
	else
	{
		ar >> x;
		ar >> y;
		ar >> m_Start;
		ar >> m_End;
		ar >> m_cStart;
		ar >> m_cEnd;

		for (int i=0; i<x; i++) {
			for (int j=0; j<y; j++) {
				ar >> m_cBoard[i][j];
			}
		}
	}
}

/////////////////////////////////////////////////////////////////////////////
// CPathDoc diagnostics

#ifdef _DEBUG
void CPathDoc::AssertValid() const
{
	CDocument::AssertValid();
}

void CPathDoc::Dump(CDumpContext& dc) const
{
	CDocument::Dump(dc);
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CPathDoc commands

//***** APF *****
bool CPathDoc::GetMapInfo()
//获取地图的相应点信息
{
	m_cObsArray.RemoveAll();
	m_cPathArray.RemoveAll();
	m_cEndArray.RemoveAll();
	//获得障碍节点坐标
	int i,j;
	for(i=0;i<MAP_BOARDX;i++)
		for(j=0;j<MAP_BOARDY;j++)
		{
			if(m_cBoard[i][j]==1)
			{
				CPoint pt = CPoint(i*MAP_GRIDSIZE+4,j*MAP_GRIDSIZE+4);
				m_cObsArray.Add(pt);
			}
		}
	//判断起始，终止节点
	if (m_Start.x == -1 || m_End.x == -1) {
		MessageBox("请确定已经设置了起始、终止节点!\n红色表示起点，蓝色表示终点.", "APF", MB_ICONERROR);
		return false;
	}
	//存储开始，目标点
	m_cStart.x=m_Start.x*MAP_GRIDSIZE+4;
	m_cStart.y=m_Start.y*MAP_GRIDSIZE+4;
	m_cNow=m_cStart;
	m_cEnd.x=m_End.x*MAP_GRIDSIZE+4;
	m_cEnd.y=m_End.y*MAP_GRIDSIZE+4;

	//记入相应点数组
	m_cPathArray.Add(m_cNow);
	m_cEndArray.Add(m_cEnd);

	return true;
}

float CPathDoc::ComDistance(CPoint x,CPoint y)
//计算距离
{
	float dx=(float)(y.x - x.x);
	float dy=(float)(y.y - x.y);
	float dis=(float)sqrt(dx*dx+dy*dy);
	return dis;
}

CVector CPathDoc::ComAngle(CPoint now,CPoint end)
//角度方向 now-->end
{
	CVector t;
	if(now.y==end.y && now.x<end.x)
	{
		t.SetX(1);//cos
		t.SetY(0);//sin
		return t;
	}
	if(now.y==end.y && now.x>end.x)
	{
		t.SetX(-1);//
		t.SetY(0);
		return t;
	}
	if(now.x==end.x && now.y>end.y)
	{
		t.SetX(0);//
		t.SetY(-1);
		return t;
	}
	if(now.x==end.x && now.y<end.y)
	{
		t.SetX(0);//
		t.SetY(1);
		return t;
	}
	//第四象限
	if(now.x<end.x && now.y>end.y)
	{
		float dis=ComDistance(now,end);
		float cos=abs(end.x-now.x)/dis;
		float sin=abs(end.y-now.y)/dis;
		t.SetX(cos);//
		t.SetY(-sin);
		return t;
	}
	//第三象限
	if(now.x>end.x && now.y>end.y)
	{
		float dis=ComDistance(now,end);
		float cos=abs(end.x-now.x)/dis;
		float sin=abs(end.y-now.y)/dis;
		t.SetX(-cos);//
		t.SetY(-sin);
		return t;
	}
	//第二象限
	if(now.x>end.x && now.y<end.y)
	{
		float dis=ComDistance(now,end);
		float cos=abs(end.x-now.x)/dis;
		float sin=abs(end.y-now.y)/dis;
		t.SetX(-cos);//
		t.SetY(sin);
		return t;
	}
	//第一象限
	if(now.x<end.x && now.y<end.y)
	{
		float dis=ComDistance(now,end);
		float cos=abs(end.x-now.x)/dis;
		float sin=abs(end.y-now.y)/dis;
		t.SetX(cos);
		t.SetY(sin);
		return t;
	}

	return CVector(0,0);
}

CVector CPathDoc::ComAttract(CPoint robot,CPoint goal, CVector v)
//goal是相对目标节点，robot是当前节点。向量方向 robot->goal
//v向量是力的方向，用来计算返回的向量 t 存储的X，Y轴的相应分力
{
	float dis=ComDistance(robot,goal);
	CVector t;
	t.SetX(K_ATTRACT*dis*v.GetX());//（系数*距离）*cos（X轴分量）
	t.SetY(K_ATTRACT*dis*v.GetY());//（系数*距离）*cos（Y轴分量）

	return t;
}

CVector CPathDoc::ComRepulsion(CPoint now,CPoint obs,CPoint goal,CVector vobs,CVector vgoal)
//now是当前节点，obs 是障碍节点，方向 obs->now
//vobs,vgoal 存储的是 cos,sin 的值,作为方向正余玄用
{	
	CVector t;
	float Rat,rat,Rre,rre;
	Rat=(goal.x-now.x)*(goal.x-now.x)+(goal.y-now.y)*(goal.y-now.y);
	rat=(float)sqrt(Rat);//与目标点距离
	Rre=(obs.x-now.x)*(obs.x-now.x)+(obs.y-now.y)*(obs.y-now.y);
	rre=(float)sqrt(Rre);//与障碍点距离

	CVector _obs,_goal;
	_obs.SetX(K_REPULSION*(1/rre-1/K_DISTANCE)*1/Rre*Rat*vobs.GetX());
	_obs.SetY(K_REPULSION*(1/rre-1/K_DISTANCE)*1/Rre*Rat*vobs.GetY());
	_goal.SetX(K_REPULSION*(1/rre-1/K_DISTANCE)*(1/rre-1/K_DISTANCE)*rat*vgoal.GetX());
	_goal.SetY(K_REPULSION*(1/rre-1/K_DISTANCE)*(1/rre-1/K_DISTANCE)*rat*vgoal.GetY());
	
	t=_obs+_goal;

	return t;
}

void CPathDoc::MoveToNext(CVector v)
//v 里储存当前节点的合力,可以计算出 cos ,sin
{
	CPoint pt=m_cEnd;
	//pt.x=pt.x*MAP_GRIDSIZE+4;
	//pt.y=pt.y*MAP_GRIDSIZE+4;

	if(ComDistance(m_cNow,pt)<=6)
	{
		m_cNow=pt;
		return;
	}
	float x=v.GetX();
	float y=v.GetY();
	float d=(float)sqrt(x*x+y*y);
	m_cNow.x=m_cNow.x+K_MOVE*x/d;//K_MOVE 移动步长
	m_cNow.y=m_cNow.y+K_MOVE*y/d;
}

bool CPathDoc::IsArrivalGoal()
{
	if(abs(m_cNow.x-m_cEnd.x)<3 && abs(m_cNow.y-m_cEnd.y)<3)
		return true;
	else return false;
}
//**********

void CPathDoc::MessageBox(CString title, CString caption, UINT type)
{
	AfxGetMainWnd()->MessageBox(title, caption, type);
}

//***** 设置中间牵引点 ******
void CPathDoc::CalculateLine1(CPoint now,CPoint end)
//水平或垂直的直线不在计算里面
{
	k1 = (now.y - end.y)/(now.x - end.x);
	b1 = now.y - k1*now.x;
}

void CPathDoc::CheckAlongLine1(float k1,float b1,CPoint now,CPoint end,CPoint &middle)
{
	bool flag=false;
	if(end.x > now.x)
		flag=true;
	for(int ii=1;ii<20;ii++)
	{
		float px;
		if(flag)
			px = now.x + 6*ii;
		else
			px = now.x - 6*ii;
		float py = k1*px + b1;
		int i=px/MAP_GRIDSIZE;
		int j=py/MAP_GRIDSIZE;
		if(m_cBoard[i][j]==1)
		{
			middle.x = px + 8;
			middle.y = k1*middle.x + b1;
			return;
		}
	}
}

void CPathDoc::CalculateLine2(CPoint middle,float k1)
{
	k2 = 1/k1;
	b2 = middle.y - k2*middle.x;
}

void CPathDoc::CheckAlongLine2(float k2,float b2,CPoint middle,CPoint end,CPoint &newend)
{
	CPoint pt1,pt2;
	for(int ii=1;ii<40;ii++)
	{
		float px = middle.x + 6*ii;
		float py = k2*px + b2;
		int i=px/MAP_GRIDSIZE;
		int j=py/MAP_GRIDSIZE;
		for(int jj=0;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)
		{
			px=px+6;
			py=k2*px+b2;
			pt1 = CPoint(px,py);
			//pt1 = CPoint(px+6,k2*(px+6)+b2);
			ii = 40;
		}
	}

	for(ii=1;ii<40;ii++)
	{
		float px = middle.x - 6*ii;
		float py = k2*px + b2;
		int i=px/MAP_GRIDSIZE;
		int j=py/MAP_GRIDSIZE;
		for(int jj=0;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)
		{
			px=px-6;
			py=k2*px+b2;
			pt2 = CPoint(px,py);
			//pt2 = CPoint(px-6,k2*(px+6)+b2);
			ii = 40;
		}
	}

	//计算两个点与中间点的距离，取小的
	float ds1=ComDistance(pt1,middle);
	float ds2=ComDistance(pt2,middle);
	if(ds1<=ds2)
	{
		newend=pt1;
		return;
	}
	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;
12 下一页
⌨️ 快捷键说明

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