geometry.cpp

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