boxrect.cpp

多机器人合作中的动态角色分配仿真算法是多机器人合作领域的一个比较著名的仿真软件

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

#include "stdafx.h"
#include "simulator.h"
#include "BoxRect.h"
#include "const.h"
#include <math.h>

#include "robotXR4000.h"

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

#include "myglobals.h"

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

CBoxRect::CBoxRect()
{
	m_polygon = new CPolygon();
}

CBoxRect::~CBoxRect()
{

}

CBoxRect::CBoxRect(CRect rect)
{
	m_exist = TRUE;
	m_rect = rect;

	m_x = rect.CenterPoint().x; 
	m_y = rect.CenterPoint().y;
	m_theta = 0;

	m_radius = rect.Size().cx / 2;

	m_polygon = new CPolygon(rect);
	m_polygon->SetPointsCenterFrame();
}

void CBoxRect::Draw(CDC *pDC)
{
	pDC->SelectObject(&YELLOW);
	m_polygon->Draw(pDC);
}

void CBoxRect::Update(CArray<CRobot*, CRobot*> *robots, double dt, CMapPath *map)
{
	int i;

	double fx = 0;
	double fy = 0;
	double ftheta = 0;

	CBox::Update(robots, dt, map);
	m_mass = BoxMass;

	// Repulsive forces from obstacles
	// for (i=0; i<map->m_obstacles.GetSize(); i++)
	// 	 ForceFromObstacles(map->m_obstacles[i], fx, fy);

	// Repulsive forces from robots that are transporting
	for(i=0; i<robots->GetSize(); i++) 
		(robots->GetAt(i) )->ForceOnBoxes(this, fx, fy, ftheta);

	// Damping
	fx += -Cb * m_mass * m_v[0];
	fy += -Cb * m_mass * m_v[1];
	ftheta += -Cbw * m_mass * m_v[2];

	// Computing Velocities
	m_v[0] += fx * dt / m_mass;
	m_v[1] += fy * dt / m_mass;
	m_v[2] += ftheta * dt / m_mass;

	// Saturation
	double factor;
	if ( (m_v[0] > 300) || (m_v[1] > 300) )
		if (m_v[0] > m_v[1]) {
			factor = m_v[0] / 300;
			m_v[0] = m_v[0] / factor;
			m_v[1] = m_v[1] / factor;
		}
		else{
			factor = m_v[1] / 300;
			m_v[1] = m_v[1] / factor;
			m_v[0] = m_v[0] / factor;
		}
	if ( (m_v[0] < -300) || (m_v[1] < -300) )
		if (m_v[0] < m_v[1]) {
			factor = m_v[0] / -300;
			m_v[0] = m_v[0] / factor;
			m_v[1] = m_v[1] / factor;
		}
		else{
			factor = m_v[1] / -200;
			m_v[1] = m_v[1] / factor;
			m_v[0] = m_v[0] / factor;
		}

	m_x += m_v[0] * dt;
	m_y += m_v[1] * dt;
	m_theta += m_v[2] * dt;

	// Computing the new coordinates of the polygon for redrawing
	m_polygon->Recompute(m_x, m_y, m_theta);
}

// not being used for now.
void CBoxRect::ForceFromObstacles(CObstacle *obst, double &fx, double &fy)
{
	double dist;
	double angle;
	double deltaN;
	double deltaNDot;
	double lambdaN;

	if (!obst->m_erased) {

		// distance from circle obstacles
		DistanceCircleCircle(obst->m_center.x, obst->m_center.y, m_x, m_y, obst->m_radius, m_radius, dist, angle);
		angle += PI; // normal

		if (dist < 0) {
			deltaN = -dist;
			deltaNDot = m_v[0] * cos(angle) + m_v[1] * sin(angle); // velocity in the normal direction
			lambdaN = Kc_bo * deltaN - Cc_bo * deltaNDot;

				fx +=  lambdaN * cos(angle);
				fy +=  lambdaN * sin(angle);
			}
		}	
}

void CBoxRect::Serialize(CArchive& ar)
{
	CBox::Serialize(ar);
	m_polygon->Serialize(ar);
}