boxpolygon.cpp

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

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

#include "stdafx.h"
#include "simulator.h"
#include "BoxPolygon.h"
#include "const.h"
#include "myglobals.h"

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

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

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

CBoxPolygon::CBoxPolygon(CPolygon *polygon) : CBox()
{
	m_polygon = new CPolygon(polygon);
	m_x = m_polygon->m_center.x;
	m_y = m_polygon->m_center.y;
	m_theta = 0;

	m_polygon->SetPointsCenterFrame();
	m_rect = m_polygon->m_rect;

	m_radius = 0;
	m_exist = TRUE;
}

CBoxPolygon::~CBoxPolygon()
{
}

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

void CBoxPolygon::Serialize(CArchive& ar)
{
	CBox::Serialize(ar); // Base class method
	m_polygon->Serialize(ar);
}

void CBoxPolygon::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 now, it is working only for circle obstacles and objects
	// 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];
	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 CBoxPolygon::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);
			}
		}	
}