11 QRectF addBBox(QRectF r1, QRectF r2)
13 // Find smallest QRectF containing given rectangles
17 if (r1.left() <= r2.left())
23 if (r1.top() <= r2.top())
29 if (r1.right() <= r2.right())
30 n.setRight(r2.right());
32 n.setRight(r1.right());
35 if (r1.bottom() <= r2.bottom())
36 n.setBottom(r2.bottom());
38 n.setBottom(r1.bottom());
42 QSize addBBoxSize(QSize s1, QSize s2)
44 // Find the minimimum smallest QSize which could include 2 given sizes
47 if (s1.width() <= s2.width())
48 s.setWidth(s2.width());
49 if (s1.height() <= s2.height())
50 s.setHeight(s2.height());
54 bool isInBox(const QPointF &p, const QRectF &box)
56 if (p.x() >= box.left() && p.x() <= box.right() && p.y() <= box.bottom() &&
62 qreal Geometry::distance(const QPointF &p, const QPointF &q)
64 return sqrt(p.x() * q.x() + p.y() * q.y());
67 Vector::Vector() : QPointF() {}
69 Vector::Vector(const QPointF &p) : QPointF(p) {}
71 Vector::Vector(qreal x, qreal y) : QPointF(x, y) {}
75 //! Check if length is 0
78 if (x() == 0 && y() == 0)
84 void Vector::normalize()
88 qreal l = sqrt(x() * x() + y() * y());
93 //! Dot product of two vectors
94 qreal Vector::dotProduct(const QPointF &b) { return x() * b.x() + y() * b.y(); }
96 void Vector::scale(const qreal &f)
102 void Vector::invert()
108 QPointF Vector::toQPointF() { return QPointF(x(), y()); }
110 /*! Calculate the projection of a polygon on an axis
111 and returns it as a [min, max] interval */
112 ConvexPolygon::ConvexPolygon() {}
114 ConvexPolygon::ConvexPolygon(QPolygonF p) : QPolygonF(p) {}
116 ConvexPolygon::~ConvexPolygon() {}
118 void ConvexPolygon::calcCentroid()
120 // Calculate area and centroid
121 // http://en.wikipedia.org/wiki/Centroid
127 for (int i = 0; i < size() - 1; i++) {
128 p = at(i).x() * at(i + 1).y() - at(i + 1).x() * at(i).y();
130 cx += (at(i).x() + at(i + 1).x()) * p;
131 cy += (at(i).y() + at(i + 1).y()) * p;
134 // area is negative if vertices ordered counterclockwise
135 // (in mirrored graphicsview!)
138 _centroid.setX(cx / p);
139 _centroid.setY(cy / p);
142 QPointF ConvexPolygon::centroid() const { return _centroid; }
144 qreal ConvexPolygon::weight() const { return _area; }
146 std::string ConvexPolygon::toStdString()
149 for (int i = 0; i < size(); ++i) {
150 s += QString("(%1,%2)").arg(at(i).x()).arg(at(i).y());
155 return s.toStdString();
158 Vector ConvexPolygon::at(const int &i) const
160 return Vector(QPolygonF::at(i).x(), QPolygonF::at(i).y());
163 void ConvexPolygon::translate(const Vector &offset)
165 translate(offset.x(), offset.y());
168 void ConvexPolygon::translate(qreal dx, qreal dy)
170 QPolygonF::translate(dx, dy);
171 _centroid = _centroid + QPointF(dx, dy);
174 void projectPolygon(Vector axis, ConvexPolygon polygon, qreal &min, qreal &max)
176 // To project a point on an axis use the dot product
178 // qDebug() << "Projecting on "<< axis;
179 qreal d = axis.dotProduct(polygon.at(0));
182 for (int i = 0; i < polygon.size(); i++) {
183 d = polygon.at(i).dotProduct(axis);
188 // qDebug() << "p="<<polygon.at(i)<<" d="<<d<<" (min,
189 //max)=("<<min<<","<<max<<")";
193 // Calculate the signed distance between [minA, maxA] and [minB, maxB]
194 // The distance will be negative if the intervals overlap
196 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB)
207 Check if polygon A is going to collide with polygon B.
208 The last parameter is the *relative* velocity
209 of the polygons (i.e. velocityA - velocityB)
212 PolygonCollisionResult polygonCollision(ConvexPolygon polygonA,
213 ConvexPolygon polygonB, Vector velocity)
215 PolygonCollisionResult result;
216 result.intersect = true;
217 result.willIntersect = true;
219 int edgeCountA = polygonA.size();
220 int edgeCountB = polygonB.size();
221 qreal minIntervalDistance = 1000000000;
222 QPointF translationAxis;
227 for (int k=0; k<edgeCountA;k++)
228 qDebug() <<polygonA.at(k);
230 for (int k=0; k<edgeCountB;k++)
231 qDebug() <<polygonB.at(k);
235 // Loop through all the edges of both polygons
236 for (int i = 0; i < edgeCountA + edgeCountB; i++) {
237 if (i < edgeCountA) {
238 // Loop through polygon A
239 if (i < edgeCountA - 1)
240 edge = QPointF(polygonA.at(i + 1).x() - polygonA.at(i).x(),
241 polygonA.at(i + 1).y() - polygonA.at(i).y());
243 edge = QPointF(polygonA.at(0).x() - polygonA.at(i).x(),
244 polygonA.at(0).y() - polygonA.at(i).y());
247 // Loop through polygon B
248 if (i < edgeCountA + edgeCountB - 1)
249 edge = QPointF(polygonB.at(i - edgeCountA + 1).x() -
250 polygonB.at(i - edgeCountA).x(),
251 polygonB.at(i - edgeCountA + 1).y() -
252 polygonB.at(i - edgeCountA).y());
255 polygonB.at(0).x() - polygonB.at(i - edgeCountA).x(),
256 polygonB.at(0).y() - polygonB.at(i - edgeCountA).y());
259 // ===== 1. Find if the polygons are currently intersecting =====
261 // Find the axis perpendicular to the current edge
263 Vector axis(-edge.y(), edge.x());
266 // Find the projection of the polygon on the current axis
272 projectPolygon(axis, polygonA, minA, maxA);
273 projectPolygon(axis, polygonB, minB, maxB);
275 // Check if the polygon projections are currentlty intersecting
277 qreal d = intervalDistance(minA, maxA, minB, maxB);
279 result.intersect = false;
281 // ===== 2. Now find if the polygons *will* intersect =====
283 // Project the velocity on the current axis
285 qreal velocityProjection = axis.dotProduct(velocity);
287 // Get the projection of polygon A during the movement
289 if (velocityProjection < 0)
290 minA += velocityProjection;
292 maxA += velocityProjection;
294 // Do the same test as above for the new projection
296 // d = intervalDistance(minA, maxA, minB, maxB);
297 // if (d > 0) result.willIntersect = false;
300 qDebug() << "edge="<<edge<<" ";
301 qDebug() <<"axis="<<axis<<" ";
302 qDebug() <<"dA=("<<minA<<","<<maxA<<") dB=("<<minB<<","<<maxB<<")";
303 qDebug() <<" d="<<d<<" ";
304 //qDebug() <<"minD="<<minIntervalDistance<<" ";
305 qDebug() <<"int="<<result.intersect<<" ";
306 //qDebug() <<"wint="<<result.willIntersect<<" ";
307 //qDebug() <<"velProj="<<velocityProjection<<" ";
311 if (result.intersect) // || result.willIntersect)
313 // Check if the current interval distance is the minimum one. If so
314 // store the interval distance and the current distance. This will
315 // be used to calculate the minimum translation vector
319 if (d < minIntervalDistance) {
320 minIntervalDistance = d;
321 // translationAxis = axis;
322 // qDebug() << "tAxix="<<translationAxis;
324 // QPointF t = polygonA.Center - polygonB.Center;
325 // QPointF t = polygonA.at(0) - polygonB.at(0);
326 // if (dotProduct(t,translationAxis) < 0)
327 // translationAxis = -translationAxis;
332 // The minimum translation vector
333 // can be used to push the polygons appart.
335 if (result.willIntersect)
336 result.minTranslation = translationAxis * minIntervalDistance;
341 /* The function can be used this way:
342 QPointF polygonATranslation = new QPointF();
346 PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
349 // Move the polygon by its velocity, then move
350 // the polygons appart using the Minimum Translation Vector
351 polygonATranslation = velocity + r.minTranslation;
353 // Just move the polygon by its velocity
354 polygonATranslation = velocity;
356 polygonA.Offset(polygonATranslation);