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