polygon.cpp

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

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

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

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

IMPLEMENT_SERIAL( CPolygon, CObject, 1 )

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

// Empty Contructor
CPolygon::CPolygon()
{
	m_center = CPoint(-1,-1);
}

// Contructor receiving an array of points (vertex)
CPolygon::CPolygon(CArray<CPoint, CPoint> *points)
{
	for (int i=0; i < points->GetSize(); i++)
		m_points.Add(points->GetAt(i));

	ComputeCenter();
}

// Copy Contructor: receiving other polygon
CPolygon::CPolygon(CPolygon *polygon)
{
	for (int i=0; i < polygon->m_points.GetSize(); i++)
		m_points.Add(polygon->m_points[i]);

	m_rect = polygon->m_rect;
	m_center = polygon->m_center;
}

// Constructor receiving a rectangle
CPolygon::CPolygon(CRect rect)
{
	rect.NormalizeRect();

	m_points.Add(CPoint(rect.left, rect.top));
	m_points.Add(CPoint(rect.right, rect.top));
	m_points.Add(CPoint(rect.right, rect.bottom));
	m_points.Add(CPoint(rect.left, rect.bottom));

	m_rect = rect;
	m_center = rect.CenterPoint();
}

// Compute the point positions relative to the center of the polygon
// This is necessary for computing new positions in case of translations and rotations
void CPolygon::SetPointsCenterFrame()
{
	CPoint p;

	for (int i=0; i < m_points.GetSize(); i++) {
		p.x = m_points[i].x - m_center.x;
		p.y = m_points[i].y - m_center.y;
		m_pointsCenterFrame.Add(p);
	}
}


CPolygon::~CPolygon()
{
}

// Add a new vertex to the polygon, testing if it is valid.
void CPolygon::AddPoint(CPoint p)
{
	if (m_points.GetSize() == 0)
		m_points.Add(p);
	else
		m_points[m_points.GetSize()-1] = p;

	if ( IsSimple() ) {
		m_points.Add(p); // repeat insertion to help drawing function (OnMouseMove)
	}
	else {
		m_points.RemoveAll();
		AfxMessageBox("Invalid polygon. The polygon can not intersect itself", MB_OK + MB_ICONWARNING);
	}
}

// Finish the point insertion, testing if it is simple, removin auxiliary popints
// and computing the polygon center.
BOOL CPolygon::FinishPointInsertion()
{
	BOOL simple = TRUE;

	if (m_points.GetSize() > 2) {
		m_points.RemoveAt(m_points.GetSize() - 1); // removing extra point
		if ( !(IsSimple(TRUE) ) ) {
			simple = FALSE;
			m_points.RemoveAll();
			AfxMessageBox("Invalid polygon. The polygon can not intersect itself", MB_OK + MB_ICONWARNING);
		}
		else {
			ComputeCenter();
		}
	}

	return ( (m_points.GetSize() > 1) && simple );
}

CPoint CPolygon::GetPoint(int i)
{
	return m_points[i];
}

void CPolygon::RemoveAllPoints()
{
	m_points.RemoveAll();
}

void CPolygon::Draw(CDC* pDC) 
{
	pDC->Polygon(m_points.GetData(), m_points.GetSize());
	if (m_center.x != -1) {
		pDC->SelectObject(&RED_PEN);
		CRect aux = CRect(m_center.x-1, m_center.y-1, m_center.x+1, m_center.y+1);
		pDC->Rectangle(aux);
	}
}	

// To draw lines during mousemove
void CPolygon::Redraw(CPoint p, CClientDC *dc) 
{
	if (m_points.GetSize() > 0) {
		dc->SelectStockObject(WHITE_PEN);
		dc->Polygon(m_points.GetData(), m_points.GetSize());
		
		m_points[m_points.GetSize()-1] = p;

		dc->SelectObject(&BLACK);
		dc->Polygon(m_points.GetData(), m_points.GetSize());
	}
}

// Detect if the circle intersect with the polygon. This is done testing the 
// intersection between the circle eqaution x^2 + y^2 = R and all the polygon 
// segments ax + by + c = 0. Can be optmized ??
//
// The function return the number of intersection points 0,1,2 or 4(for special cases)
// Intersection with multiple corners are not treated. The intersection points are 
// returnet in the variables intesect 1 and 2.
short CPolygon::IntersectCircle(CPoint robot, double robotRadius, CPoint &intersect1, CPoint &intersect2)
{
	double a, b, c, k, n, x, y, delta;
	CArray<CPoint, CPoint> result;
	CPoint ip1, ip2, p1, p2;
	CByteArray segment;

	// Testing the intersection between the line and circle equations
	// delta < 0 -> no intersection
	// delta = 0 -> 1 intersection point
	// delta > 0 -> 2 intersection points
	for (int j=0; j<m_points.GetSize(); j++) {
		p1 = m_points[j];
		p2 = m_points[(j+1)%m_points.GetSize()];

		if (p1.x == p2.x) {
			a = 1;
			b = -2 * robot.y;
			c = robot.y * robot.y + (p1.x - robot.x) * (p1.x - robot.x) - robotRadius * robotRadius;

			delta = b * b - 4 * a * c;
			if (delta < 0) {
				continue; // No intersection, get the next line
			}
			else if (delta == 0) {
				ip1 = CPoint(p1.x, round(-b/2.0) );
				ip2 = CPoint(-1, -1);
			}
			else{
				ip1 = CPoint( p1.x, round( ( -b + sqrt(delta) ) / (2.0 * a) ) );
				ip2 = CPoint( p1.x, round( ( -b - sqrt(delta) ) / (2.0 * a) ) );
			}
		}
		else {
			k = ( (p2.y - p1.y) / (double) (p2.x - p1.x) );
			n = p1.y - k * p1.x;
			a = (1 + k * k);
			b = -2 * robot.x + 2 * k * (n - robot.y);
			c = robot.x * robot.x + (n - robot.y) * (n - robot.y) - robotRadius * robotRadius;

			delta = b * b - 4 * a * c;
			if (delta < 0) {
				continue; // No intersection, get the next line
			}
			else if (delta == 0) {
				x = -b/2;
				y = k * x + n;
				ip1 = CPoint(round(x), round(y) );
				ip2 = CPoint(-1, -1);
			}
			else{
				x = (-b + sqrt(delta) ) / (2.0 * a);
				y = k * x + n;
				ip1 = CPoint( round(x), round(y) );

				x = (-b - sqrt(delta) ) / (2.0 * a);
				y = k * x + n;
				ip2 = CPoint( round(x), round(y) );
			}
		}

		// test if the intersection pont is inside the line segment
		if ( ( (ip1.x - p1.x) * (ip1.x - p2.x) <= 0 ) && ( (ip1.y - p1.y) * (ip1.y - p2.y) <= 0 ) ) {
			result.Add(ip1);
			segment.Add(j);
		}

		if ( ( (ip2.x - p1.x) * (ip2.x - p2.x) <= 0 ) && ( (ip2.y - p1.y) * (ip2.y - p2.y) <= 0 ) ) {
			result.Add(ip2);
			segment.Add(j);
		}

		if (result.GetSize() == 2)
			break;	// two intersection points have already been found
	}

	if (result.GetSize() == 2) { // 2 intersection points
		intersect1 = result[0];
		intersect2 = result[1];

		if (segment[0] == segment[1])
			return 2; // the intersection points are in the same polygon segment
		else
			return 4; // intersect 2 different segments (one of the polygon corners)
	}
	else if (result.GetSize() == 1) { // 1 intersection point
		intersect1 = result[0];
		intersect2 = CPoint(-1,-1);
		return 1;
	}
	else { // No intersection
		intersect1 = CPoint(-1,-1);
		intersect2 = CPoint(-1,-1);
		return 0;
	}
}


// Test if the poly is simple (no self intersecting)
BOOL CPolygon::IsSimple(BOOL lastSegment)
{

	double xi, yi, n1, n2, k1, k2;
	CPoint p1, p2, p3, p4;
	short first, last;
	BOOL isSimple = TRUE;
	
	if (m_points.GetSize() > 3) {

		p1 = m_points.GetAt(m_points.GetSize()-1);
		// If is the last segment, need to use points n-1 and 0
		if (lastSegment) {
			p2 = m_points.GetAt(0);
			first = 1;
			last = (short) m_points.GetSize()-2;
		}
		else{
			p2 = m_points.GetAt(m_points.GetSize()-2);
			first = 0;
			last = (short) m_points.GetSize()-3;
		}
		k1 = ( (p2.y - p1.y) / (double) (p2.x - p1.x) );
		n1 = p1.y - k1 * p1.x;


		for (short i=first; i<last; i++) {
			p3 = m_points.GetAt(i);
			p4 = m_points.GetAt(i+1);

			k2 = ( (p4.y - p3.y) / (double) (p4.x - p3.x) );
			n2 = p3.y - k2 * p3.x;

			if ( (p1.x == p2.x) && (p3.x == p4.x) ){ // both segments are parallel to y=0
					xi = -1;
					yi = -1;
				}
			else if (p1.x == p2.x) { //p1p2 parallel to y=0
				xi = p2.x;
				yi = k2 * xi + n2;
			}
			else if (p3.x == p4.x) { //p3p4 parallel to y=0
				xi = p1.x;
				yi = k1 * xi + n1;
			}
			else {
				xi = (n2-n1) / (k1-k2);
				yi = k1 * xi + n1;
			}

			if ( ( (xi - p1.x) * (xi - p2.x) <= 0 ) && ( (yi - p1.y) * (yi - p2.y) <= 0 ) &&
				 ( (xi - p3.x) * (xi - p4.x) <= 0 ) && ( (yi - p3.y) * (yi - p4.y) <= 0 ) )
					isSimple = FALSE;
		}
	}
	return isSimple;			
}



void CPolygon::Serialize(CArchive& ar)
{
	int i, aux;
	CPoint p;

	CObject::Serialize(ar); // Base class method

	if (ar.IsStoring()) {
		ar << m_points.GetSize();
		for (i=0; i<m_points.GetSize(); i++)
			ar << m_points[i];

		ar << m_pointsCenterFrame.GetSize();
		for (i=0; i<m_pointsCenterFrame.GetSize(); i++)
			ar << m_pointsCenterFrame[i];

		ar << m_rect;
		ar << m_center;
		ar << m_intersect1;
		ar << m_intersect2;
	}
	else {
		ar >> aux;
		for (i=0; i<aux; i++) {
			ar >> p;
			m_points.Add(p);
		}

		ar >> aux;
		for (i=0; i<aux; i++) {
			ar >> p;
			m_pointsCenterFrame.Add(p);
		}

		ar >> m_rect;
		ar >> m_center;
		ar >> m_intersect1;
		ar >> m_intersect2;
	}
}

// Compute polygon area
double CPolygon::Area()
{
   int i,j;
   double area = 0;

   for (i=0; i<m_points.GetSize(); i++) {
      j = (i + 1) % m_points.GetSize();
      area += m_points[i].x * m_points[j].y;
      area -= m_points[i].y * m_points[j].x;
   }

   area /= 2;
   return(area < 0 ? -area : area);
}

// Compute polygon center
void CPolygon::ComputeCenter()
{
	int i,j;
	long double a,b,c,d,e,f;
	long double cmX = 0;
	long double cmY = 0;
	double area = Area();

 
	for (i=0; i<m_points.GetSize(); i++) {
		j = (i + 1) % m_points.GetSize();
		a = (m_points[i].x + m_points[j].x);
		b = (m_points[i].x * m_points[j].y);
		c = (m_points[i].y * m_points[j].x);
		d = (m_points[i].y + m_points[j].y);
		e = (m_points[i].x * m_points[j].y);
		f = (m_points[i].y * m_points[j].x);

		cmX += ( a * (b - c) );
		cmY += ( d * (e - f) );
	}

	cmX = fabs(cmX / (6*area));
	cmY = fabs(cmY / (6*area));

	m_center = CPoint(round(cmX), round(cmY));

	CRgn rgn;
	rgn.CreatePolygonRgn(m_points.GetData(), m_points.GetSize(), ALTERNATE);
	rgn.GetRgnBox(&m_rect);
}


// given the new position of the center, recompute the position of the 
// vertices
void CPolygon::Recompute(double x, double y, double theta)
{
	double xc, yc; // x and y coordinates related to the center of the polygon
	double xw, yw; // x and y coordinates related to the world frame

	xc = 0;
	for (int i=0; i < m_points.GetSize(); i++) {
		CPoint p = m_points[i];
		CPoint p2 = m_pointsCenterFrame[i];
		xc = m_pointsCenterFrame[i].x;
		yc = m_pointsCenterFrame[i].y;

		xw = xc * cos(theta) - yc * sin(theta) + x;
		yw = xc * sin(theta) + yc * cos(theta) + y;

		m_points[i] = CPoint(round(xw), round(yw));
	}
	m_center = CPoint(round(x), round(y));

}

// Get shortest distance between a point and a polygon.
// First test the distance between the point and the vertices, and after,
// test if the distance to one the two edges of the short vertex is shorter
void CPolygon::ShortDistance(double x, double y, double &minDist, double &minAngle)
{
	double dist, angle, a, b, c, xi, yi;
	CPoint p1, p2, out;
	int i, point;

	point = -1;
	minDist = 10000;

	// Getting closest vertice
	for(i=0; i<m_points.GetSize(); i++){
		CPoint aux = m_points[i];
		DistanceCircleCircle(m_points[i].x, m_points[i].y, x, y, 0, 0, dist, angle);
		if (dist < minDist) {
			minDist = dist;
			minAngle = angle;
			point = i;
			out = m_points[i];
		}
	}


	// Get the two edges of the vertice and test if the distance from teh point
	// to the closest point in the edge is less the the distance to the vertice
	point = (point == 0) ? m_points.GetSize()-1 : point - 1;

	p2 =  m_points[(point+1) % m_points.GetSize()];
	p1 =  m_points[point];
	a = (p2.y - p1.y);
	b = (p1.x - p2.x) ;
	c = (p1.y - p2.y) * p1.x + (p2.x - p1.x) * p1.y;

	xi = (b * b * x - a * b * y - a * c) / (a * a + b * b);
	yi = (a * a * y - a * b * x - b * c) / (a * a + b * b);

	if ( ( (xi - p1.x) * (xi - p2.x) <= 0 ) && ( (yi - p1.y) * (yi - p2.y) <= 0 ) ) {
		DistanceCircleCircle(xi, yi, x, y, 0, 0, dist, angle);
		if (dist < minDist) {
			minDist = dist;
			minAngle = angle;
			out = CPoint(round(xi), round(yi));
		}
	}

	p2 =  m_points[(point+2) % m_points.GetSize()];
	p1 =  m_points[(point+1) % m_points.GetSize()];
	a = (p2.y - p1.y);
	b = (p1.x - p2.x) ;
	c = (p1.y - p2.y) * p1.x + (p2.x - p1.x) * p1.y;

	xi = (b * b * x - a * b * y - a * c) / (a * a + b * b);
	yi = (a * a * y - a * b * x - b * c) / (a * a + b * b);

	if ( ( (xi - p1.x) * (xi - p2.x) <= 0 ) && ( (yi - p1.y) * (yi - p2.y) <= 0 ) ) {
		DistanceCircleCircle(xi, yi, x, y, 0, 0, dist, angle);
		if (dist < minDist) {
			minDist = dist;
			minAngle = angle;
			out = CPoint(round(xi), round(yi));
		}
	}

}