simulator.cpp

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

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// RobotHolonomic.cpp: implementation of the CRobotHolonomic class.
//
// simulator.cpp : Defines the class behaviors for the application.
//
///////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "simulator.h"

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

#include <math.h>

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

/////////////////////////////////////////////////////////////////////////////
// CSimulatorApp

BEGIN_MESSAGE_MAP(CSimulatorApp, CWinApp)
	//{{AFX_MSG_MAP(CSimulatorApp)
	ON_COMMAND(ID_APP_ABOUT, OnAppAbout)
		// NOTE - the ClassWizard will add and remove mapping macros here.
		//    DO NOT EDIT what you see in these blocks of generated code!
	//}}AFX_MSG_MAP
	// Standard file based document commands
	ON_COMMAND(ID_FILE_NEW, CWinApp::OnFileNew)
	ON_COMMAND(ID_FILE_OPEN, CWinApp::OnFileOpen)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CSimulatorApp construction

CSimulatorApp::CSimulatorApp()
{
	// TODO: add construction code here,
	// Place all significant initialization in InitInstance
}

/////////////////////////////////////////////////////////////////////////////
// The one and only CSimulatorApp object

CSimulatorApp theApp;

// initial gains for the potential and collision functions
extern int Kc_ro = 10000;				// collision K robot-obstacle
extern int Cc_ro = 200;					// collision C robot-obstacle	
extern int Kc_rr = 5000;				// collision K robot-robot
extern int Cc_rr = 140;					// collision C robot-robot
extern int Kc_rb = 5000;				// collision K robot-box
extern int Cc_rb = 140;					// collision C robot-box
extern int Kc_bo = 10000;				// collision K box-obstacle
extern int Cc_bo = 200;					// collision C box-obstacle

extern int Ka_rb = 100;					// Potential: attraction robot-box
extern int Ka_rg = 10;					// Potential: attraction robot-goal
extern int Kr_rr = 2000;				// Potential: repulsion robot-robot 
extern int Kr_ro = 0;					// Potential: repulsion robot-obstacle
extern BOOL boxAttraction = FALSE;		// Box is attracted during transportantion

extern int Cb = 10;						// Damping box
extern int Cr = 6;						// Damping robot (linear velocity)
extern int Cbw = 2;						// Damping robot (angular velocity)

extern double intStep = 0.01;			// Integration step
extern int BoxMass = 1;					// Box Mass


// Global functions.

// Compute the distance between two circles
void DistanceCircleCircle(double x1, double y1, double x2, double y2, double r1, double r2, double &dist, double &angle)
{
	dist = sqrt( (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) ) - r1 - r2;
	angle = atan2( (y1 - y2), (x1 - x2) );
}

void DistanceCircleRect(double x, double y, double radius, CRect rect, double &dist,  double &angle, BOOL &pointInside, CPoint &ptContact)
{
	// always return a positive distance between the circle and the closest rectangle border
	// pointInside indicates if the point is inside the rectangle;

	pointInside = FALSE;

	double dx1 = x - rect.TopLeft().x;
	double dx2 = x - rect.BottomRight().x;
	double dy1 = y - rect.TopLeft().y;
	double dy2 = y - rect.BottomRight().y;
				
	if ( (dx1 > 0) && (dx2 > 0) ){
		dx1 -= radius;
		dx2 -= radius;
	}
	else if ( (dx1 < 0) && (dx2 < 0) ){
		dx1 += radius;
		dx2 += radius;
	}
	
	if ( (dy1 > 0) && (dy2 > 0) ){
		dy1 -= radius;
		dy2 -= radius;
	}
	else if ( (dy1 < 0) && (dy2 < 0) ){
		dy1 += radius;
		dy2 += radius;
	}
				
	BOOL sign1 = (dx1 * dx2 > 0);
	BOOL sign2 = (dy1 * dy2 > 0);
				
	ptContact = CPoint(-1, -1);
	if ( (!sign1) && (!sign2) ){  //InContact;
		dist = fabs(dx1);
		angle = 0;
		ptContact = CPoint(rect.TopLeft().x, round(y));
		
		if (fabs(dx2) < dist) {
			dist = fabs(dx2);
			angle = PI;
			ptContact = CPoint(rect.BottomRight().x, round(y));
		}
		if (fabs(dy1) < dist) {
			dist = fabs(dy1);
			angle = PI/2;
			ptContact = CPoint(round(x), rect.TopLeft().y);
		}
		if (fabs(dy2) < dist) {
			dist = fabs(dy2);
			angle = -PI/2;
			ptContact = CPoint(round(x), rect.BottomRight().y);
		}
		pointInside = TRUE;
	}
	else if ( sign1 && sign2 ) {
		double dx = (dx1>=0) ? min(dx1,dx2) : max(dx1,dx2);
		double dy = (dy1>=0) ? min(dy1,dy2) : max(dy1,dy2);
		dist = sqrt(dx * dx + dy * dy );
		angle = atan2(-dy, -dx);
	}
	else if (sign1) {
		dist = min(fabs(dx1), fabs(dx2) );
		angle = (fabs(dx1) < fabs(dx2)) ? 0 : PI;
	}
	else if (sign2) {
		dist = min(fabs(dy1), fabs(dy2) );
		angle = (fabs(dy1) < fabs(dy2)) ? PI/2 : -PI/2;
	}
}


/////////////////////////////////////////////////////////////////////////////
// CSimulatorApp initialization

BOOL CSimulatorApp::InitInstance()
{
	// Standard initialization
	// If you are not using these features and wish to reduce the size
	//  of your final executable, you should remove from the following
	//  the specific initialization routines you do not need.

#ifdef _AFXDLL
	Enable3dControls();			// Call this when using MFC in a shared DLL
#else
	Enable3dControlsStatic();	// Call this when linking to MFC statically
#endif

	// Change the registry key under which our settings are stored.
	// TODO: You should modify this string to be something appropriate
	// such as the name of your company or organization.
	SetRegistryKey(_T("Local AppWizard-Generated Applications"));
	LoadStdProfileSettings();  // Load standard INI file options (including MRU)

	// Register the application's document templates.  Document templates
	//  serve as the connection between documents, frame windows and views.

	CSingleDocTemplate* pDocTemplate;
	pDocTemplate = new CSingleDocTemplate(
		IDR_MAINFRAME,
		RUNTIME_CLASS(CSimulatorDoc),
		RUNTIME_CLASS(CMainFrame),       // main SDI frame window
		RUNTIME_CLASS(CSimulatorView));
	AddDocTemplate(pDocTemplate);

	// Parse command line for standard shell commands, DDE, file open
	CCommandLineInfo cmdInfo;
	ParseCommandLine(cmdInfo);

	// Dispatch commands specified on the command line
	if (!ProcessShellCommand(cmdInfo))
		return FALSE;

	// The one and only window has been initialized, so show and update it.
	m_pMainWnd->SetWindowText("MuRoS: Multirobot Simulator");
	m_pMainWnd->ShowWindow(SW_SHOWMAXIMIZED);
	m_pMainWnd->UpdateWindow();

	return TRUE;
}


// Thread that updates the robots and box positons

UINT UpdateThreadProc(LPVOID pParam )
{
    CSimulatorDoc* pDoc = (CSimulatorDoc*)pParam;

    if (pDoc == NULL ||
        !pDoc->IsKindOf(RUNTIME_CLASS(CSimulatorDoc)))
    return 1;   // if pObject is not valid

	double invTime = 0;
//	CSingleLock sLock(&(pDoc->m_mutex));
	while (1) {
		int i;

//		sLock.Lock(); // mutual exclusion between treads
		
		double iStep = intStep;
		pDoc->m_simTime += iStep;

		for(i=0; i<pDoc->m_robots.GetSize(); i++){
			pDoc->m_robots[i]->Update(&(pDoc->m_robots), pDoc->m_simTime, iStep, pDoc->m_box, pDoc->m_globalMap);
		}

		if (pDoc->m_box->m_exist) {
			pDoc->m_box->Update( &(pDoc->m_robots), iStep, pDoc->m_globalMap);
		}

//		sLock.Unlock(); // mutual exclusion between treads

		/*
		invTime += iStep;
		if (invTime >= 0.01){
			pDoc->m_view->Invalidate();
			invTime = 0;
		} 
		*/
		pDoc->m_view->Invalidate();
	}

    return 0;   // thread completed successfully
}


/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
		// No message handlers
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

// App command to run the dialog
void CSimulatorApp::OnAppAbout()
{
	CAboutDlg aboutDlg;
	aboutDlg.DoModal();
}

/////////////////////////////////////////////////////////////////////////////
// CSimulatorApp message handlers