geometry.cpp
author insilmaril
Tue, 06 Apr 2010 13:30:07 +0000
changeset 843 2d36a7bb0867
parent 835 31841b366d5e
permissions -rw-r--r--
(Very) minor changes for debugging output
     1 #include "geometry.h"
     2 
     3 #include <math.h>
     4 #include "misc.h"
     5 
     6 #include <QString>
     7 
     8 #include <iostream>
     9 using namespace std;
    10 
    11 
    12 QRectF addBBox(QRectF r1, QRectF r2)
    13 {	
    14 	// Find smallest QRectF containing given rectangles
    15 
    16 	QRectF n;
    17 	// Set left border
    18 	if (r1.left() <= r2.left() )
    19 		n.setLeft(r1.left() );
    20 	else
    21 		n.setLeft(r2.left() );
    22 		
    23 	// Set top border		
    24 	if (r1.top() <= r2.top() )
    25 		n.setTop(r1.top() );
    26 	else
    27 		n.setTop(r2.top() );
    28 		
    29 	// Set right border
    30 	if (r1.right() <= r2.right() )
    31 		n.setRight(r2.right() );
    32 	else
    33 		n.setRight(r1.right() );
    34 		
    35 	// Set bottom 
    36 	if (r1.bottom() <= r2.bottom() )
    37 		n.setBottom(r2.bottom() );
    38 	else
    39 		n.setBottom(r1.bottom() );
    40 	return n;
    41 }
    42 
    43 bool isInBox(const QPointF &p, const QRectF &box)
    44 {
    45     if (p.x() >= box.left() && p.x() <= box.right()  
    46 	&& p.y() <= box.bottom() && p.y() >= box.top() )
    47 		return true;
    48     return false;	
    49 }
    50 
    51 qreal distance (const QPointF &p, const QPointF &q)
    52 {
    53 	return sqrt (p.x()*q.x() + p.y()*q.y());
    54 }
    55 
    56 Vector::Vector ():QPointF ()
    57 {
    58 }
    59 
    60 Vector::Vector (const QPointF &p):QPointF (p)
    61 {
    62 }
    63 
    64 Vector::Vector (qreal x, qreal y):QPointF (x,y)
    65 {
    66 }
    67 
    68 //! Check if length is 0
    69 bool Vector::isNull()
    70 {
    71 	if (x()==0 && y()==0)
    72 		return true;
    73 	return false;	
    74 }
    75 
    76 //! Normalize vector
    77 void Vector::normalize ()
    78 {
    79 	if (isNull() ) return;
    80 	qreal l=sqrt ( x()*x() + y()*y() );
    81 	setX (x()/l);
    82 	setY (y()/l);
    83 }
    84 
    85 //! Dot product of two vectors
    86 qreal Vector::dotProduct (const QPointF &b)
    87 {
    88 	return x()*b.x() + y()*b.y();
    89 }
    90 
    91 
    92 void Vector::scale (const qreal &f)
    93 {
    94 	setX (x()*f);
    95 	setY (y()*f);
    96 }
    97 
    98 void Vector::invert ()
    99 {
   100 	setX (-x());
   101 	setY (-y());
   102 }
   103 
   104 QPointF Vector::toQPointF ()
   105 {
   106 	return QPointF (x(),y());
   107 }
   108 
   109 /*! Calculate the projection of a polygon on an axis
   110     and returns it as a [min, max] interval  */
   111 ConvexPolygon::ConvexPolygon ()
   112 {
   113 }
   114 
   115 ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)
   116 {
   117 }
   118 
   119 void ConvexPolygon::calcCentroid() 
   120 {
   121 	// Calculate area and centroid
   122 	// http://en.wikipedia.org/wiki/Centroid
   123 	qreal cx,cy,p;
   124 	cx=cy=0;
   125 	_area=0;
   126 
   127 	append (at(0));
   128 	for (int i=0;i<size()-1;i++)
   129 	{
   130 		p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();
   131 		_area+=p;
   132 		cx+=(at(i).x()+at(i+1).x()) * p;
   133 		cy+=(at(i).y()+at(i+1).y()) * p;
   134 	}	
   135 	pop_back();
   136 	// area is negative if vertices ordered counterclockwise
   137 	// (in mirrored graphicsview!)
   138 	_area=_area/2;	
   139 	p=_area*6;
   140 	_centroid.setX (cx/p);
   141 	_centroid.setY (cy/p);
   142 }
   143 
   144 QPointF ConvexPolygon::centroid() const
   145 {
   146 	return _centroid;
   147 }
   148 
   149 qreal ConvexPolygon::weight() const
   150 {
   151 	return _area;
   152 }
   153 
   154 std::string ConvexPolygon::toStdString()
   155 {
   156 	QString s ("(");
   157 	for (int i=0;i<size();++i)
   158 	{
   159 		s+=QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());
   160 		if (i<size()-1) s+=",";
   161 	}
   162 	s+=")";	
   163 	return s.toStdString();
   164 }
   165 
   166 Vector ConvexPolygon::at(const int &i) const
   167 {
   168 	return Vector (QPolygonF::at(i).x(),QPolygonF::at(i).y());
   169 }
   170 
   171 void ConvexPolygon::translate ( const Vector & offset )
   172 { translate (offset.x(),offset.y());}
   173 
   174 void ConvexPolygon::translate ( qreal dx, qreal dy )
   175 {
   176 	QPolygonF::translate (dx,dy);
   177 	_centroid=_centroid+QPointF (dx,dy);
   178 }
   179 
   180 void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max) 
   181 {
   182     // To project a point on an axis use the dot product
   183 
   184 	//cout << "Projecting on "<< axis<<endl;
   185     qreal d = axis.dotProduct(polygon.at(0));
   186     min = d;
   187     max = d;
   188     for (int i = 0; i < polygon.size(); i++) 
   189 	{
   190         d= polygon.at(i).dotProduct (axis);
   191         if (d < min) 
   192             min = d;
   193         else 
   194             if (d> max) max = d;
   195 	//	cout << "p="<<polygon.at(i)<<"  d="<<d<<"  (min, max)=("<<min<<","<<max<<")\n";	
   196     }
   197 }
   198 
   199 // Calculate the signed distance between [minA, maxA] and [minB, maxB]
   200 // The distance will be negative if the intervals overlap
   201 
   202 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
   203     if (minA < minB) {
   204         return minB - maxA;
   205     } else {
   206         return minA - maxB;
   207     }
   208 }
   209 
   210 /*!
   211  Check if polygon A is going to collide with polygon B.
   212  The last parameter is the *relative* velocity 
   213  of the polygons (i.e. velocityA - velocityB)
   214 */
   215 
   216 PolygonCollisionResult polygonCollision(ConvexPolygon polygonA, 
   217                               ConvexPolygon polygonB, Vector velocity) 
   218 {
   219     PolygonCollisionResult result;
   220     result.intersect = true;
   221     result.willIntersect = true;
   222 
   223     int edgeCountA = polygonA.size();
   224     int edgeCountB = polygonB.size();
   225     qreal minIntervalDistance = 1000000000;
   226     QPointF translationAxis;
   227     QPointF edge;
   228 
   229 /*
   230 	cout << "\nA: ";
   231 	for (int k=0; k<edgeCountA;k++)
   232 		cout <<polygonA.at(k);
   233 	cout << "\nB: ";
   234 	for (int k=0; k<edgeCountB;k++)
   235 		cout <<polygonB.at(k);
   236 	cout <<endl;	
   237 */		
   238 		
   239     // Loop through all the edges of both polygons
   240 	for (int i=0;i<edgeCountA + edgeCountB;i++)
   241 	{
   242         if (i< edgeCountA) 
   243 		{
   244 			// Loop through polygon A
   245 			if (i<edgeCountA-1)
   246 				edge = QPointF (
   247 					polygonA.at(i+1).x()-polygonA.at(i).x(), 
   248 					polygonA.at(i+1).y()-polygonA.at(i).y());
   249 			else		
   250 				edge = QPointF (
   251 					polygonA.at(0).x()-polygonA.at(i).x(), 
   252 					polygonA.at(0).y()-polygonA.at(i).y());
   253         } else 
   254 		{
   255 			// Loop through polygon B
   256 			if (i < edgeCountA +edgeCountB -1 )
   257 				edge = QPointF (
   258 					polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(), 
   259 					polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
   260 			else	
   261 				edge = QPointF (
   262 					polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(), 
   263 					polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
   264 		}
   265 
   266         // ===== 1. Find if the polygons are currently intersecting =====
   267 
   268         // Find the axis perpendicular to the current edge
   269 
   270         Vector axis (-edge.y(), edge.x());
   271         axis.normalize();
   272 
   273         // Find the projection of the polygon on the current axis
   274 
   275         qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
   276         projectPolygon(axis, polygonA, minA, maxA);
   277         projectPolygon(axis, polygonB, minB, maxB);
   278 
   279         // Check if the polygon projections are currentlty intersecting
   280 
   281         qreal d = intervalDistance(minA, maxA, minB, maxB);
   282         if (d > 0) result.intersect = false;
   283 
   284        // ===== 2. Now find if the polygons *will* intersect =====
   285 
   286 
   287         // Project the velocity on the current axis
   288 
   289         qreal velocityProjection = axis.dotProduct(velocity);
   290 
   291         // Get the projection of polygon A during the movement
   292 
   293         if (velocityProjection < 0) 
   294             minA += velocityProjection;
   295         else 
   296             maxA += velocityProjection;
   297 
   298         // Do the same test as above for the new projection
   299 
   300         // d = intervalDistance(minA, maxA, minB, maxB);
   301         //if (d > 0) result.willIntersect = false;
   302 		/*
   303 		cout <<"   ";
   304 		cout << "edge="<<edge<<"  ";
   305 		cout <<"axis="<<axis<<"  ";
   306 		cout <<"dA=("<<minA<<","<<maxA<<")  dB=("<<minB<<","<<maxB<<")";
   307 		cout <<"  d="<<d<<"   ";
   308 		//cout <<"minD="<<minIntervalDistance<<"  ";
   309 		cout <<"int="<<result.intersect<<"  ";
   310 		//cout <<"wint="<<result.willIntersect<<"  ";
   311 		//cout <<"velProj="<<velocityProjection<<"  ";
   312 		cout <<endl;
   313 		*/
   314 	
   315         if (result.intersect )// || result.willIntersect) 
   316 		{
   317 			// Check if the current interval distance is the minimum one. If so
   318 			// store the interval distance and the current distance.  This will
   319 			// be used to calculate the minimum translation vector
   320 
   321 			if (d<0) d=-d;
   322 			if (d < minIntervalDistance) {
   323 				minIntervalDistance = d;
   324 				//translationAxis = axis;
   325 				//cout << "tAxix="<<translationAxis<<endl;
   326 
   327 				//QPointF t = polygonA.Center - polygonB.Center;
   328 				//QPointF t = polygonA.at(0) - polygonB.at(0);
   329 				//if (dotProduct(t,translationAxis) < 0)
   330 				//	translationAxis = -translationAxis;
   331 			}
   332 		}
   333     }
   334 
   335     // The minimum translation vector
   336     // can be used to push the polygons appart.
   337 
   338     if (result.willIntersect)
   339         result.minTranslation = 
   340                translationAxis * minIntervalDistance;
   341     
   342     return result;
   343 }
   344 
   345 /* The function can be used this way: 
   346    QPointF polygonATranslation = new QPointF();
   347 */   
   348 
   349 
   350 /*
   351 PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
   352 
   353 if (r.WillIntersect) 
   354   // Move the polygon by its velocity, then move
   355   // the polygons appart using the Minimum Translation Vector
   356   polygonATranslation = velocity + r.minTranslation;
   357 else 
   358   // Just move the polygon by its velocity
   359   polygonATranslation = velocity;
   360 
   361 polygonA.Offset(polygonATranslation);
   362 
   363 */
   364 
   365