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),
	RGB(192,192,192)
};

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;
	pDoc->m_cAddArray.RemoveAll();

}

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);
				}
			}

			//for(ii=0;ii<pDoc->m_cAddArray.GetSize();ii++)
			//{
			//	if(pDoc->ComDistance(now,pDoc->m_cAddArray[ii])<K_DISTANCE)
			//	{
			//		temp=pDoc->ComAngle(pDoc->m_cAddArray[ii],now);
			//		sum_rep+=pDoc->ComRepulsion(now,pDoc->m_cAddArray[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<absDX)
					{
						pDoc->ResetAllFlag();
						pDoc->CheckUpDown(now);
						pDoc->GetUDEndPoint(now,end,newend);
						pDoc->m_cEndArray.Add(newend);
						pDoc->SetNewEndPoint(newend);
						SetTimer(1,100,NULL);
						return;
					}
					else
					{
						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);
				}
			}
		}
		else
		{
			KillTimer(1);
			//pDoc->MessageBox("可以生成避碰路径!","APF",MB_ICONINFORMATION);
		}

	CView::OnTimer(nIDEvent);
}

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

	
	pDoc->CheckCurrentPoint(now);//设置相应的标志
	CPoint temp;
	temp = pDoc->GetNextPoint(now);//计算下一个要移动到的节点
	m_cMiddle = temp;
	temp.x = temp.x * MAP_GRIDSIZE;
	temp.y = temp.y * MAP_GRIDSIZE;	
	m_cNow = temp;
	pDoc->m_cPathArray.Add(m_cNow);
}

//***** 绘制作用区间 *****
void CPathView::DrawCircle(int x, int y, CDC *pDC, COLORREF cr,int radius)
{
	CBrush br(cr), *oldbrush;
	CPen pen(PS_SOLID, 0, RGB(0,0,0)), *oldpen;
	CRect rect(x-radius, y-radius, 
			   x+radius,
			   y+radius);

	oldbrush = pDC->SelectObject(&br);
	pDC->SetROP2(R2_NOT);
	pDC->SetMapMode(TRANSPARENT);
	pDC->SelectStockObject(NULL_BRUSH);	
	oldpen   = pDC->SelectObject(&pen);

	pDC->Ellipse(rect);

	pDC->SelectObject(oldbrush);
	pDC->SelectObject(oldpen);
}

//***** 绘制路径 *****
void CPathView::DrawRoute(CDC *pDC)
{
	CPathDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	CPen pen(PS_SOLID,1,RGB(0,0,0)),*oldpen;

	oldpen = pDC->SelectObject(&pen);
		
	int x,y;
	
	CPoint pt;

	for(int i=0;i<pDoc->m_cPathArray.GetSize ()-1 ;i++)
	{
		if(i==0)
		{
			pt.x=pDoc->m_cPathArray[i].x;
			pt.y=pDoc->m_cPathArray[i].y;
			DrawCircle(pt.x,pt.y,pDC,RGB(0,0,0),K_DISTANCE);
		}
		else
		{
			DrawCircle(pt.x,pt.y,pDC,RGB(0,0,0),K_DISTANCE);
			pDC->MoveTo(pt);
			x = pDoc->m_cPathArray[i].x;
			y = pDoc->m_cPathArray[i].y;
			DrawCircle(x,y,pDC,RGB(0,0,0),K_DISTANCE);
			pt.x=x;
			pt.y=y;
			pDC->LineTo(pt);
		}
	}
	pDC->SelectObject(oldpen);
}

//***** 操作鼠标 *****
void CPathView::MouseToPoint(CPoint point, UINT brush)
{
	CClientDC dc(this);

	int px = point.x/8, py = point.y/8;//取整

	CPoint round(px*MAP_GRIDSIZE, py*MAP_GRIDSIZE),temp;

	GetDocument()->GetStartEnd(m_cStart, m_cEnd);

	// If start or end
	if (brush == 2) {
		//开始节点
		temp.x = m_cStart.x * MAP_GRIDSIZE;
		temp.y = m_cStart.y * MAP_GRIDSIZE;

		m_cStart.x = px;
		m_cStart.y = py;

		InvalidateRect(CRect(round,CSize(MAP_GRIDSIZE+1,MAP_GRIDSIZE+1)), false);
		InvalidateRect(CRect(temp,CSize(MAP_GRIDSIZE+1,MAP_GRIDSIZE+1)), false);
	} else if (brush == 3) {
		//目标节点
		temp.x = m_cEnd.x * MAP_GRIDSIZE;
		temp.y = m_cEnd.y * MAP_GRIDSIZE;

		m_cEnd.x = px;
		m_cEnd.y = py;
		
		InvalidateRect(CRect(round,CSize(MAP_GRIDSIZE+1,MAP_GRIDSIZE+1)), false);
		InvalidateRect(CRect(temp,CSize(MAP_GRIDSIZE+1,MAP_GRIDSIZE+1)), false);
	} else {
		char *board = GetDocument()->GetBoard();
		board[ci(px,py)] = brush;// 0 或 1
		InvalidateRect(CRect(round,CSize(MAP_GRIDSIZE+1,MAP_GRIDSIZE+1)), false);
	}

	GetDocument()->SetStartEnd(m_cStart, m_cEnd);
}

void CPathView::OnLButtonDown(UINT nFlags, CPoint point) 
//根据所选画刷颜色设置界面矩形块
{
	// TODO: Add your message handler code here and/or call default
	MouseToPoint(point, m_uBrushType);
	if (m_uBrushType < 2) m_bDragging = true;//设置或清除障碍，允许拖动

	CView::OnLButtonDown(nFlags, point);
}

void CPathView::OnMouseMove(UINT nFlags, CPoint point) 
{
	// TODO: Add your message handler code here and/or call default
	if (m_bDragging) {
		MouseToPoint(point, m_uBrushType);
	}

	CView::OnMouseMove(nFlags, point);
}

void CPathView::OnLButtonUp(UINT nFlags, CPoint point) 
{
	// TODO: Add your message handler code here and/or call default
	m_bDragging = false;

	CView::OnLButtonUp(nFlags, point);
}

BOOL CPathView::OnEraseBkgnd(CDC* pDC) 
{
	// TODO: Add your message handler code here and/or call default
	return false;
	return CView::OnEraseBkgnd(pDC);
}

void CPathView::OnInitialUpdate() 
{
	CView::OnInitialUpdate();
	
	// TODO: Add your specialized code here and/or call the base class
	GetDocument()->GetStartEnd(m_cStart, m_cEnd);
	m_cNow=m_cStart;
}