box.cpp

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

//////////////////////////////////////////////////////////////////////
// MuRoS - Multi Robot Simulator
//
// Luiz Chaimowicz
// GRASP Lab. University of Pennsylvania
// VERLab - DCC - UFMG - Brasil
//
// Box.cpp: implementation of the CBox class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "Box.h"
#include "const.h"
#include "robotHolonomic.h"
#include <math.h>

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

#include "robotXR4000.h"
#include "myglobals.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CBox, CObject, 1 )

CBox::CBox()
{
	m_exist = FALSE;

	m_x = 0;
	m_y = 0;
	m_theta = 0;
	m_mass = 1;
	
	for(short i=0; i<3; i++){
		m_a[i] = 0;
		m_v[i] = 0;
	}
	m_path.RemoveAll();
}

CBox::CBox(CRect rect)
{
	m_exist = TRUE;
	m_rect = rect;
	m_x = (double) rect.CenterPoint().x; 
	m_y = (double) rect.CenterPoint().y; 
	m_theta = 0;
	m_path.RemoveAll();
}


CBox::~CBox()
{
}

void CBox::Draw(CDC *pDC)
{
}

void CBox::Update(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map)
{
	// Leaving a track of box position
	CPoint p = CPoint(round(m_x), round(m_y));
	if (m_path.GetSize() > 0) {
		if (p != m_path[m_path.GetSize()-1])
			m_path.Add(p);
	}
	else
		m_path.Add(p);
}


void CBox::Serialize(CArchive& ar)
{
	int i;

	CObject::Serialize(ar);

	if (ar.IsStoring()) {
		ar << m_exist;
		ar << m_x;
		ar << m_y;
		ar << m_theta;
		ar << m_radius;
		ar << m_mass;
		ar << m_rect;
		
		for(i=0; i<3; i++) {
			ar << m_f[i];
			ar << m_a[i];
			ar << m_v[i];
		}
	}
	else {
		ar >> m_exist;
		ar >> m_x;
		ar >> m_y;
		ar >> m_theta;
		ar >> m_radius;
		ar >> m_mass;
		ar >> m_rect;
		
		for(i=0; i<3; i++) {
			ar >> m_f[i];
			ar >> m_a[i];
			ar >> m_v[i];
		}
	}
}