findwaydlg.cpp

国内的一个A*算法演示

void CfindwayDlg::OnActivate(UINT nState, CWnd* pWndOther, BOOL bMinimized)
{
	CDialog::OnActivate(nState, pWndOther, bMinimized);

	////如果当前有未画完的多边形，则清除
	//ZeroMemory(&m_CurPolygon,sizeof(ObjPolygon));
	//m_nCurPoint=0;
	//m_nState=0;
	//m_strHint1=_T("  请按住CTRL键，请点击鼠标左键开始画多边形");
	////清除结束
	//DrawPolygon();//重画
	OnBnClickedButton2();
}

void CfindwayDlg::OnRButtonUp(UINT nFlags, CPoint point)
{
	if(m_nState==0)
	{
		if(nFlags==MK_CONTROL)
		{
			m_endPos=point;
		}
		else
		{
			m_startPos=point;
		}
		m_strHint2.Format("  开始点%d,%d,结束点%d,%d",m_startPos.x,m_startPos.y,m_endPos.x,m_endPos.y);
	}
	else
	{
		//如果当前有未画完的多边形，则清除
		ZeroMemory(&m_CurPolygon,sizeof(ObjPolygon));
		m_nCurPoint=0;
		m_nState=0;
		m_strHint1=_T("  请按住CTRL键，请点击鼠标左键开始画多边形");	
	}
	DrawPolygon();//重画
	CDialog::OnRButtonUp(nFlags, point);
}


// 检测直线AB是否有和当前地图某个多边形相交
bool CfindwayDlg::CheckPolygon(POINT a, POINT b)
{
	ObjPolygon *curPoly;
	for(int i=0;i<m_nNumPolygons;i++)
	{
		curPoly=&m_Polygon[i];
		//判断是否与圆相交
		if(inter_SR(a,b,curPoly->Center,curPoly->nRadius))
		{	//如果是，再检测是否和其多边形相交
			for(int j=0;j<curPoly->nNumSides-1;j++)
			{
				if(inter_SS(a,b,curPoly->LocalVertex[j],curPoly->LocalVertex[j+1]))
					return true;
			}
			//至少有3条边
			if(curPoly->nNumSides>=3&&inter_SS(a,b,curPoly->LocalVertex[0],curPoly->LocalVertex[curPoly->nNumSides-1]))
				return true;
		}
	}
	return false;
}

// 求路径
bool CfindwayDlg::Run(ObjPointLen StartObj,POINT EndPos,POINT* pWay,float *lWayLength,int *nWayNum)
{
	*nWayNum=0;
	ObjPointLen temp;//临时点
	ObjPolygon *curPoly;

	list<ObjPointLen> OpenList;//存储当前CURPOINT点的 可用顶点
	OpenList.push_back(StartObj);

	LONG lIndex=-1;
	ObjPointLen CloseList[MAX_CLOSELIST];
	ObjPointLen p;

	CPen *oldpen,newpen;
	oldpen=m_pDC->GetCurrentPen();
	srand( (unsigned)time( NULL ) );

	while (!OpenList.empty())
	{
		//检测是否超时,如果是退出
		//if(GetTickCount()-m_dwStart>=TIMELIMITE)
		//	return false;
		p=*(OpenList.begin());
		OpenList.pop_front();
		//如果和当前使用的所有点有相同，继续下一个,防止循环
		bool flag=false;
		for(int n=0;n<=lIndex;n++)
		{
			if(equal_PP(p.old,CloseList[n].old))
			{
				flag=true;
				break;
			}
		}
		if(flag)
			continue;
		CloseList[++lIndex]=p;
		if(lIndex>=MAX_CLOSELIST-1)
		{
			//AfxMessageBox(_T("CloseList队列溢出！"));
			if(m_DisplayTempWay)
			{
				newpen.DeleteObject();
				m_pDC->SelectObject(oldpen);
			}
			return false;
		}
		//检测是否可以到终点,如果终点在多边形内，p1.p是调整后的终点
		if(!CheckPolygon(p.p,EndPos))
		{
			temp.FatherIndex=lIndex;
			temp.p=EndPos;
			temp.lway =p.lway;
			temp.lway+=dis_PP(p.p,EndPos);
			CloseList[++lIndex]=temp;
            break;
		}
		if(m_DisplayTempWay)
		{
			newpen.DeleteObject();
			newpen.CreatePen(PS_DASH,1,RGB((rand()%6)*40+50,(rand()%6)*40+50,(rand()%6)*40+50));
			m_pDC->SelectObject(&newpen);
			Sleep(400);
		}
		for(int i=0;i<m_nNumPolygons;i++)
		{
			curPoly=&m_Polygon[i];
			for(int j=0;j<curPoly->nNumSides;j++)
			{	
				POINT b=curPoly->LocalVertex[j];
				//如果和当前使用的所有点有相同，继续下一个,防止循环
				bool flag=false;
				for(int n=0;n<=lIndex;n++)
				{
					if(equal_PP(b,CloseList[n].old))
					{
						flag=true;
						break;
					}
				}
				if(flag)
					continue;
				POINT a;
				POINT c;
				int nRound=curPoly->nNumSides;
				a=curPoly->LocalVertex[(j-1+nRound)%nRound];
				c=curPoly->LocalVertex[(j+1+nRound)%nRound];
				POINT d=adjust(a,b,c);
				if(!CheckPolygon(p.p,d))
				{
					if(m_DisplayTempWay)
					{
						m_pDC->MoveTo(p.p);
						m_pDC->LineTo(d);
					}
					temp.lway =p.lway;
					temp.lway+=dis_PP(p.p,d);//存储当前点的路径长度
					temp.lend=dis_PP(EndPos,d);
					temp.l=temp.lway+temp.lend*m_bl;
					temp.old=b;
					temp.p=d;
					temp.FatherIndex=lIndex;
					OpenList.push_back(temp);
				}
			}
		}
		OpenList.sort(compare);
	}
	if(m_DisplayTempWay)
	{
		newpen.DeleteObject();
		m_pDC->SelectObject(oldpen);
	}
	//如果路径终点和结束点不同,则没找到路径,返回距离终点最近的路径
	if(!equal_PP(CloseList[lIndex].p,EndPos))
	{
		LONG li=0;
		for(int i=1;i<lIndex;i++)
		{
			if(CloseList[li].lend>CloseList[i].lend)
				li=i;
		}
		lIndex=li;
		//return false;
	}
	//把当前路径转换到PWAY里
	ObjPointLen *pP;
	LONG ln=lIndex;
	*lWayLength=CloseList[ln].lway;
	while (ln!=-1)
	{
		++*nWayNum;
		pP=&CloseList[ln];
		*pWay=pP->p;
		++pWay;
		ln=pP->FatherIndex;
	}
	return true;
}

inline bool compare(const ObjPointLen &x,const ObjPointLen &y)
{
	return x.l<y.l;
}

void CfindwayDlg::OnFileNew32776()
{
	SetWindowText("FindWay");
	m_nNumPolygons=0;
	//如果当前有未画完的多边形，则清除
	ZeroMemory(&m_CurPolygon,sizeof(ObjPolygon));
	m_nCurPoint=0;
	m_nState=0;
	m_strHint1=_T("  请按住CTRL键，请点击鼠标左键开始画多边形");	
	DrawPolygon();
}
//现实所有调整点
void CfindwayDlg::OnBnClickedButton1()
{
	//画每个点的调整点 ADJUST
	int n=0;
	ObjPolygon *curPoly;
	for(int i=0;i<m_nNumPolygons;i++)
	{
		curPoly=&m_Polygon[i];
		for(int j=0;j<curPoly->nNumSides;j++)
		{	
			POINT b=curPoly->LocalVertex[j];
			POINT a;
			POINT c;
			int nRound=curPoly->nNumSides;
			a=curPoly->LocalVertex[(j-1+nRound)%nRound];
			c=curPoly->LocalVertex[(j+1+nRound)%nRound];
			POINT d=adjust(a,b,c);
			m_pDC->SetPixel(d,RGB(250,0,0));
			m_pDC->SetPixel(d.x+1,d.y,RGB(250,0,0));
			m_pDC->SetPixel(d.x,d.y+1,RGB(250,0,0));
			m_pDC->SetPixel(d.x+1,d.y+1,RGB(250,0,0));
		}
	}
}

//清除重画
void CfindwayDlg::OnBnClickedButton2()
{
	//如果当前有未画完的多边形，则清除
	ZeroMemory(&m_CurPolygon,sizeof(ObjPolygon));
	m_nCurPoint=0;
	m_nState=0;
	m_strHint1=_T("  请按住CTRL键，请点击鼠标左键开始画多边形");
	//清除结束
	DrawPolygon();
}

// 检测点P是否位于某个多边形内
ObjPolygon *CfindwayDlg::CheckPointInsidePolygon(POINT p)
{
	ObjPolygon *curPoly;
	for(int i=0;i<m_nNumPolygons;i++)
	{
		curPoly=&m_Polygon[i];
		//判断是否与圆相交
		if(insideRound(curPoly->Center,curPoly->nRadius,p))
			//判断是否在当前多边形内，如果是则返回此多边形指针
			if(insidePolygon(curPoly->nNumSides,curPoly->LocalVertex,p))
				return curPoly;
	}
	return NULL;
}

// 调整终点位置,很费时啊
POINT CfindwayDlg::AdjustEndPos(ObjPolygon *curPoly)
{
	list<ObjPointLen> pList;
	ObjPointLen temp;
	//ObjPolygon *curPoly=NULL;
	POINT p=m_endPos;
	//curPoly= CheckPointInsidePolygon(p);
	//返回空值，说明终点不在任何一个多边形内
	if(curPoly==NULL)
	{
		//AfxMessageBox("终点不在任何一个多边形内");
		return p;
	}
	POINT d;//垂足
	POINT e;//调整点
	POINT a,b;
	POINT t=curPoly->LocalVertex[curPoly->nNumSides];
	curPoly->LocalVertex[curPoly->nNumSides]=curPoly->LocalVertex[0];
	//终点在多边形内则，求终点的调整点
	for(int j=0;j<curPoly->nNumSides;j++)
	{
		a=curPoly->LocalVertex[j];
		b=curPoly->LocalVertex[j+1];
		d=dis_CZ(p,a,b);
		if(equal_PP(d,p))//如果垂足和点p相同，则说明P在直线AB上
		{	
			POINT p2;
			temp.l=g_nAdjLen;
			if(a.x==b.x)//平行于Y轴
			{
				p2.y=p.y;
				p2.x=a.x-g_nAdjLen;
				temp.p=p2;
				pList.push_back(temp);
				p2.x=a.x+g_nAdjLen;
				temp.p=p2;
				pList.push_back(temp);
			}
			else if(a.y==b.y)
			{
				p2.x=p.x;
				p2.y=a.y-g_nAdjLen;
				temp.p=p2;
				pList.push_back(temp);
				p2.y=a.y+g_nAdjLen;
				temp.p=p2;
				pList.push_back(temp);
			}
			else
			{
				float ax=a.x;
				float ay=a.y;
				float bx=b.x;
				float by=b.y;
				float ba=(ay-by)/(ax-bx);
				ba=-1.0/ba;
				//求点B在直线BD上左右一个单位的两个点M,N的坐标
				float fGen=sqrt(ba*ba+1);
				float fGen1=fGen*-1;
				float mx,my,nx,ny;
				mx=g_nAdjLen/fGen+p.x;
				my=ba*(mx-p.x)+p.y;
				nx=g_nAdjLen/fGen1+p.x;
				ny=ba*(nx-p.x)+p.y;
				POINT m,n;
				m.x=mx;
				m.y=my;
				n.x=nx;
				n.y=ny;
				temp.p=m;
				pList.push_back(temp);
				temp.p=n;
				pList.push_back(temp);
			}
		}
		else //当点P不在直线上时，过该点P与直线AB垂直的直线L，求在直线L上距离d为g_nAdjLen的点
		{
			//如果垂足在线段AB上，应该用ONLINE但精度还是高，所以此处简化，判断D是否在AB所形成的矩形内
			if((min(a.x,b.x)<=d.x&&d.x<=max(a.x,b.x) )||( min(a.y,b.y)<=d.y&&d.y<=max(a.y,b.y)))
			{
				e= LineProlong(p,d,g_nAdjLen);
			}
			else
			{
				if(dis_PP(a.x,a.y,p.x,p.y)>dis_PP(b.x,b.y,p.x,p.y))
					e= LineProlong(p,b,g_nAdjLen);
				else
					e= LineProlong(p,a,g_nAdjLen);
			}
			temp.p=e;
			temp.l=dis_PP(e,p);
			pList.push_back(temp);
		}
	}
	curPoly->LocalVertex[curPoly->nNumSides]=t;

	pList.sort(compare);
	list<ObjPointLen>::iterator iter=pList.begin();
	while(iter!=pList.end())
	{
		if(CheckPointInsidePolygon(iter->p)==NULL)
		{
			for(int i=iter->p.x-1;i<=iter->p.x+1;i++)
				for(int j=iter->p.y-1;j<=iter->p.y+1;j++)
					m_pDC->SetPixel(i,j,RGB(128,0,128));
			return iter->p;
		}
		++iter;
	}
	return m_endPos;
}
//显示寻路过程
void CfindwayDlg::OnSystemDisplaytempway()
{
	UINT flag;
	m_DisplayTempWay=!m_DisplayTempWay;
	if(m_DisplayTempWay)
		flag=MF_CHECKED;
	else
		flag=MF_UNCHECKED;
	GetMenu()->CheckMenuItem(ID_SYSTEM_DISPLAYTEMPWAY,flag);
}