mrpathplanningview.cpp

在建立的仿真环境中

// MRpathPlanningView.cpp : implementation of the CMRpathPlanningView class
//

#include "stdafx.h"
#include "MRpathPlanning.h"

#include "MRpathPlanningDoc.h"
#include "MRpathPlanningView.h"
#include "Math.h"

#define pi 3.1415926535897932384

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

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView

IMPLEMENT_DYNCREATE(CMRpathPlanningView, CView)

BEGIN_MESSAGE_MAP(CMRpathPlanningView, CView)
	//{{AFX_MSG_MAP(CMRpathPlanningView)
	ON_WM_CREATE()
	ON_WM_DESTROY()
	ON_WM_TIMER()
	ON_WM_ERASEBKGND()
	ON_WM_LBUTTONDOWN()
	//}}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)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView construction/destruction

CMRpathPlanningView::CMRpathPlanningView()
{
	// TODO: add construction code here

}

CMRpathPlanningView::~CMRpathPlanningView()
{
}

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

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView drawing

void CMRpathPlanningView::OnDraw(CDC* pDC)
{
	CMRpathPlanningDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here
	CClientDC dc(this);
	CPen penMR(PS_SOLID,1,RGB(255,0,0));
	dc.SelectObject(&penMR);

	dc.Ellipse(Sx-10,Sy-10,Sx+10,Sy+10);
	dc.Ellipse(Gx-10,Gy-10,Gx+10,Gy+10);

//=================================================
	CFile tfile;
    tfile.Open("1.bmp",CFile::modeRead);	
    tDib.Read(&tfile); 
	
	CPoint tpos(1,1);
    CSize tsize;
	tsize=tDib.GetDimensions();
 //   tDib.Draw( pDC,tpos,tsize);

	Binarization(tDib.m_lpImage,dataImg1,tsize.cx,tsize.cy);
    ShowMap(dataImg1,tsize.cx,tsize.cy,pDC);
    ReleaseDC(pDC);

}

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView printing

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

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

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

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView diagnostics

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

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

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

/////////////////////////////////////////////////////////////////////////////
// CMRpathPlanningView message handlers

int CMRpathPlanningView::OnCreate(LPCREATESTRUCT lpCreateStruct) 
{
	if (CView::OnCreate(lpCreateStruct) == -1)
		return -1;
	
	// TODO: Add your specialized creation code here
	SetTimer(1,100,0);
	Sx=300, Sy=300;
	Gx=100, Gy=310;
	MRu=0, MRw=0;
	
	MRx=Sx;
	MRy=Sy;
	MRa=0;

	return 0;
}

void CMRpathPlanningView::OnDestroy() 
{
	CView::OnDestroy();
	
	// TODO: Add your message handler code here
	
}

void CMRpathPlanningView::OnTimer(UINT nIDEvent) 
{
	// TODO: Add your message handler code here and/or call default
	if(1 == nIDEvent)
	{
		MRx=MRx+MRu*2*cos(MRa*pi/180);
   	    MRy=MRy+MRu*2*sin(MRa*pi/180);
	    MRa=MRa+MRw*2;

        if(MRa>180)
        MRa=MRa-360;
        if(MRa<-180)
        MRa=MRa+360;
		
		DrawMR(MRx,MRy,MRa);

		if(Gx>MRx)
		    PLa=180*atan((Gy-MRy)/(Gx-MRx))/pi;
		else if(Gy>=MRy)
			PLa=180+180*atan((Gy-MRy)/(Gx-MRx))/pi;
		else
			PLa=-180+180*atan((Gy-MRy)/(Gx-MRx))/pi;
		Ea=PLa-MRa;

		MRw=0.1*Ea;
		MRu=0.01*sqrt((Gy-MRy)*(Gy-MRy)+(Gx-MRx)*(Gx-MRx));
	}
	CView::OnTimer(nIDEvent);
}

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

void CMRpathPlanningView::DrawMR(float robotX, float robotY, float robotA)
{
	CClientDC dc(this);
	CPen penSsor(PS_SOLID,1,RGB(0,0,255));
	dc.SelectObject(&penSsor);

	float Amrx,Amry;
	int i;
	for(i=0;i<180;i+=1)
	{
		Amrx=robotX+100*cos(pi*(robotA+i-90)/180);
		Amry=robotY+100*sin(pi*(robotA+i-90)/180);
		dc.MoveTo(robotX,robotY);
        dc.LineTo(Amrx,Amry);

		dc.Ellipse(robotX-10,robotY-10,robotX+10,robotY+10);
	}

	CPen penMR(PS_SOLID,1,RGB(255,0,0));
	dc.SelectObject(&penMR);
//	SetLineStyle(0,0,1);
	dc.Ellipse(Gx-10,Gy-10,Gx+10,Gy+10);
}

void CMRpathPlanningView::Binarization(unsigned char *dataIn, unsigned char *dataOut, int Width, int Height)
{
	int i,j,gray;
	for(i=0;i<Width;i++)           
	{
		for(j=0;j<Height;j++)          
		{
			gray=dataIn[fGpsr(i,j,Width,Height)]+dataIn[fGpsg(i,j,Width,Height)]+dataIn[fGpsb(i,j,Width,Height)];
			if(gray>200)
			{
				dataIn[fGpsr(i,j,Width,Height)]=255;
                dataIn[fGpsg(i,j,Width,Height)]=255;
                dataIn[fGpsb(i,j,Width,Height)]=255;
			}
			else
			{
				dataIn[fGpsr(i,j,Width,Height)]=0;
                dataIn[fGpsg(i,j,Width,Height)]=0;
                dataIn[fGpsb(i,j,Width,Height)]=0;
			}
			dataOut[fGpsr(i,j,Width,Height)]=dataIn[fGpsr( i,j, Width,Height)]; 
            dataOut[fGpsg(i,j,Width,Height)]=dataIn[fGpsg( i,j, Width,Height)]; 
            dataOut[fGpsb(i,j,Width,Height)]=dataIn[fGpsb( i,j, Width,Height)]; 
 		}
	}

}

void CMRpathPlanningView::ShowMap(unsigned char *dataIn, int Width, int Height, CDC *pDC)
{
	int i,j;

    for(i=1;i<Width;i++)
	{
	   for(j=0;j<Height;j++)
	   {
            pDC->SetPixel(i+100,j+100,RGB(dataIn[fGpsr(i,j,Width,Height)],
			   	                  dataIn[fGpsg(i,j,Width,Height)],
				                  dataIn[fGpsb(i,j,Width,Height)])) ;
	   }
	 }

}

long CMRpathPlanningView::fGpsr(int x, int y,int Width,int Height)/*GetPixelOffset*/
{
 return((Height-y-1)*Width+x)*3+2;
}

long CMRpathPlanningView::fGpsg(int x, int y,int Width,int Height)/*GetPixelOffset*/
{
 return((Height-y-1)*Width+x)*3+1;
}

long CMRpathPlanningView::fGpsb(int x, int y,int Width,int Height)/*GetPixelOffset*/
{
 return((Height-y-1)*Width+x)*3;
}

void CMRpathPlanningView::OnLButtonDown(UINT nFlags, CPoint point) 
{
	// TODO: Add your message handler code here and/or call default
	CClientDC dc(this);
	CPen pen(PS_SOLID,1,RGB(255,0,0));
	dc.SelectObject(&pen);

	CPoint pnt=point;
	dc.Ellipse(pnt.x-10,pnt.y-10,pnt.x+10,pnt.y+10);
    Gx=pnt.x;
    Gy=pnt.y;
	
	CView::OnLButtonDown(nFlags, point);
}