pathview.cpp

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

// PathView.cpp : implementation of the CPathView class
//

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

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

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

#define  ci(x,y)  x*MAP_BOARDY+y

/////////////////////////////////////////////////////////////////////////////
// CPathView

static COLORREF crBrushes [] = {
	RGB(255,255,255),
	RGB(000,000,000),
	RGB(255,000,000),
	RGB(000,255,000),
};

IMPLEMENT_DYNCREATE(CPathView, CView)

BEGIN_MESSAGE_MAP(CPathView, CView)
	//{{AFX_MSG_MAP(CPathView)
	ON_WM_LBUTTONDOWN()
	ON_WM_MOUSEMOVE()
	ON_WM_LBUTTONUP()
	ON_WM_TIMER()
	ON_WM_ERASEBKGND()
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)

	ON_COMMAND_RANGE(ID_COLOR0, ID_COLOR3, OnBrushType)
	ON_UPDATE_COMMAND_UI_RANGE(ID_COLOR0, ID_COLOR3, OnUpdateUIBrushType)
	ON_COMMAND(ID_PATHSEARCH,OnPathSearch)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CPathView construction/destruction

CPathView::CPathView()
{
	// TODO: add construction code here
	m_bDragging = false;
	m_uBrushType = 0;
	flagUp=flagDown=flagLeft=flagRight=true;
}

CPathView::~CPathView()
{
}

BOOL CPathView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CPathView drawing

void CPathView::OnDraw(CDC* pDC)
{
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here

	CRect rect;
	GetClientRect(rect);

	CDC memdc;
	CBitmap bmp, *oldbmp;
	CPen *oldpen, pen(PS_SOLID, 1, RGB(255,0,255));

	memdc.CreateCompatibleDC(GetDC());
	bmp.CreateDiscardableBitmap(pDC, rect.Width(), rect.Height());
	oldbmp = memdc.SelectObject(&bmp);
	oldpen = memdc.SelectObject(&pen);
	
	char *board = pDoc->GetBoard();
	
	memdc.FillSolidRect(rect, RGB(255,255,255));//填充屏幕为白色

	//界面坐标显示
	memdc.MoveTo(CPoint(20,40));
	memdc.LineTo(CPoint(80,40));
	memdc.MoveTo(CPoint(72,36));
	memdc.LineTo(CPoint(80,40));
	memdc.MoveTo(CPoint(72,45));
	memdc.LineTo(CPoint(80,40));
	memdc.TextOut(78,42,"x");
	memdc.MoveTo(CPoint(50,70));
	memdc.LineTo(CPoint(50,10));
	memdc.MoveTo(CPoint(46,62));
	memdc.LineTo(CPoint(50,70));
	memdc.MoveTo(CPoint(54,62));
	memdc.LineTo(CPoint(50,70));
	memdc.TextOut(56,60,"y");

	int i;
	int dx = rect.Width();
	int dy = rect.Height();

	// Cycle through the board and draw any squares

	for (i=0; i<MAP_BOARDX; i++) {
		for (int j=0; j<MAP_BOARDY; j++) {
			if (board[ci(i,j)] != 0) {
				DrawRectangle(i, j, &memdc, crBrushes[board[ci(i,j)]]);
			}
		}
	}

	// Draw start, end and debug points
	if (m_cStart.x != -1) DrawRectangle(m_cStart.x, m_cStart.y, &memdc, crBrushes[2]);
	//绘制起始点
	if (m_cEnd.x != -1)   DrawRectangle(m_cEnd.x, m_cEnd.y, &memdc, crBrushes[3]);

	//for (i=0; i<dx; i+=8) {
	//	for (int j=0;j<dy;j+=8) {
	//		memdc.SetPixel(i, j, RGB(192, 192, 192));
	//	}
	//}//绘制整个平面

	if (pDoc->DrawRoute()) DrawRoute(&memdc);

	pDC->BitBlt(0,0,rect.Width(), rect.Height(), &memdc, 0, 0, SRCCOPY);

	memdc.SelectObject(oldpen);
	memdc.SelectObject(oldbmp);
}

/////////////////////////////////////////////////////////////////////////////
// CPathView printing

BOOL CPathView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CPathView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CPathView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CPathView diagnostics

#ifdef _DEBUG
void CPathView::AssertValid() const
{
	CView::AssertValid();
}

void CPathView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CPathDoc* CPathView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CPathDoc)));
	return (CPathDoc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CPathView message handlers

//***** 绘制矩形块 *****
void CPathView::DrawRectangle(int x, int y, CDC *pDC, COLORREF cr)
{
	CBrush br(RGB(0,0,0));
	CRect rect(x*MAP_GRIDSIZE, y*MAP_GRIDSIZE, 
			   x*MAP_GRIDSIZE+MAP_GRIDSIZE+1,
			   y*MAP_GRIDSIZE+MAP_GRIDSIZE+1);

	pDC->FillSolidRect(rect, cr);
	pDC->FrameRect(&rect, &br);
}

//***** 设置画刷颜色 *****
void CPathView::OnBrushType(UINT uType)
{
	m_uBrushType = uType - ID_COLOR0;
	
	GetDocument()->SetBrushType(m_uBrushType);
}

//***** 显示所选颜色 *****
void CPathView::OnUpdateUIBrushType(CCmdUI *pCmdUI)
{
	UINT uType = pCmdUI->m_nID;

	pCmdUI->SetCheck((m_uBrushType == uType - ID_COLOR0));
}

//**
void CPathView::CheckRightLeft(CPoint now)
{
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	int px=now.x/MAP_GRIDSIZE;
	int py=now.y/MAP_GRIDSIZE;
	for(int i=1;i<15;i++)
	{
		if(pDoc->m_cBoard[px+i][py])
			flagRight=false;
		if(pDoc->m_cBoard[px-i][py])
			flagLeft=false;
	}
}

void CPathView::CheckUpDown(CPoint now)
{
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	int px=now.x/MAP_GRIDSIZE;
	int py=now.y/MAP_GRIDSIZE;
	for(int i=1;i<15;i++)
	{
		if(pDoc->m_cBoard[px][py+i])
			flagUp=false;
		if(pDoc->m_cBoard[px][py-i])
			flagDown=false;
	}
}

//***** 寻找路径 *****
void CPathView::OnPathSearch()
{
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	if(pDoc->GetMapInfo())
		SetTimer(1,100,NULL);
	m_iCount=0;

}

void CPathView::OnTimer(UINT nIDEvent) 
{
	// TODO: Add your message handler code here and/or call default
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	char *board = pDoc->GetBoard();

		CPoint start,end,now;
		pDoc->MyGetStartEnd(start,end);//获得起始，终止节点
		pDoc->GetNowPoint(now);//获得当前节点
		m_cNow = now;

		//******问题的解决*******
		if(m_iCount==0)
			m_cBefore=now;
		m_iCount++;
		//***********************

		if(pDoc->m_cEndArray.GetSize()!=0)
		{
			CVector sum,sum_att,sum_rep,t,temp;
		
			t=pDoc->ComAngle(now,end);//计算当前节点与终点的引力方向
			sum_att=pDoc->ComAttract(now,end,t);//计算引力在坐标轴的分量
			int ii;
			for(ii=0;ii<pDoc->m_cObsArray.GetSize();ii++)
			{
				if(pDoc->ComDistance(now,pDoc->m_cObsArray[ii])<K_DISTANCE)
				{
					temp=pDoc->ComAngle(pDoc->m_cObsArray[ii],now);
					sum_rep+=pDoc->ComRepulsion(now,pDoc->m_cObsArray[ii],end,temp,t);
				}
			}
			sum=sum_att+sum_rep;

			pDoc->MoveToNext(sum);
			pDoc->GetNowPoint(m_cNow);
			pDoc->m_cPathArray.Add(m_cNow);

			pDoc->SetDrawRoute();
			pDoc->UpdateAllViews(NULL);

			if(pDoc->IsArrivalGoal())
			{
				int i=pDoc->m_cEndArray.GetSize();
				pDoc->m_cEndArray.RemoveAt(i-1);
				if(i>1)
					pDoc->SetNewEndPoint(pDoc->m_cEndArray[i-2]);
			}
			
			//****** 问题的解决 *****
			//检测当前点周围的信息，设置中间目标节点
			if(m_iCount>7)
			{
				m_iCount=0;
				if(pDoc->ComDistance(m_cNow,m_cBefore)<6)
				{
					//pDoc->MessageBox("陷入局部稳定区域!","APF",MB_ICONINFORMATION);
					float absDX=abs(now.x-end.x);
					float absDY=abs(now.y-end.y);
					KillTimer(1);
					CPoint newend;
					/*if(absDY<10)
					{
						pDoc->ResetAllFlag();
						pDoc->CheckUpDown(now);
						pDoc->GetUDEndPoint(now,end,newend);
						pDoc->m_cEndArray.Add(newend);
						pDoc->SetNewEndPoint(newend);
						SetTimer(1,100,NULL);
						return;
					}
					if(absDX<10)
					{
						pDoc->ResetAllFlag();
						pDoc->CheckRightLeft(now);
						pDoc->GetRLEndPoint(now,end,newend);
						pDoc->m_cEndArray.Add(newend);
						pDoc->SetNewEndPoint(newend);
						SetTimer(1,100,NULL);
						return;
					}*/
					//KillTimer(1);
					//pDoc->MessageBox("啊，代码测试中!","APF",MB_ICONINFORMATION);
					pDoc->CalculateLine1(now,end);
					pDoc->MyCalculateLine2(now,pDoc->k1);
					pDoc->ResetAllFlag2();
					pDoc->CheckAlongLine(pDoc->k3,pDoc->b3,now);

					CPoint newmiddle;
					pDoc->CheckAlongLine1(pDoc->k1,pDoc->b1,now,end,newmiddle);
					pDoc->CalculateLine2(newmiddle,pDoc->k1);			
						
					pDoc->CheckAlongLine2(pDoc->k2,pDoc->b2,newmiddle,end,newend);
					pDoc->m_cEndArray.Add(newend);
					pDoc->SetNewEndPoint(newend);
					SetTimer(1,150,NULL);
					return;
					/*{
						//**********
						bool flag;
						if(end.y > now.y)
							flag=true;
						else
							flag=false;
						CPoint middle;
						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 + 6*ii;
							float px = now.x;
							int i=px/MAP_GRIDSIZE;
							int j=py/MAP_GRIDSIZE;
							if(pDoc->m_cBoard[i][j]==1)
							{
								if(flag)
									middle.y = py + 4;
								else
									middle.y = py - 4;
								middle.x = px;
								ii=22;
							}
						}
						//CheckUpDown();
						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(pDoc->m_cBoard[i][j+jj])
									jj=5;
								if(pDoc->m_cBoard[i][j-jj])
									jj=5;