1.1 --- a/geometry.cpp Tue Aug 18 12:39:07 2009 +0000
1.2 +++ b/geometry.cpp Mon Aug 24 14:39:07 2009 +0000
1.3 @@ -1,11 +1,12 @@
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 +#include <iostream>
1.11 using namespace std;
1.12
1.13 +
1.14 QRectF addBBox(QRectF r1, QRectF r2)
1.15 {
1.16 // Find smallest QRectF containing given rectangles
1.17 @@ -45,6 +46,50 @@
1.18 return false;
1.19 }
1.20
1.21 +ConvexPolygon::ConvexPolygon ()
1.22 +{
1.23 +}
1.24 +
1.25 +ConvexPolygon::ConvexPolygon (QPolygonF p):QPolygonF (p)
1.26 +{
1.27 +}
1.28 +
1.29 +void ConvexPolygon::calcCentroid()
1.30 +{
1.31 + // Calculate area and centroid
1.32 + // http://en.wikipedia.org/wiki/Centroid
1.33 + qreal cx,cy,p;
1.34 + cx=cy=0;
1.35 + _area=0;
1.36 +
1.37 + append (at(0));
1.38 + for (int i=0;i<size()-1;i++)
1.39 + {
1.40 + p=at(i).x() * at(i+1).y() - at(i+1).x() * at(i).y();
1.41 + _area+=p;
1.42 + cx+=(at(i).x()+at(i+1).x()) * p;
1.43 + cy+=(at(i).y()+at(i+1).y()) * p;
1.44 + }
1.45 + pop_back();
1.46 + // area is negative if vertices ordered counterclockwise
1.47 + // (in mirrored graphicsview!)
1.48 + _area=_area/2;
1.49 + p=_area*6;
1.50 + _centroid.setX (cx/p);
1.51 + _centroid.setY (cy/p);
1.52 +}
1.53 +
1.54 +QPointF ConvexPolygon::centroid() const
1.55 +{
1.56 + return _centroid;
1.57 +}
1.58 +
1.59 +qreal ConvexPolygon::weight() const
1.60 +{
1.61 + return _area;
1.62 +}
1.63 +
1.64 +//! Normalize vector
1.65 QPointF normalize (const QPointF &p)
1.66 {
1.67 if (p==QPointF(0,0)) return p;
1.68 @@ -52,38 +97,47 @@
1.69 return QPointF (p.x()/l,p.y()/l);
1.70 }
1.71
1.72 -// Dot product of two vectors
1.73 +//! Dot product of two vectors
1.74 qreal dotProduct (const QPointF &a, const QPointF &b)
1.75 {
1.76 return a.x()*b.x() + a.y()*b.y();
1.77 }
1.78
1.79
1.80 -/* Calculate the projection of a polygon on an axis
1.81 - and returns it as a [min, max] interval
1.82 -*/
1.83 -void ProjectPolygon(QPointF axis, QPolygonF polygon, qreal &min, qreal &max)
1.84 +QPointF scale (const QPointF &v,const qreal &f)
1.85 +{
1.86 + return QPointF (v.x()*f,v.y()*f);
1.87 +}
1.88 +
1.89 +QPointF invert (const QPointF &v)
1.90 +{
1.91 + return QPointF (-v.x(),-v.y());
1.92 +}
1.93 +
1.94 +/*! Calculate the projection of a polygon on an axis
1.95 + and returns it as a [min, max] interval */
1.96 +
1.97 +void projectPolygon(QPointF axis, QPolygonF polygon, qreal &min, qreal &max)
1.98 {
1.99 // To project a point on an axis use the dot product
1.100
1.101 + //cout << "Projecting on "<< axis<<endl;
1.102 qreal d = dotProduct(axis,polygon.at(0));
1.103 min = d;
1.104 max = d;
1.105 - for (int i = 0; i < polygon.size(); i++) {
1.106 + for (int i = 0; i < polygon.size(); i++)
1.107 + {
1.108 d= dotProduct (polygon.at(i),axis);
1.109 if (d < min)
1.110 min = d;
1.111 else
1.112 - {
1.113 if (d> max) max = d;
1.114 - }
1.115 + // cout << "p="<<polygon.at(i)<<" d="<<d<<" (min, max)=("<<min<<","<<max<<")\n";
1.116 }
1.117 }
1.118
1.119 -/* Calculate the signed distance between [minA, maxA] and [minB, maxB]
1.120 - The distance will be negative if the intervals overlap
1.121 -*/
1.122 -
1.123 +// Calculate the signed distance between [minA, maxA] and [minB, maxB]
1.124 +// The distance will be negative if the intervals overlap
1.125
1.126 qreal intervalDistance(qreal minA, qreal maxA, qreal minB, qreal maxB) {
1.127 if (minA < minB) {
1.128 @@ -92,14 +146,16 @@
1.129 return minA - maxB;
1.130 }
1.131 }
1.132 -/*
1.133 +
1.134 +/*!
1.135 Check if polygon A is going to collide with polygon B.
1.136 The last parameter is the *relative* velocity
1.137 of the polygons (i.e. velocityA - velocityB)
1.138 +*/
1.139
1.140 -*/
1.141 -PolygonCollisionResult PolygonCollision(QPolygonF polygonA,
1.142 - QPolygonF polygonB, QPointF velocity) {
1.143 +PolygonCollisionResult polygonCollision(QPolygonF polygonA,
1.144 + QPolygonF polygonB, QPointF velocity)
1.145 +{
1.146 PolygonCollisionResult result;
1.147 result.intersect = true;
1.148 result.willIntersect = true;
1.149 @@ -110,22 +166,23 @@
1.150 QPointF translationAxis;
1.151 QPointF edge;
1.152
1.153 +/*
1.154 cout << "\nA: ";
1.155 for (int k=0; k<edgeCountA;k++)
1.156 cout <<polygonA.at(k);
1.157 cout << "\nB: ";
1.158 for (int k=0; k<edgeCountB;k++)
1.159 cout <<polygonB.at(k);
1.160 -
1.161 + cout <<endl;
1.162 +*/
1.163
1.164 // Loop through all the edges of both polygons
1.165 - int i=0;
1.166 - int j=0;
1.167 - while (i<edgeCountA && j<edgeCountB)
1.168 + for (int i=0;i<edgeCountA + edgeCountB;i++)
1.169 {
1.170 if (i< edgeCountA)
1.171 {
1.172 - if (i<edgeCountA - 1)
1.173 + // Loop through polygon A
1.174 + if (i<edgeCountA-1)
1.175 edge = QPointF (
1.176 polygonA.at(i+1).x()-polygonA.at(i).x(),
1.177 polygonA.at(i+1).y()-polygonA.at(i).y());
1.178 @@ -133,23 +190,21 @@
1.179 edge = QPointF (
1.180 polygonA.at(0).x()-polygonA.at(i).x(),
1.181 polygonA.at(0).y()-polygonA.at(i).y());
1.182 - i++;
1.183 } else
1.184 {
1.185 - if (i < edgeCountB -1)
1.186 + // Loop through polygon B
1.187 + if (i < edgeCountA +edgeCountB -1 )
1.188 edge = QPointF (
1.189 - polygonB.at(j+1).x() - polygonA.at(i).x(),
1.190 - polygonB.at(j+1).y() - polygonA.at(i).y());
1.191 + polygonB.at(i-edgeCountA+1).x() - polygonB.at(i-edgeCountA).x(),
1.192 + polygonB.at(i-edgeCountA+1).y() - polygonB.at(i-edgeCountA).y());
1.193 else
1.194 edge = QPointF (
1.195 - polygonB.at(0).x() - polygonA.at(i).x(),
1.196 - polygonB.at(0).y() - polygonA.at(i).y());
1.197 - j++;
1.198 + polygonB.at(0).x() - polygonB.at(i-edgeCountA).x(),
1.199 + polygonB.at(0).y() - polygonB.at(i-edgeCountA).y());
1.200 }
1.201
1.202 // ===== 1. Find if the polygons are currently intersecting =====
1.203
1.204 -
1.205 // Find the axis perpendicular to the current edge
1.206
1.207 QPointF axis (-edge.y(), edge.x());
1.208 @@ -158,17 +213,15 @@
1.209 // Find the projection of the polygon on the current axis
1.210
1.211 qreal minA = 0; qreal minB = 0; qreal maxA = 0; qreal maxB = 0;
1.212 - ProjectPolygon(axis, polygonA, minA, maxA);
1.213 - ProjectPolygon(axis, polygonB, minB, maxB);
1.214 + projectPolygon(axis, polygonA, minA, maxA);
1.215 + projectPolygon(axis, polygonB, minB, maxB);
1.216
1.217 // Check if the polygon projections are currentlty intersecting
1.218
1.219 - if (intervalDistance(minA, maxA, minB, maxB) > 0)
1.220 - result.intersect = false;
1.221 - else
1.222 - result.intersect = true;
1.223 + qreal d = intervalDistance(minA, maxA, minB, maxB);
1.224 + if (d > 0) result.intersect = false;
1.225
1.226 - // ===== 2. Now find if the polygons *will* intersect =====
1.227 + // ===== 2. Now find if the polygons *will* intersect =====
1.228
1.229
1.230 // Project the velocity on the current axis
1.231 @@ -181,30 +234,25 @@
1.232 minA += velocityProjection;
1.233 else
1.234 maxA += velocityProjection;
1.235 -
1.236
1.237 // Do the same test as above for the new projection
1.238
1.239 - qreal d = intervalDistance(minA, maxA, minB, maxB);
1.240 - if (d > 0) result.willIntersect = false;
1.241 + // d = intervalDistance(minA, maxA, minB, maxB);
1.242 + //if (d > 0) result.willIntersect = false;
1.243 /*
1.244 - */
1.245 cout <<" ";
1.246 - cout <<"minA="<<minA<<" ";
1.247 - cout <<"maxA="<<maxA<<" ";
1.248 - cout <<"minB="<<minB<<" ";
1.249 - cout <<"maxB="<<maxB<<" ";
1.250 + cout << "edge="<<edge<<" ";
1.251 + cout <<"axis="<<axis<<" ";
1.252 + cout <<"dA=("<<minA<<","<<maxA<<") dB=("<<minB<<","<<maxB<<")";
1.253 cout <<" d="<<d<<" ";
1.254 - cout <<"minD="<<minIntervalDistance<<" ";
1.255 - cout <<"axis="<<axis<<" ";
1.256 + //cout <<"minD="<<minIntervalDistance<<" ";
1.257 cout <<"int="<<result.intersect<<" ";
1.258 - cout <<"wint="<<result.willIntersect<<" ";
1.259 + //cout <<"wint="<<result.willIntersect<<" ";
1.260 //cout <<"velProj="<<velocityProjection<<" ";
1.261 cout <<endl;
1.262 -
1.263 -
1.264 + */
1.265
1.266 - if (result.intersect || result.willIntersect)
1.267 + if (result.intersect )// || result.willIntersect)
1.268 {
1.269 // Check if the current interval distance is the minimum one. If so
1.270 // store the interval distance and the current distance. This will
1.271 @@ -213,13 +261,13 @@
1.272 if (d<0) d=-d;
1.273 if (d < minIntervalDistance) {
1.274 minIntervalDistance = d;
1.275 - translationAxis = axis;
1.276 - cout << "tAxix="<<translationAxis<<endl;
1.277 + //translationAxis = axis;
1.278 + //cout << "tAxix="<<translationAxis<<endl;
1.279
1.280 //QPointF t = polygonA.Center - polygonB.Center;
1.281 - QPointF t = polygonA.at(0) - polygonB.at(0);
1.282 - if (dotProduct(t,translationAxis) < 0)
1.283 - translationAxis = -translationAxis;
1.284 + //QPointF t = polygonA.at(0) - polygonB.at(0);
1.285 + //if (dotProduct(t,translationAxis) < 0)
1.286 + // translationAxis = -translationAxis;
1.287 }
1.288 }
1.289 }