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