1.1 --- a/geometry.cpp Wed Jan 16 15:45:19 2008 +0000
1.2 +++ b/geometry.cpp Wed Feb 04 16:33:16 2009 +0000
1.3 @@ -1,7 +1,10 @@
1.4 #include "geometry.h"
1.5
1.6 #include <math.h>
1.7 +#include <iostream>
1.8 +#include "misc.h"
1.9
1.10 +using namespace std;
1.11
1.12 QRectF addBBox(QRectF r1, QRectF r2)
1.13 {
1.14 @@ -44,8 +47,9 @@
1.15
1.16 QPointF normalize (const QPointF &p)
1.17 {
1.18 - qreal n=sqrt ( p.x()*p.x() + p.y()*p.y() );
1.19 - return QPointF (p.x()/n,p.y()/n);
1.20 + if (p==QPointF(0,0)) return p;
1.21 + qreal l=sqrt ( p.x()*p.x() + p.y()*p.y() );
1.22 + return QPointF (p.x()/l,p.y()/l);
1.23 }
1.24
1.25 // Dot product of two vectors
1.26 @@ -54,19 +58,6 @@
1.27 return a.x()*b.x() + a.y()*b.y();
1.28 }
1.29
1.30 -// Structure that stores the results of the PolygonCollision function
1.31 -class PolygonCollisionResult {
1.32 -public:
1.33 - // Are the polygons going to intersect forward in time?
1.34 - bool WillIntersect;
1.35 -
1.36 - // Are the polygons currently intersecting?
1.37 - bool Intersect;
1.38 -
1.39 - // The translation to apply to the first polygon to push the polygons apart.
1.40 - QPointF MinimumTranslationVector;
1.41 -};
1.42 -
1.43
1.44 /* Calculate the projection of a polygon on an axis
1.45 and returns it as a [min, max] interval
1.46 @@ -110,8 +101,8 @@
1.47 PolygonCollisionResult PolygonCollision(QPolygonF polygonA,
1.48 QPolygonF polygonB, QPointF velocity) {
1.49 PolygonCollisionResult result;
1.50 - result.Intersect = true;
1.51 - result.WillIntersect = true;
1.52 + result.intersect = true;
1.53 + result.willIntersect = true;
1.54
1.55 int edgeCountA = polygonA.size();
1.56 int edgeCountB = polygonB.size();
1.57 @@ -119,14 +110,42 @@
1.58 QPointF translationAxis;
1.59 QPointF edge;
1.60
1.61 + cout << "\nA: ";
1.62 + for (int k=0; k<edgeCountA;k++)
1.63 + cout <<polygonA.at(k);
1.64 + cout << "\nB: ";
1.65 + for (int k=0; k<edgeCountB;k++)
1.66 + cout <<polygonB.at(k);
1.67 +
1.68 +
1.69 // Loop through all the edges of both polygons
1.70 -
1.71 - for (int i=0; i < edgeCountA + edgeCountB; i++)
1.72 + int i=0;
1.73 + int j=0;
1.74 + while (i<edgeCountA && j<edgeCountB)
1.75 {
1.76 if (i< edgeCountA)
1.77 - edge = polygonA.at(i);
1.78 - else
1.79 - edge = polygonB.at(i - edgeCountA);
1.80 + {
1.81 + if (i<edgeCountA - 1)
1.82 + edge = QPointF (
1.83 + polygonA.at(i+1).x()-polygonA.at(i).x(),
1.84 + polygonA.at(i+1).y()-polygonA.at(i).y());
1.85 + else
1.86 + edge = QPointF (
1.87 + polygonA.at(0).x()-polygonA.at(i).x(),
1.88 + polygonA.at(0).y()-polygonA.at(i).y());
1.89 + i++;
1.90 + } else
1.91 + {
1.92 + if (i < edgeCountB -1)
1.93 + edge = QPointF (
1.94 + polygonB.at(j+1).x() - polygonA.at(i).x(),
1.95 + polygonB.at(j+1).y() - polygonA.at(i).y());
1.96 + else
1.97 + edge = QPointF (
1.98 + polygonB.at(0).x() - polygonA.at(i).x(),
1.99 + polygonB.at(0).y() - polygonA.at(i).y());
1.100 + j++;
1.101 + }
1.102
1.103 // ===== 1. Find if the polygons are currently intersecting =====
1.104
1.105 @@ -134,7 +153,7 @@
1.106 // Find the axis perpendicular to the current edge
1.107
1.108 QPointF axis (-edge.y(), edge.x());
1.109 - normalize(axis);
1.110 + axis=normalize(axis);
1.111
1.112 // Find the projection of the polygon on the current axis
1.113
1.114 @@ -144,8 +163,10 @@
1.115
1.116 // Check if the polygon projections are currentlty intersecting
1.117
1.118 - if (intervalDistance(minA, maxA, minB, maxB) > 0)\
1.119 - result.Intersect = false;
1.120 + if (intervalDistance(minA, maxA, minB, maxB) > 0)
1.121 + result.intersect = false;
1.122 + else
1.123 + result.intersect = true;
1.124
1.125 // ===== 2. Now find if the polygons *will* intersect =====
1.126
1.127 @@ -156,42 +177,58 @@
1.128
1.129 // Get the projection of polygon A during the movement
1.130
1.131 - if (velocityProjection < 0) {
1.132 + if (velocityProjection < 0)
1.133 minA += velocityProjection;
1.134 - } else {
1.135 + else
1.136 maxA += velocityProjection;
1.137 - }
1.138 +
1.139
1.140 // Do the same test as above for the new projection
1.141
1.142 qreal d = intervalDistance(minA, maxA, minB, maxB);
1.143 - if (d > 0) result.WillIntersect = false;
1.144 + if (d > 0) result.willIntersect = false;
1.145 + /*
1.146 + */
1.147 + cout <<" ";
1.148 + cout <<"minA="<<minA<<" ";
1.149 + cout <<"maxA="<<maxA<<" ";
1.150 + cout <<"minB="<<minB<<" ";
1.151 + cout <<"maxB="<<maxB<<" ";
1.152 + cout <<" d="<<d<<" ";
1.153 + cout <<"minD="<<minIntervalDistance<<" ";
1.154 + cout <<"axis="<<axis<<" ";
1.155 + cout <<"int="<<result.intersect<<" ";
1.156 + cout <<"wint="<<result.willIntersect<<" ";
1.157 + //cout <<"velProj="<<velocityProjection<<" ";
1.158 + cout <<endl;
1.159
1.160 - // If the polygons are not intersecting and won't intersect, exit the loop
1.161
1.162 - if (!result.Intersect && !result.WillIntersect) break;
1.163 +
1.164 + if (result.intersect || result.willIntersect)
1.165 + {
1.166 + // Check if the current interval distance is the minimum one. If so
1.167 + // store the interval distance and the current distance. This will
1.168 + // be used to calculate the minimum translation vector
1.169
1.170 - // Check if the current interval distance is the minimum one. If so store
1.171 - // the interval distance and the current distance.
1.172 - // This will be used to calculate the minimum translation vector
1.173 + if (d<0) d=-d;
1.174 + if (d < minIntervalDistance) {
1.175 + minIntervalDistance = d;
1.176 + translationAxis = axis;
1.177 + cout << "tAxix="<<translationAxis<<endl;
1.178
1.179 - if (d<0) d=-d;
1.180 - if (d < minIntervalDistance) {
1.181 - minIntervalDistance = d;
1.182 - translationAxis = axis;
1.183 -
1.184 - //QPointF t = polygonA.Center - polygonB.Center;
1.185 - QPointF t = polygonA.at(0) - polygonB.at(0);
1.186 - if (dotProduct(t,translationAxis) < 0)
1.187 - translationAxis = -translationAxis;
1.188 - }
1.189 + //QPointF t = polygonA.Center - polygonB.Center;
1.190 + QPointF t = polygonA.at(0) - polygonB.at(0);
1.191 + if (dotProduct(t,translationAxis) < 0)
1.192 + translationAxis = -translationAxis;
1.193 + }
1.194 + }
1.195 }
1.196
1.197 // The minimum translation vector
1.198 // can be used to push the polygons appart.
1.199
1.200 - if (result.WillIntersect)
1.201 - result.MinimumTranslationVector =
1.202 + if (result.willIntersect)
1.203 + result.minTranslation =
1.204 translationAxis * minIntervalDistance;
1.205
1.206 return result;
1.207 @@ -208,7 +245,7 @@
1.208 if (r.WillIntersect)
1.209 // Move the polygon by its velocity, then move
1.210 // the polygons appart using the Minimum Translation Vector
1.211 - polygonATranslation = velocity + r.MinimumTranslationVector;
1.212 + polygonATranslation = velocity + r.minTranslation;
1.213 else
1.214 // Just move the polygon by its velocity
1.215 polygonATranslation = velocity;