src/geometry.cpp

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