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