insilmaril@650: #include "geometry.h"
insilmaril@650: 
insilmaril@656: #include <math.h>
insilmaril@662: #include "misc.h"
insilmaril@656: 
insilmaril@792: #include <QString>
insilmaril@792: 
insilmaril@789: #include <iostream>
insilmaril@662: using namespace std;
insilmaril@650: 
insilmaril@789: 
insilmaril@650: QRectF addBBox(QRectF r1, QRectF r2)
insilmaril@650: {	
insilmaril@650: 	// Find smallest QRectF containing given rectangles
insilmaril@650: 
insilmaril@650: 	QRectF n;
insilmaril@650: 	// Set left border
insilmaril@650: 	if (r1.left() <= r2.left() )
insilmaril@650: 		n.setLeft(r1.left() );
insilmaril@650: 	else
insilmaril@650: 		n.setLeft(r2.left() );
insilmaril@650: 		
insilmaril@650: 	// Set top border		
insilmaril@650: 	if (r1.top() <= r2.top() )
insilmaril@650: 		n.setTop(r1.top() );
insilmaril@650: 	else
insilmaril@650: 		n.setTop(r2.top() );
insilmaril@650: 		
insilmaril@650: 	// Set right border
insilmaril@650: 	if (r1.right() <= r2.right() )
insilmaril@650: 		n.setRight(r2.right() );
insilmaril@650: 	else
insilmaril@650: 		n.setRight(r1.right() );
insilmaril@650: 		
insilmaril@650: 	// Set bottom 
insilmaril@650: 	if (r1.bottom() <= r2.bottom() )
insilmaril@650: 		n.setBottom(r2.bottom() );
insilmaril@650: 	else
insilmaril@650: 		n.setBottom(r1.bottom() );
insilmaril@650: 	return n;
insilmaril@650: }
insilmaril@650: 
insilmaril@754: bool isInBox(const QPointF &p, const QRectF &box)
insilmaril@650: {
insilmaril@650:     if (p.x() >= box.left() && p.x() <= box.right()  
insilmaril@650: 	&& p.y() <= box.bottom() && p.y() >= box.top() )
insilmaril@650: 		return true;
insilmaril@650:     return false;	
insilmaril@650: }
insilmaril@656: 
insilmaril@824: qreal distance (const QPointF &p, const QPointF &q)
insilmaril@824: {
insilmaril@824: 	return sqrt (p.x()*q.x() + p.y()*q.y());
insilmaril@824: }
insilmaril@824: 
insilmaril@792: Vector::Vector ():QPointF ()
insilmaril@792: {
insilmaril@792: }
insilmaril@792: 
insilmaril@792: Vector::Vector (const QPointF &p):QPointF (p)
insilmaril@792: {
insilmaril@792: }
insilmaril@792: 
insilmaril@792: Vector::Vector (qreal x, qreal y):QPointF (x,y)
insilmaril@792: {
insilmaril@792: }
insilmaril@792: 
insilmaril@835: //! Check if length is 0
insilmaril@835: bool Vector::isNull()
insilmaril@835: {
insilmaril@835: 	if (x()==0 && y()==0)
insilmaril@835: 		return true;
insilmaril@835: 	return false;	
insilmaril@835: }
insilmaril@835: 
insilmaril@792: //! Normalize vector
insilmaril@792: void Vector::normalize ()
insilmaril@792: {
insilmaril@835: 	if (isNull() ) return;
insilmaril@792: 	qreal l=sqrt ( x()*x() + y()*y() );
insilmaril@792: 	setX (x()/l);
insilmaril@792: 	setY (y()/l);
insilmaril@792: }
insilmaril@792: 
insilmaril@792: //! Dot product of two vectors
insilmaril@792: qreal Vector::dotProduct (const QPointF &b)
insilmaril@792: {
insilmaril@792: 	return x()*b.x() + y()*b.y();
insilmaril@792: }
insilmaril@792: 
insilmaril@792: 
insilmaril@792: void Vector::scale (const qreal &f)
insilmaril@792: {
insilmaril@792: 	setX (x()*f);
insilmaril@792: 	setY (y()*f);
insilmaril@792: }
insilmaril@792: 
insilmaril@792: void Vector::invert ()
insilmaril@792: {
insilmaril@792: 	setX (-x());
insilmaril@792: 	setY (-y());
insilmaril@792: }
insilmaril@792: 
insilmaril@798: QPointF Vector::toQPointF ()
insilmaril@798: {
insilmaril@798: 	return QPointF (x(),y());
insilmaril@798: }
insilmaril@798: 
insilmaril@792: /*! Calculate the projection of a polygon on an axis
insilmaril@792:     and returns it as a [min, max] interval  */
insilmaril@789: ConvexPolygon::ConvexPolygon ()
insilmaril@789: {
insilmaril@789: }
insilmaril@789: 
insilmaril@789: ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)
insilmaril@789: {
insilmaril@789: }
insilmaril@789: 
insilmaril@789: void ConvexPolygon::calcCentroid() 
insilmaril@789: {
insilmaril@789: 	// Calculate area and centroid
insilmaril@789: 	// http://en.wikipedia.org/wiki/Centroid
insilmaril@789: 	qreal cx,cy,p;
insilmaril@789: 	cx=cy=0;
insilmaril@789: 	_area=0;
insilmaril@789: 
insilmaril@789: 	append (at(0));
insilmaril@789: 	for (int i=0;i<size()-1;i++)
insilmaril@789: 	{
insilmaril@789: 		p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();
insilmaril@789: 		_area+=p;
insilmaril@789: 		cx+=(at(i).x()+at(i+1).x()) * p;
insilmaril@789: 		cy+=(at(i).y()+at(i+1).y()) * p;
insilmaril@789: 	}	
insilmaril@789: 	pop_back();
insilmaril@789: 	// area is negative if vertices ordered counterclockwise
insilmaril@789: 	// (in mirrored graphicsview!)
insilmaril@789: 	_area=_area/2;	
insilmaril@789: 	p=_area*6;
insilmaril@789: 	_centroid.setX (cx/p);
insilmaril@789: 	_centroid.setY (cy/p);
insilmaril@789: }
insilmaril@789: 
insilmaril@789: QPointF ConvexPolygon::centroid() const
insilmaril@789: {
insilmaril@789: 	return _centroid;
insilmaril@789: }
insilmaril@789: 
insilmaril@789: qreal ConvexPolygon::weight() const
insilmaril@789: {
insilmaril@789: 	return _area;
insilmaril@789: }
insilmaril@789: 
insilmaril@792: std::string ConvexPolygon::toStdString()
insilmaril@656: {
insilmaril@792: 	QString s ("(");
insilmaril@792: 	for (int i=0;i<size();++i)
insilmaril@792: 	{
insilmaril@792: 		s+=QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());
insilmaril@792: 		if (i<size()-1) s+=",";
insilmaril@792: 	}
insilmaril@792: 	s+=")";	
insilmaril@792: 	return s.toStdString();
insilmaril@656: }
insilmaril@656: 
insilmaril@792: Vector ConvexPolygon::at(const int &i) const
insilmaril@656: {
insilmaril@792: 	return Vector (QPolygonF::at(i).x(),QPolygonF::at(i).y());
insilmaril@656: }
insilmaril@656: 
insilmaril@792: void ConvexPolygon::translate ( const Vector & offset )
insilmaril@792: { translate (offset.x(),offset.y());}
insilmaril@656: 
insilmaril@792: void ConvexPolygon::translate ( qreal dx, qreal dy )
insilmaril@789: {
insilmaril@792: 	QPolygonF::translate (dx,dy);
insilmaril@792: 	_centroid=_centroid+QPointF (dx,dy);
insilmaril@789: }
insilmaril@789: 
insilmaril@792: void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max) 
insilmaril@656: {
insilmaril@656:     // To project a point on an axis use the dot product
insilmaril@656: 
insilmaril@789: 	//cout << "Projecting on "<< axis<<endl;
insilmaril@792:     qreal d = axis.dotProduct(polygon.at(0));
insilmaril@656:     min = d;
insilmaril@656:     max = d;
insilmaril@789:     for (int i = 0; i < polygon.size(); i++) 
insilmaril@789: 	{
insilmaril@792:         d= polygon.at(i).dotProduct (axis);
insilmaril@656:         if (d < min) 
insilmaril@656:             min = d;
insilmaril@656:         else 
insilmaril@656:             if (d> max) max = d;
insilmaril@789: 	//	cout << "p="<<polygon.at(i)<<"  d="<<d<<"  (min, max)=("<<min<<","<<max<<")\n";	
insilmaril@656:     }
insilmaril@656: }
insilmaril@656: 
insilmaril@789: // Calculate the signed distance between [minA, maxA] and [minB, maxB]
insilmaril@789: // The distance will be negative if the intervals overlap
insilmaril@656: 
insilmaril@656: qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
insilmaril@656:     if (minA < minB) {
insilmaril@656:         return minB - maxA;
insilmaril@656:     } else {
insilmaril@656:         return minA - maxB;
insilmaril@656:     }
insilmaril@656: }
insilmaril@789: 
insilmaril@789: /*!
insilmaril@656:  Check if polygon A is going to collide with polygon B.
insilmaril@656:  The last parameter is the *relative* velocity 
insilmaril@656:  of the polygons (i.e. velocityA - velocityB)
insilmaril@789: */
insilmaril@656: 
insilmaril@792: PolygonCollisionResult polygonCollision(ConvexPolygon polygonA, 
insilmaril@792:                               ConvexPolygon polygonB, Vector velocity) 
insilmaril@789: {
insilmaril@656:     PolygonCollisionResult result;
insilmaril@662:     result.intersect = true;
insilmaril@662:     result.willIntersect = true;
insilmaril@656: 
insilmaril@656:     int edgeCountA = polygonA.size();
insilmaril@656:     int edgeCountB = polygonB.size();
insilmaril@656:     qreal minIntervalDistance = 1000000000;
insilmaril@656:     QPointF translationAxis;
insilmaril@656:     QPointF edge;
insilmaril@656: 
insilmaril@789: /*
insilmaril@662: 	cout << "\nA: ";
insilmaril@662: 	for (int k=0; k<edgeCountA;k++)
insilmaril@662: 		cout <<polygonA.at(k);
insilmaril@662: 	cout << "\nB: ";
insilmaril@662: 	for (int k=0; k<edgeCountB;k++)
insilmaril@662: 		cout <<polygonB.at(k);
insilmaril@789: 	cout <<endl;	
insilmaril@789: */		
insilmaril@662: 		
insilmaril@656:     // Loop through all the edges of both polygons
insilmaril@789: 	for (int i=0;i<edgeCountA + edgeCountB;i++)
insilmaril@656: 	{
insilmaril@656:         if (i< edgeCountA) 
insilmaril@662: 		{
insilmaril@789: 			// Loop through polygon A
insilmaril@789: 			if (i<edgeCountA-1)
insilmaril@662: 				edge = QPointF (
insilmaril@662: 					polygonA.at(i+1).x()-polygonA.at(i).x(), 
insilmaril@662: 					polygonA.at(i+1).y()-polygonA.at(i).y());
insilmaril@662: 			else		
insilmaril@662: 				edge = QPointF (
insilmaril@662: 					polygonA.at(0).x()-polygonA.at(i).x(), 
insilmaril@662: 					polygonA.at(0).y()-polygonA.at(i).y());
insilmaril@662:         } else 
insilmaril@662: 		{
insilmaril@789: 			// Loop through polygon B
insilmaril@789: 			if (i < edgeCountA +edgeCountB -1 )
insilmaril@662: 				edge = QPointF (
insilmaril@789: 					polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(), 
insilmaril@789: 					polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
insilmaril@662: 			else	
insilmaril@662: 				edge = QPointF (
insilmaril@789: 					polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(), 
insilmaril@789: 					polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
insilmaril@662: 		}
insilmaril@656: 
insilmaril@656:         // ===== 1. Find if the polygons are currently intersecting =====
insilmaril@656: 
insilmaril@656:         // Find the axis perpendicular to the current edge
insilmaril@656: 
insilmaril@792:         Vector axis (-edge.y(), edge.x());
insilmaril@792:         axis.normalize();
insilmaril@656: 
insilmaril@656:         // Find the projection of the polygon on the current axis
insilmaril@656: 
insilmaril@656:         qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
insilmaril@789:         projectPolygon(axis, polygonA, minA, maxA);
insilmaril@789:         projectPolygon(axis, polygonB, minB, maxB);
insilmaril@656: 
insilmaril@656:         // Check if the polygon projections are currentlty intersecting
insilmaril@656: 
insilmaril@789:         qreal d = intervalDistance(minA, maxA, minB, maxB);
insilmaril@789:         if (d > 0) result.intersect = false;
insilmaril@656: 
insilmaril@789:        // ===== 2. Now find if the polygons *will* intersect =====
insilmaril@656: 
insilmaril@656: 
insilmaril@656:         // Project the velocity on the current axis
insilmaril@656: 
insilmaril@792:         qreal velocityProjection = axis.dotProduct(velocity);
insilmaril@656: 
insilmaril@656:         // Get the projection of polygon A during the movement
insilmaril@656: 
insilmaril@662:         if (velocityProjection < 0) 
insilmaril@656:             minA += velocityProjection;
insilmaril@662:         else 
insilmaril@656:             maxA += velocityProjection;
insilmaril@656: 
insilmaril@656:         // Do the same test as above for the new projection
insilmaril@656: 
insilmaril@789:         // d = intervalDistance(minA, maxA, minB, maxB);
insilmaril@789:         //if (d > 0) result.willIntersect = false;
insilmaril@662: 		/*
insilmaril@662: 		cout <<"   ";
insilmaril@789: 		cout << "edge="<<edge<<"  ";
insilmaril@789: 		cout <<"axis="<<axis<<"  ";
insilmaril@789: 		cout <<"dA=("<<minA<<","<<maxA<<")  dB=("<<minB<<","<<maxB<<")";
insilmaril@662: 		cout <<"  d="<<d<<"   ";
insilmaril@789: 		//cout <<"minD="<<minIntervalDistance<<"  ";
insilmaril@662: 		cout <<"int="<<result.intersect<<"  ";
insilmaril@789: 		//cout <<"wint="<<result.willIntersect<<"  ";
insilmaril@662: 		//cout <<"velProj="<<velocityProjection<<"  ";
insilmaril@662: 		cout <<endl;
insilmaril@789: 		*/
insilmaril@662: 	
insilmaril@789:         if (result.intersect )// || result.willIntersect) 
insilmaril@662: 		{
insilmaril@662: 			// Check if the current interval distance is the minimum one. If so
insilmaril@662: 			// store the interval distance and the current distance.  This will
insilmaril@662: 			// be used to calculate the minimum translation vector
insilmaril@656: 
insilmaril@662: 			if (d<0) d=-d;
insilmaril@662: 			if (d < minIntervalDistance) {
insilmaril@662: 				minIntervalDistance = d;
insilmaril@789: 				//translationAxis = axis;
insilmaril@789: 				//cout << "tAxix="<<translationAxis<<endl;
insilmaril@656: 
insilmaril@662: 				//QPointF t = polygonA.Center - polygonB.Center;
insilmaril@789: 				//QPointF t = polygonA.at(0) - polygonB.at(0);
insilmaril@789: 				//if (dotProduct(t,translationAxis) < 0)
insilmaril@789: 				//	translationAxis = -translationAxis;
insilmaril@662: 			}
insilmaril@662: 		}
insilmaril@656:     }
insilmaril@656: 
insilmaril@656:     // The minimum translation vector
insilmaril@656:     // can be used to push the polygons appart.
insilmaril@656: 
insilmaril@662:     if (result.willIntersect)
insilmaril@662:         result.minTranslation = 
insilmaril@656:                translationAxis * minIntervalDistance;
insilmaril@656:     
insilmaril@656:     return result;
insilmaril@656: }
insilmaril@656: 
insilmaril@656: /* The function can be used this way: 
insilmaril@656:    QPointF polygonATranslation = new QPointF();
insilmaril@656: */   
insilmaril@656: 
insilmaril@656: 
insilmaril@656: /*
insilmaril@656: PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
insilmaril@656: 
insilmaril@656: if (r.WillIntersect) 
insilmaril@656:   // Move the polygon by its velocity, then move
insilmaril@656:   // the polygons appart using the Minimum Translation Vector
insilmaril@662:   polygonATranslation = velocity + r.minTranslation;
insilmaril@656: else 
insilmaril@656:   // Just move the polygon by its velocity
insilmaril@656:   polygonATranslation = velocity;
insilmaril@656: 
insilmaril@656: polygonA.Offset(polygonATranslation);
insilmaril@656: 
insilmaril@656: */
insilmaril@656: 
insilmaril@656: