simulatorview.cpp

基于vc 的环境下机器人自主避障的算法 图形处理 具有载物功能

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// simulatorView.cpp : implementation of the CSimulatorView class
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "simulator.h"

#include "simulatorDoc.h"
#include "simulatorView.h"
#include "MainFrm.h"

#include "RandomGenerator.h"
#include "NormalGenerator.h"
#include "MapView.h"
#include "MapPath.h"
#include "const.h"
#include "robotScout.h"
#include "robotLabmate.h"
#include "robotNonHolonomic.h"
#include "robotHolonomic.h"
#include "robotForage.h"
#include "DlgConstants.h"
#include "DlgOrientation.h"
#include "obstaclecircle.h"
#include "obstaclerect.h"
#include "obstaclePolygon.h"
#include "box.h"
#include "boxcircle.h"
#include "boxrect.h"
#include "boxpolygon.h"
#include "polygon.h"
#include <math.h>
#include "MemDC.h"


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

CDlgConstants *dlgConsts;
CPolygon *polygon;

/////////////////////////////////////////////////////////////////////////////
// CSimulatorView

IMPLEMENT_DYNCREATE(CSimulatorView, CScrollView)

BEGIN_MESSAGE_MAP(CSimulatorView, CScrollView)
	ON_MESSAGE(WM_DATAMSG, OnDataMsg)
	ON_MESSAGE(WM_CONTROLMSG, OnControlMsg)
	ON_WM_ERASEBKGND()

	//{{AFX_MSG_MAP(CSimulatorView)
	ON_WM_LBUTTONDBLCLK()
	ON_COMMAND(ID_ROBOT_INFO, OnRobotInfo)
	ON_WM_LBUTTONDOWN()
	ON_COMMAND(ID_CREATE_MAP, OnCreateMap)
	ON_WM_KEYDOWN()
	ON_WM_HSCROLL()
	ON_WM_VSCROLL()
	ON_COMMAND(ID_OPEN_PCONST, OnOpenPconst)
	ON_WM_RBUTTONUP()
	ON_WM_MOUSEMOVE()
	ON_COMMAND(ID_SETORIENTATION, OnSetOrientation)
	ON_COMMAND(ID_SETGOAL, OnSetGoal)
	ON_COMMAND(ID_CREATEHOLONOMIC, OnCreateHolonomic)
	ON_COMMAND(ID_CREATENONHOLONOMIC, OnCreateNonHolonomic)
	ON_COMMAND(ID_SETSTATUS, OnSetStatus)
	ON_COMMAND(ID_DELETEROBOT, OnDeleteRobot)
	//}}AFX_MSG_MAP
	ON_WM_SIZE()
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CSimulatorView construction/destruction

CSimulatorView::CSimulatorView() : CScrollView()
{
	// Creating the info dialog
	m_info.Create(IDD_INFO_DIALOG);
	m_info.m_robotList.InsertColumn(0,"Robot");
	m_info.m_robotList.InsertColumn(1," x ");
	m_info.m_robotList.InsertColumn(2," y ");
	m_info.m_robotList.InsertColumn(3," theta ");
	m_info.m_robotList.InsertColumn(4," info ");
	m_info.m_robotList.SetColumnWidth(0, 70);
	m_info.m_robotList.SetColumnWidth(1, 70);
	m_info.m_robotList.SetColumnWidth(2, 70);
	m_info.m_robotList.SetColumnWidth(3, 70);
	m_info.m_robotList.SetColumnWidth(4, 500);

	// Creating tracker
	m_tracker.m_nStyle = CRectTracker::dottedLine + CRectTracker::resizeOutside; 
	m_trackerArray.RemoveAll();

	// Creating context menu
	m_contextMenu = new CMenu();
	m_contextMenu->LoadMenu(IDR_POPUP);

	// Auxilizry polygon for creting obstacles and boxes
	polygon = new CPolygon();
}

CSimulatorView::~CSimulatorView()
{
}

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

	return CScrollView::PreCreateWindow(cs);
}

void CSimulatorView::OnInitialUpdate()
{
	CScrollView::OnInitialUpdate();

	CSimulatorDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	// initialing scroll
	CSize sizeTotal;
	sizeTotal.cx = sizeTotal.cy = 5000;
    CSize sizePage(sizeTotal.cx / 10, sizeTotal.cy / 10);
    CSize sizeLine(sizeTotal.cx / 500, sizeTotal.cy / 500);
	SetScrollSizes(MM_TEXT, sizeTotal, sizePage, sizeLine);

	ScrollToPosition( CPoint(2630-500, 2200-300) );

	// initialzing the doc member the stores a pointer to the view
	pDoc->m_view = this;
}

/////////////////////////////////////////////////////////////////////////////
// CSimulatorView drawing

BOOL CSimulatorView::OnEraseBkgnd(CDC* pDC) 
{
	return FALSE;
}

void CSimulatorView::OnSize(UINT nType, int cx, int cy)
{
	CScrollView::OnSize(nType, cx, cy);

	CreateViewBitmap(); 
}

void CSimulatorView::CreateViewBitmap() 
{ 
	/* This function creates the bitmap used for drawing */ 

	// First destroy the old bitmap 
	HBITMAP hBmp = (HBITMAP)m_bmpView.GetSafeHandle(); 

	if (hBmp != NULL) 
		m_bmpView.DeleteObject(); 

	CClientDC dc(this); 

	CRect rcClient; 
	GetClientRect(&rcClient); 

	m_bmpView.CreateCompatibleBitmap(&dc, rcClient.Width(), rcClient.Height()); 
} 

void CSimulatorView::OnDraw(CDC* dc)
{
	int i;

	CSimulatorDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	
	CMemDC	pDC(dc, &m_bmpView);

	pDC->SelectObject(&BLACK);
	polygon->Draw(pDC);

//	CSingleLock sLock(&(pDoc->m_mutex));
//	sLock.Lock();


	pDC->SelectObject(&BLUE_PEN);
	for (i=0; i<pDoc->m_robots.GetSize(); i++) {
		// Drawing the planned path for the Leader
		if ( ( (pDoc->m_robots[i]->m_status == LEAD) || (pDoc->m_robots[i]->m_status == FPPATH) ) && (pDoc->m_robots[i]->m_localMap->m_isComputed)){
			pDC->Polyline(pDoc->m_robots[i]->m_localMap->m_path.GetData(), pDoc->m_robots[i]->m_localMap->m_path.GetSize());
		}
		// Drawing the robots' trajectoris
		if (pDoc->m_drawPath) {
			pDC->SelectObject(&BLACK);
			pDC->Polyline(pDoc->m_robots[i]->m_path.GetData(), pDoc->m_robots[i]->m_path.GetSize());
		}

	}

	// Drawing goal
	if(pDoc->m_globalMap->m_goalReal.x != -1)
		pDC->TextOut(pDoc->m_globalMap->m_goalReal.x-4, pDoc->m_globalMap->m_goalReal.y-8, "X", 1);
	
	// Drawing robots
	for(i=0; i<pDoc->m_robots.GetSize(); i++)
		pDoc->m_robots[i]->Draw(pDC);

	// drawing box and box trajectory
	if (pDoc->m_box->m_exist) {
		pDoc->m_box->Draw(pDC);
		if (pDoc->m_drawPath){
			pDC->SelectObject(&RED_PEN);
			pDC->Polyline(pDoc->m_box->m_path.GetData(), pDoc->m_box->m_path.GetSize());
		}
	}

	// drawing obstacles
	for(i=0; i<pDoc->m_globalMap->m_obstacles.GetSize(); i++)
		pDoc->m_globalMap->m_obstacles[i]->Draw(pDC, pDoc->m_drawErased);

	// drawing sensor range
	if (pDoc->m_drawRange) {
		pDC->SelectStockObject(NULL_BRUSH);
		pDC->SelectObject(&BLACK);
		for (i=0; i<pDoc->m_robots.GetSize(); i++) {
			CRect sensorRect = CRect(round(pDoc->m_robots[i]->m_x - pDoc->m_robots[i]->m_sensorRange), round(pDoc->m_robots[i]->m_y - pDoc->m_robots[i]->m_sensorRange), 
									 round(pDoc->m_robots[i]->m_x + pDoc->m_robots[i]->m_sensorRange), round(pDoc->m_robots[i]->m_y + pDoc->m_robots[i]->m_sensorRange) );
			pDC->Ellipse(sensorRect);
		}
	}

	// updating status bar with simulation time and number of robots
	CString str;
	CMainFrame* pFrame = (CMainFrame*) AfxGetApp()->m_pMainWnd;
	CStatusBar* pStatus = &pFrame->m_wndStatusBar;
	if (pStatus){
		str.Format("Time: %12.4f", pDoc->m_simTime);
		pStatus->SetPaneText(1,str);
		str.Format("Robots: %4d", pDoc->m_robots.GetSize());
		pStatus->SetPaneText(2,str);
	}

//	sLock.Unlock();

	// drawing tracker (for object selection)
	for (i=0; i<m_trackerArray.GetSize(); i++)
		m_trackerArray[i].Draw(pDC);

	// Foraging: drawing items and area
	pDC->SelectObject(&YELLOW);
	for(i=0; i< CRobotForage::GetNumItems(); i++){
		CPoint p = CRobotForage::GetItem(i);
		p.x = abs(p.x);
		p.y = abs(p.y);
		pDC->Ellipse(CRect(p.x-4,p.y-4, p.x+4, p.y+4));
	}

	if ( CRobotForage::GetArea() != CSize(0,0) ) {
		pDC->SelectStockObject(NULL_BRUSH);
		pDC->SelectObject(&BLACK);
		pDC->Rectangle(CRect(0,0, CRobotForage::GetArea().cx, CRobotForage::GetArea().cy));
	}

}

/////////////////////////////////////////////////////////////////////////////
// CSimulatorView diagnostics

#ifdef _DEBUG
void CSimulatorView::AssertValid() const
{
	CScrollView::AssertValid();
}

void CSimulatorView::Dump(CDumpContext& dc) const
{
	CScrollView::Dump(dc);
}

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

/////////////////////////////////////////////////////////////////////////////
// CSimulatorView message handlers

//----------------------------------------------------------
// mouse and keybord events
//----------------------------------------------------------

// mouse left button click
void CSimulatorView::OnLButtonDown(UINT nFlags, CPoint point) 
{
	CObstacle *obst ;
	CRobot *robot;
	int i;
	BOOL rectOK;

	CSimulatorDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	
	CClientDC dc(this);
	OnPrepareDC(&dc);

	CPoint *aux;
	aux = new CPoint(point);
	dc.DPtoLP(aux);

	CRectTracker tracker;
	CRect rect;

	rectOK = FALSE;
	// tracker
	if ( (pDoc->m_selectedButton == SELECT) || (pDoc->m_selectedButton == CIRCLEOBST) || (pDoc->m_selectedButton == RECTOBST) || (pDoc->m_selectedButton == CIRCLEBOX) ||(pDoc->m_selectedButton == RECTBOX) )
		if (tracker.TrackRubberBand(this, point, TRUE)){
			rect = tracker.m_rect;
			rect.NormalizeRect();
			dc.DPtoLP(&rect);
			rectOK = ( (rect.Size().cx > 10) && (rect.Size().cy > 10) );
		}

	switch (pDoc->m_selectedButton) {
	
	// adding points to polygons (box or object)
	case POLYGONOBST:
	case POLYGONBOX:

		SetCapture();
		polygon->AddPoint(*aux);

		Invalidate();
		break;

	// creating circle obstacle
	case CIRCLEOBST:
		if (rectOK) {
			rect.SetRect(rect.TopLeft().x,rect.TopLeft().y,rect.BottomRight().x, rect.TopLeft().y + rect.BottomRight().x - rect.TopLeft().x);
			obst = new CObstacleCircle(rect);
			pDoc->AddObstacle(obst);
		}
		break;

	// creating rectangular obstacle
	case RECTOBST:
		if (rectOK) {
			obst = new CObstacleRect(rect);
			pDoc->AddObstacle(obst);
		}
		break;
		
	// creating circle box
	case CIRCLEBOX:
		if (rectOK) {
			rect.SetRect(rect.TopLeft().x,rect.TopLeft().y,rect.BottomRight().x, rect.TopLeft().y + rect.BottomRight().x - rect.TopLeft().x);
			pDoc->m_box = new CBoxCircle(rect);
		}
		break;

	// creating rectangular box
	case RECTBOX: 
		if (rectOK) {
			pDoc->m_box = new CBoxRect(rect);
		}
	break;

	// selecting obstacles or robots
	case SELECT:
			pDoc->m_selectedObst = -1;
			m_trackerArray.RemoveAll();

//			pDoc->m_selectedObst = -1;
//			for (int i=pDoc->m_globalMap->m_obstacles.GetSize()-1; i>=0; i--)
//				if ( !( (pDoc->m_globalMap->m_obstacles[i]->m_rect & rect).IsRectEmpty() ) ) {
//					if (!pDoc->m_globalMap->m_obstacles[i]->m_erased){
//						pDoc->m_selectedObst = i;
//						m_tracker.m_rect = pDoc->m_globalMap->m_obstacles[i]->m_rect;
//						dc.LPtoDP(&(m_tracker.m_rect));
//						break;
//					}
//				}
			// select obstacle by clicking
		if (!rectOK) {
			for (i=pDoc->m_globalMap->m_obstacles.GetSize()-1; i>=0; i--){
				if ( pDoc->m_globalMap->m_obstacles[i]->m_rect.PtInRect(*aux) ) {
					if (!pDoc->m_globalMap->m_obstacles[i]->m_erased){
						pDoc->m_selectedObst = i;
						m_tracker.m_rect = pDoc->m_globalMap->m_obstacles[i]->m_rect;
						dc.LPtoDP(&(m_tracker.m_rect));
						m_trackerArray.Add(m_tracker);
						break;
					}
				}
			}

			pDoc->m_selectedRobot = -1;
			for (i=pDoc->m_robots.GetSize()-1; i>=0; i--){
				if ( pDoc->m_robots[i]->m_rect.PtInRect(*aux) ) {
					pDoc->m_selectedRobot = i;
					m_tracker.m_rect = pDoc->m_robots[i]->m_rect;
					dc.LPtoDP(&(m_tracker.m_rect));
					m_trackerArray.Add(m_tracker);
					break;
				}