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