geometry.cpp
author insilmaril
Tue, 08 Apr 2008 08:28:37 +0000
changeset 682 16af22e2c155
parent 662 9abdd5e6c3dc
child 754 db0ec4bcf416
permissions -rw-r--r--
1.11.7: Fixed hanging find function
     1 #include "geometry.h"
     2 
     3 #include <math.h>
     4 #include <iostream>
     5 #include "misc.h"
     6 
     7 using namespace std;
     8 
     9 QRectF addBBox(QRectF r1, QRectF r2)
    10 {	
    11 	// Find smallest QRectF containing given rectangles
    12 
    13 	QRectF n;
    14 	// Set left border
    15 	if (r1.left() <= r2.left() )
    16 		n.setLeft(r1.left() );
    17 	else
    18 		n.setLeft(r2.left() );
    19 		
    20 	// Set top border		
    21 	if (r1.top() <= r2.top() )
    22 		n.setTop(r1.top() );
    23 	else
    24 		n.setTop(r2.top() );
    25 		
    26 	// Set right border
    27 	if (r1.right() <= r2.right() )
    28 		n.setRight(r2.right() );
    29 	else
    30 		n.setRight(r1.right() );
    31 		
    32 	// Set bottom 
    33 	if (r1.bottom() <= r2.bottom() )
    34 		n.setBottom(r2.bottom() );
    35 	else
    36 		n.setBottom(r1.bottom() );
    37 	return n;
    38 }
    39 
    40 bool inBox(const QPointF &p, const QRectF &box)
    41 {
    42     if (p.x() >= box.left() && p.x() <= box.right()  
    43 	&& p.y() <= box.bottom() && p.y() >= box.top() )
    44 		return true;
    45     return false;	
    46 }
    47 
    48 QPointF normalize (const QPointF &p)
    49 {
    50 	if (p==QPointF(0,0)) return p;
    51 	qreal l=sqrt ( p.x()*p.x() + p.y()*p.y() );
    52 	return QPointF (p.x()/l,p.y()/l);
    53 }
    54 
    55 // Dot product of two vectors
    56 qreal dotProduct (const QPointF &a, const QPointF &b)
    57 {
    58 	return a.x()*b.x() + a.y()*b.y();
    59 }
    60 
    61 
    62 /* Calculate the projection of a polygon on an axis
    63    and returns it as a [min, max] interval
    64 */
    65 void ProjectPolygon(QPointF axis, QPolygonF polygon, qreal &min, qreal &max) 
    66 {
    67     // To project a point on an axis use the dot product
    68 
    69     qreal d = dotProduct(axis,polygon.at(0));
    70     min = d;
    71     max = d;
    72     for (int i = 0; i < polygon.size(); i++) {
    73         d= dotProduct (polygon.at(i),axis);
    74         if (d < min) 
    75             min = d;
    76         else 
    77 		{
    78             if (d> max) max = d;
    79         }
    80     }
    81 }
    82 
    83 /* Calculate the signed distance between [minA, maxA] and [minB, maxB]
    84    The distance will be negative if the intervals overlap
    85 */
    86 
    87 
    88 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
    89     if (minA < minB) {
    90         return minB - maxA;
    91     } else {
    92         return minA - maxB;
    93     }
    94 }
    95 /*
    96  Check if polygon A is going to collide with polygon B.
    97  The last parameter is the *relative* velocity 
    98  of the polygons (i.e. velocityA - velocityB)
    99 
   100 */
   101 PolygonCollisionResult PolygonCollision(QPolygonF polygonA, 
   102                               QPolygonF polygonB, QPointF velocity) {
   103     PolygonCollisionResult result;
   104     result.intersect = true;
   105     result.willIntersect = true;
   106 
   107     int edgeCountA = polygonA.size();
   108     int edgeCountB = polygonB.size();
   109     qreal minIntervalDistance = 1000000000;
   110     QPointF translationAxis;
   111     QPointF edge;
   112 
   113 	cout << "\nA: ";
   114 	for (int k=0; k<edgeCountA;k++)
   115 		cout <<polygonA.at(k);
   116 	cout << "\nB: ";
   117 	for (int k=0; k<edgeCountB;k++)
   118 		cout <<polygonB.at(k);
   119 		
   120 		
   121     // Loop through all the edges of both polygons
   122 	int i=0;
   123 	int j=0;
   124 	while (i<edgeCountA && j<edgeCountB)
   125 	{
   126         if (i< edgeCountA) 
   127 		{
   128 			if (i<edgeCountA - 1)
   129 				edge = QPointF (
   130 					polygonA.at(i+1).x()-polygonA.at(i).x(), 
   131 					polygonA.at(i+1).y()-polygonA.at(i).y());
   132 			else		
   133 				edge = QPointF (
   134 					polygonA.at(0).x()-polygonA.at(i).x(), 
   135 					polygonA.at(0).y()-polygonA.at(i).y());
   136 			i++;		
   137         } else 
   138 		{
   139 			if (i < edgeCountB -1)
   140 				edge = QPointF (
   141 					polygonB.at(j+1).x() - polygonA.at(i).x(), 
   142 					polygonB.at(j+1).y() - polygonA.at(i).y());
   143 			else	
   144 				edge = QPointF (
   145 					polygonB.at(0).x() - polygonA.at(i).x(), 
   146 					polygonB.at(0).y() - polygonA.at(i).y());
   147 			j++;
   148 		}
   149 
   150         // ===== 1. Find if the polygons are currently intersecting =====
   151 
   152 
   153         // Find the axis perpendicular to the current edge
   154 
   155         QPointF axis (-edge.y(), edge.x());
   156         axis=normalize(axis);
   157 
   158         // Find the projection of the polygon on the current axis
   159 
   160         qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
   161         ProjectPolygon(axis, polygonA, minA, maxA);
   162         ProjectPolygon(axis, polygonB, minB, maxB);
   163 
   164         // Check if the polygon projections are currentlty intersecting
   165 
   166         if (intervalDistance(minA, maxA, minB, maxB) > 0)
   167             result.intersect = false;
   168 		else	
   169             result.intersect = true;
   170 
   171         // ===== 2. Now find if the polygons *will* intersect =====
   172 
   173 
   174         // Project the velocity on the current axis
   175 
   176         qreal velocityProjection = dotProduct(axis,velocity);
   177 
   178         // Get the projection of polygon A during the movement
   179 
   180         if (velocityProjection < 0) 
   181             minA += velocityProjection;
   182         else 
   183             maxA += velocityProjection;
   184         
   185 
   186         // Do the same test as above for the new projection
   187 
   188         qreal d = intervalDistance(minA, maxA, minB, maxB);
   189         if (d > 0) result.willIntersect = false;
   190 		/*
   191 		*/
   192 		cout <<"   ";
   193 		cout <<"minA="<<minA<<"  ";
   194 		cout <<"maxA="<<maxA<<"  ";
   195 		cout <<"minB="<<minB<<"  ";
   196 		cout <<"maxB="<<maxB<<"  ";
   197 		cout <<"  d="<<d<<"   ";
   198 		cout <<"minD="<<minIntervalDistance<<"  ";
   199 		cout <<"axis="<<axis<<"  ";
   200 		cout <<"int="<<result.intersect<<"  ";
   201 		cout <<"wint="<<result.willIntersect<<"  ";
   202 		//cout <<"velProj="<<velocityProjection<<"  ";
   203 		cout <<endl;
   204 
   205 
   206 	
   207         if (result.intersect || result.willIntersect) 
   208 		{
   209 			// Check if the current interval distance is the minimum one. If so
   210 			// store the interval distance and the current distance.  This will
   211 			// be used to calculate the minimum translation vector
   212 
   213 			if (d<0) d=-d;
   214 			if (d < minIntervalDistance) {
   215 				minIntervalDistance = d;
   216 				translationAxis = axis;
   217 				cout << "tAxix="<<translationAxis<<endl;
   218 
   219 				//QPointF t = polygonA.Center - polygonB.Center;
   220 				QPointF t = polygonA.at(0) - polygonB.at(0);
   221 				if (dotProduct(t,translationAxis) < 0)
   222 					translationAxis = -translationAxis;
   223 			}
   224 		}
   225     }
   226 
   227     // The minimum translation vector
   228     // can be used to push the polygons appart.
   229 
   230     if (result.willIntersect)
   231         result.minTranslation = 
   232                translationAxis * minIntervalDistance;
   233     
   234     return result;
   235 }
   236 
   237 /* The function can be used this way: 
   238    QPointF polygonATranslation = new QPointF();
   239 */   
   240 
   241 
   242 /*
   243 PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
   244 
   245 if (r.WillIntersect) 
   246   // Move the polygon by its velocity, then move
   247   // the polygons appart using the Minimum Translation Vector
   248   polygonATranslation = velocity + r.minTranslation;
   249 else 
   250   // Just move the polygon by its velocity
   251   polygonATranslation = velocity;
   252 
   253 polygonA.Offset(polygonATranslation);
   254 
   255 */
   256 
   257