boxcircle.cpp

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

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

#include "stdafx.h"
#include "simulator.h"
#include "BoxCircle.h"

#include "const.h"
#include <math.h>

#include "robotHolonomic.h"

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

#include "myglobals.h"

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

CBoxCircle::CBoxCircle()
{
}

CBoxCircle::~CBoxCircle()
{
}

CBoxCircle::CBoxCircle(CRect rect)
{
	m_exist = TRUE;
	m_rect = rect;
	m_x = rect.CenterPoint().x; 
	m_y = rect.CenterPoint().y; 
	m_radius = rect.Size().cx / 2;
}

void CBoxCircle::Draw(CDC *pDC)
{
	m_rect = CRect(round(m_x-m_radius), round(m_y-m_radius), round(m_x+m_radius), round(m_y+m_radius));

	pDC->SelectStockObject(BLACK_PEN);
	pDC->SelectObject(&YELLOW);
	pDC->Ellipse(m_rect);
}


void CBoxCircle::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;

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

	// Contact 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];

	// Computing Velocities (vx, vy)
	m_v[0] += fx * dt / m_mass;
	m_v[1] += fy * 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] / -300;
			m_v[1] = m_v[1] / factor;
			m_v[0] = m_v[0] / factor;
		}

	// Integrating Position
	m_x += m_v[0] * dt;
	m_y += m_v[1] * dt;
}


// Compute the force that are been applied in the box by the contact with obstacles
// For now, we are considering only circle obstacles
void CBoxCircle::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);
		// the force must be in the normal direction
		angle += PI; 

		// if dist < 0 (collision), compute the contact forces in the normal direction
		// using a spring-damper contact model.
		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 CBoxCircle::Serialize(CArchive& ar)
{
	CBox::Serialize(ar);
}