12 QRectF addBBox(QRectF r1, QRectF r2)
14 // Find smallest QRectF containing given rectangles
18 if (r1.left() <= r2.left() )
19 n.setLeft(r1.left() );
21 n.setLeft(r2.left() );
24 if (r1.top() <= r2.top() )
30 if (r1.right() <= r2.right() )
31 n.setRight(r2.right() );
33 n.setRight(r1.right() );
36 if (r1.bottom() <= r2.bottom() )
37 n.setBottom(r2.bottom() );
39 n.setBottom(r1.bottom() );
43 QSize addBBoxSize (QSize s1, QSize s2)
45 // Find the minimimum smallest QSize which could include 2 given sizes
48 if (s1.width() <= s2.width() )
49 s.setWidth (s2.width() );
50 if (s1.height() <= s2.height() )
51 s.setHeight(s2.height() );
55 bool isInBox(const QPointF &p, const QRectF &box)
57 if (p.x() >= box.left() && p.x() <= box.right()
58 && p.y() <= box.bottom() && p.y() >= box.top() )
63 qreal Geometry::distance (const QPointF &p, const QPointF &q)
65 return sqrt (p.x()*q.x() + p.y()*q.y());
68 Vector::Vector ():QPointF ()
72 Vector::Vector (const QPointF &p):QPointF (p)
76 Vector::Vector (qreal x, qreal y):QPointF (x,y)
84 //! Check if length is 0
93 void Vector::normalize ()
95 if (isNull() ) return;
96 qreal l=sqrt ( x()*x() + y()*y() );
101 //! Dot product of two vectors
102 qreal Vector::dotProduct (const QPointF &b)
104 return x()*b.x() + y()*b.y();
108 void Vector::scale (const qreal &f)
114 void Vector::invert ()
120 QPointF Vector::toQPointF ()
122 return QPointF (x(),y());
125 /*! Calculate the projection of a polygon on an axis
126 and returns it as a [min, max] interval */
127 ConvexPolygon::ConvexPolygon ()
131 ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)
135 ConvexPolygon::~ConvexPolygon ()
139 void ConvexPolygon::calcCentroid()
141 // Calculate area and centroid
142 // http://en.wikipedia.org/wiki/Centroid
148 for (int i=0;i<size()-1;i++)
150 p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();
152 cx+=(at(i).x()+at(i+1).x()) * p;
153 cy+=(at(i).y()+at(i+1).y()) * p;
156 // area is negative if vertices ordered counterclockwise
157 // (in mirrored graphicsview!)
160 _centroid.setX (cx/p);
161 _centroid.setY (cy/p);
164 QPointF ConvexPolygon::centroid() const
169 qreal ConvexPolygon::weight() const
174 std::string ConvexPolygon::toStdString()
177 for (int i=0;i<size();++i)
179 s+=QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());
180 if (i<size()-1) s+=",";
183 return s.toStdString();
186 Vector ConvexPolygon::at(const int &i) const
188 return Vector (QPolygonF::at(i).x(),QPolygonF::at(i).y());
191 void ConvexPolygon::translate ( const Vector & offset )
192 { translate (offset.x(),offset.y());}
194 void ConvexPolygon::translate ( qreal dx, qreal dy )
196 QPolygonF::translate (dx,dy);
197 _centroid=_centroid+QPointF (dx,dy);
200 void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max)
202 // To project a point on an axis use the dot product
204 //qDebug() << "Projecting on "<< axis;
205 qreal d = axis.dotProduct(polygon.at(0));
208 for (int i = 0; i < polygon.size(); i++)
210 d= polygon.at(i).dotProduct (axis);
215 // qDebug() << "p="<<polygon.at(i)<<" d="<<d<<" (min, max)=("<<min<<","<<max<<")";
219 // Calculate the signed distance between [minA, maxA] and [minB, maxB]
220 // The distance will be negative if the intervals overlap
222 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
231 Check if polygon A is going to collide with polygon B.
232 The last parameter is the *relative* velocity
233 of the polygons (i.e. velocityA - velocityB)
236 PolygonCollisionResult polygonCollision(ConvexPolygon polygonA,
237 ConvexPolygon polygonB, Vector velocity)
239 PolygonCollisionResult result;
240 result.intersect = true;
241 result.willIntersect = true;
243 int edgeCountA = polygonA.size();
244 int edgeCountB = polygonB.size();
245 qreal minIntervalDistance = 1000000000;
246 QPointF translationAxis;
251 for (int k=0; k<edgeCountA;k++)
252 qDebug() <<polygonA.at(k);
254 for (int k=0; k<edgeCountB;k++)
255 qDebug() <<polygonB.at(k);
259 // Loop through all the edges of both polygons
260 for (int i=0;i<edgeCountA + edgeCountB;i++)
264 // Loop through polygon A
267 polygonA.at(i+1).x()-polygonA.at(i).x(),
268 polygonA.at(i+1).y()-polygonA.at(i).y());
271 polygonA.at(0).x()-polygonA.at(i).x(),
272 polygonA.at(0).y()-polygonA.at(i).y());
275 // Loop through polygon B
276 if (i < edgeCountA +edgeCountB -1 )
278 polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(),
279 polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
282 polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(),
283 polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
286 // ===== 1. Find if the polygons are currently intersecting =====
288 // Find the axis perpendicular to the current edge
290 Vector axis (-edge.y(), edge.x());
293 // Find the projection of the polygon on the current axis
295 qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
296 projectPolygon(axis, polygonA, minA, maxA);
297 projectPolygon(axis, polygonB, minB, maxB);
299 // Check if the polygon projections are currentlty intersecting
301 qreal d = intervalDistance(minA, maxA, minB, maxB);
302 if (d > 0) result.intersect = false;
304 // ===== 2. Now find if the polygons *will* intersect =====
307 // Project the velocity on the current axis
309 qreal velocityProjection = axis.dotProduct(velocity);
311 // Get the projection of polygon A during the movement
313 if (velocityProjection < 0)
314 minA += velocityProjection;
316 maxA += velocityProjection;
318 // Do the same test as above for the new projection
320 // d = intervalDistance(minA, maxA, minB, maxB);
321 //if (d > 0) result.willIntersect = false;
324 qDebug() << "edge="<<edge<<" ";
325 qDebug() <<"axis="<<axis<<" ";
326 qDebug() <<"dA=("<<minA<<","<<maxA<<") dB=("<<minB<<","<<maxB<<")";
327 qDebug() <<" d="<<d<<" ";
328 //qDebug() <<"minD="<<minIntervalDistance<<" ";
329 qDebug() <<"int="<<result.intersect<<" ";
330 //qDebug() <<"wint="<<result.willIntersect<<" ";
331 //qDebug() <<"velProj="<<velocityProjection<<" ";
335 if (result.intersect )// || result.willIntersect)
337 // Check if the current interval distance is the minimum one. If so
338 // store the interval distance and the current distance. This will
339 // be used to calculate the minimum translation vector
342 if (d < minIntervalDistance) {
343 minIntervalDistance = d;
344 //translationAxis = axis;
345 //qDebug() << "tAxix="<<translationAxis;
347 //QPointF t = polygonA.Center - polygonB.Center;
348 //QPointF t = polygonA.at(0) - polygonB.at(0);
349 //if (dotProduct(t,translationAxis) < 0)
350 // translationAxis = -translationAxis;
355 // The minimum translation vector
356 // can be used to push the polygons appart.
358 if (result.willIntersect)
359 result.minTranslation =
360 translationAxis * minIntervalDistance;
365 /* The function can be used this way:
366 QPointF polygonATranslation = new QPointF();
371 PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
374 // Move the polygon by its velocity, then move
375 // the polygons appart using the Minimum Translation Vector
376 polygonATranslation = velocity + r.minTranslation;
378 // Just move the polygon by its velocity
379 polygonATranslation = velocity;
381 polygonA.Offset(polygonATranslation);