git.maemo.org Git - vym/blob - geometry.cpp

   1 #include "geometry.h"
   2
   3 #include <math.h>
   4 #include <iostream>
   5 #include "misc.h"
   6
   7 using namespace std;
   8
   9 QRectF addBBox(QRectF r1, QRectF r2)
  10 {
  11         // Find smallest QRectF containing given rectangles
  12
  13         QRectF n;
  14         // Set left border
  15         if (r1.left() <= r2.left() )
  16                 n.setLeft(r1.left() );
  17         else
  18                 n.setLeft(r2.left() );
  19
  20         // Set top border
  21         if (r1.top() <= r2.top() )
  22                 n.setTop(r1.top() );
  23         else
  24                 n.setTop(r2.top() );
  25
  26         // Set right border
  27         if (r1.right() <= r2.right() )
  28                 n.setRight(r2.right() );
  29         else
  30                 n.setRight(r1.right() );
  31
  32         // Set bottom
  33         if (r1.bottom() <= r2.bottom() )
  34                 n.setBottom(r2.bottom() );
  35         else
  36                 n.setBottom(r1.bottom() );
  37         return n;
  38 }
  39
  40 bool inBox(const QPointF &p, const QRectF &box)
  41 {
  42     if (p.x() >= box.left() && p.x() <= box.right()
  43         && p.y() <= box.bottom() && p.y() >= box.top() )
  44                 return true;
  45     return false;
  46 }
  47
  48 QPointF normalize (const QPointF &p)
  49 {
  50         if (p==QPointF(0,0)) return p;
  51         qreal l=sqrt ( p.x()*p.x() + p.y()*p.y() );
  52         return QPointF (p.x()/l,p.y()/l);
  53 }
  54
  55 // Dot product of two vectors
  56 qreal dotProduct (const QPointF &a, const QPointF &b)
  57 {
  58         return a.x()*b.x() + a.y()*b.y();
  59 }
  60
  61
  62 /* Calculate the projection of a polygon on an axis
  63    and returns it as a [min, max] interval
  64 */
  65 void ProjectPolygon(QPointF axis, QPolygonF polygon, qreal &min, qreal &max)
  66 {
  67     // To project a point on an axis use the dot product
  68
  69     qreal d = dotProduct(axis,polygon.at(0));
  70     min = d;
  71     max = d;
  72     for (int i = 0; i < polygon.size(); i++) {
  73         d= dotProduct (polygon.at(i),axis);
  74         if (d < min)
  75             min = d;
  76         else
  77                 {
  78             if (d> max) max = d;
  79         }
  80     }
  81 }
  82
  83 /* Calculate the signed distance between [minA, maxA] and [minB, maxB]
  84    The distance will be negative if the intervals overlap
  85 */
  86
  87
  88 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
  89     if (minA < minB) {
  90         return minB - maxA;
  91     } else {
  92         return minA - maxB;
  93     }
  94 }
  95 /*
  96  Check if polygon A is going to collide with polygon B.
  97  The last parameter is the *relative* velocity
  98  of the polygons (i.e. velocityA - velocityB)
  99
 100 */
 101 PolygonCollisionResult PolygonCollision(QPolygonF polygonA,
 102                               QPolygonF polygonB, QPointF velocity) {
 103     PolygonCollisionResult result;
 104     result.intersect = true;
 105     result.willIntersect = true;
 106
 107     int edgeCountA = polygonA.size();
 108     int edgeCountB = polygonB.size();
 109     qreal minIntervalDistance = 1000000000;
 110     QPointF translationAxis;
 111     QPointF edge;
 112
 113         cout << "\nA: ";
 114         for (int k=0; k<edgeCountA;k++)
 115                 cout <<polygonA.at(k);
 116         cout << "\nB: ";
 117         for (int k=0; k<edgeCountB;k++)
 118                 cout <<polygonB.at(k);
 119
 120
 121     // Loop through all the edges of both polygons
 122         int i=0;
 123         int j=0;
 124         while (i<edgeCountA && j<edgeCountB)
 125         {
 126         if (i< edgeCountA)
 127                 {
 128                         if (i<edgeCountA - 1)
 129                                 edge = QPointF (
 130                                         polygonA.at(i+1).x()-polygonA.at(i).x(),
 131                                         polygonA.at(i+1).y()-polygonA.at(i).y());
 132                         else
 133                                 edge = QPointF (
 134                                         polygonA.at(0).x()-polygonA.at(i).x(),
 135                                         polygonA.at(0).y()-polygonA.at(i).y());
 136                         i++;
 137         } else
 138                 {
 139                         if (i < edgeCountB -1)
 140                                 edge = QPointF (
 141                                         polygonB.at(j+1).x() - polygonA.at(i).x(),
 142                                         polygonB.at(j+1).y() - polygonA.at(i).y());
 143                         else
 144                                 edge = QPointF (
 145                                         polygonB.at(0).x() - polygonA.at(i).x(),
 146                                         polygonB.at(0).y() - polygonA.at(i).y());
 147                         j++;
 148                 }
 149
 150         // ===== 1. Find if the polygons are currently intersecting =====
 151
 152
 153         // Find the axis perpendicular to the current edge
 154
 155         QPointF axis (-edge.y(), edge.x());
 156         axis=normalize(axis);
 157
 158         // Find the projection of the polygon on the current axis
 159
 160         qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
 161         ProjectPolygon(axis, polygonA, minA, maxA);
 162         ProjectPolygon(axis, polygonB, minB, maxB);
 163
 164         // Check if the polygon projections are currentlty intersecting
 165
 166         if (intervalDistance(minA, maxA, minB, maxB) > 0)
 167             result.intersect = false;
 168                 else
 169             result.intersect = true;
 170
 171         // ===== 2. Now find if the polygons *will* intersect =====
 172
 173
 174         // Project the velocity on the current axis
 175
 176         qreal velocityProjection = dotProduct(axis,velocity);
 177
 178         // Get the projection of polygon A during the movement
 179
 180         if (velocityProjection < 0)
 181             minA += velocityProjection;
 182         else
 183             maxA += velocityProjection;
 184
 185
 186         // Do the same test as above for the new projection
 187
 188         qreal d = intervalDistance(minA, maxA, minB, maxB);
 189         if (d > 0) result.willIntersect = false;
 190                 /*
 191                 */
 192                 cout <<"   ";
 193                 cout <<"minA="<<minA<<"  ";
 194                 cout <<"maxA="<<maxA<<"  ";
 195                 cout <<"minB="<<minB<<"  ";
 196                 cout <<"maxB="<<maxB<<"  ";
 197                 cout <<"  d="<<d<<"   ";
 198                 cout <<"minD="<<minIntervalDistance<<"  ";
 199                 cout <<"axis="<<axis<<"  ";
 200                 cout <<"int="<<result.intersect<<"  ";
 201                 cout <<"wint="<<result.willIntersect<<"  ";
 202                 //cout <<"velProj="<<velocityProjection<<"  ";
 203                 cout <<endl;
 204
 205
 206
 207         if (result.intersect || result.willIntersect)
 208                 {
 209                         // Check if the current interval distance is the minimum one. If so
 210                         // store the interval distance and the current distance.  This will
 211                         // be used to calculate the minimum translation vector
 212
 213                         if (d<0) d=-d;
 214                         if (d < minIntervalDistance) {
 215                                 minIntervalDistance = d;
 216                                 translationAxis = axis;
 217                                 cout << "tAxix="<<translationAxis<<endl;
 218
 219                                 //QPointF t = polygonA.Center - polygonB.Center;
 220                                 QPointF t = polygonA.at(0) - polygonB.at(0);
 221                                 if (dotProduct(t,translationAxis) < 0)
 222                                         translationAxis = -translationAxis;
 223                         }
 224                 }
 225     }
 226
 227     // The minimum translation vector
 228     // can be used to push the polygons appart.
 229
 230     if (result.willIntersect)
 231         result.minTranslation =
 232                translationAxis * minIntervalDistance;
 233
 234     return result;
 235 }
 236
 237 /* The function can be used this way:
 238    QPointF polygonATranslation = new QPointF();
 239 */
 240
 241
 242 /*
 243 PolygonCollisionResult r = PolygonCollision(polygonA, polygonB, velocity);
 244
 245 if (r.WillIntersect)
 246   // Move the polygon by its velocity, then move
 247   // the polygons appart using the Minimum Translation Vector
 248   polygonATranslation = velocity + r.minTranslation;
 249 else
 250   // Just move the polygon by its velocity
 251   polygonATranslation = velocity;
 252
 253 polygonA.Offset(polygonATranslation);
 254
 255 */
 256
 257