.d files are now created with .o files
[physics.git] / src / collisionManager.cpp
CommitLineData
54fe85c5
PG
1/*
2 * Copyright (C) 2008 Patrik Gornicz, Gornicz_P (at) hotmail (dot) com.
3 *
4 * This program is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program. If not, see <http://www.gnu.org/licenses/>.
16 */
17
0ac1dc80 18#include "collisionManager.h"
54fe85c5 19
5e0713e5
PG
20#include <bear/debug.h>
21#include <bear/Vector2.h>
22#include <bear/mathw.h>
23using namespace bear;
54fe85c5
PG
24
25#include "Entities/Ball.h"
aa2791cf 26#include "Entities/Polygon.h"
54fe85c5
PG
27#include "Entities/PhysicsEntity.h"
28
29#include "CollisionInfo.h"
30
54fe85c5
PG
31/// ***** Private Method Headers *****
32
7d8eb699
PG
33static void clearEntities();
34static void placeEntity(PhysicsEntity*);
35static void updateEntities();
54fe85c5 36
7d8eb699
PG
37static void applyCollision(PhysicsEntity*, PhysicsEntity*);
38static void applyCollision(Ball*, Ball*);
39static void applyCollision(Polygon*, Ball*);
14597cf6 40
7d8eb699
PG
41static bool getInfo(const Ball*, const Ball*, CollisionInfo*);
42static bool getInfo(const Polygon*, const Ball*, CollisionInfo*);
54fe85c5
PG
43
44/// ***** Private Variables *****
45
7d8eb699
PG
46static const int sc_ixDivisions = 20;
47static const int sc_iyDivisions = 16;
48static const int sc_ixScreenSize = 800;
49static const int sc_iyScreenSize = 600;
14597cf6 50
7d8eb699 51setPhys divisions[sc_ixDivisions][sc_iyDivisions];
14597cf6 52
54fe85c5
PG
53/// ***** Initializers/Cleaners *****
54
55void collision::init()
56{
57
58}
59void collision::clean()
60{
61
62}
63
64/// ***** Public Methods *****
65
2869e2e8 66void collision::update(setPhys& sp)
54fe85c5 67{
14597cf6
PG
68 clearEntities();
69
70 for( setPhys::iterator it = sp.begin();
71 it != sp.end();
72 it++ )
54fe85c5 73 {
14597cf6
PG
74 placeEntity(*it);
75 }
76
77 updateEntities();
78}
79
80/// ***** Private Methods *****
81
82void clearEntities()
83{
84 for( int x = 0;
7d8eb699 85 x < sc_ixDivisions;
14597cf6
PG
86 x++ )
87 {
88 for( int y = 0;
7d8eb699 89 y < sc_iyDivisions;
14597cf6 90 y++ )
54fe85c5 91 {
14597cf6 92 divisions[x][y].clear();
54fe85c5
PG
93 }
94 }
14597cf6 95}
54fe85c5 96
7d8eb699 97void placeEntity(PhysicsEntity* ppe)
14597cf6 98{
7d8eb699
PG
99 DASSERT(ppe != NULL);
100
101 Vector2 vecMin;
102 Vector2 vecMax;
14597cf6 103
aa2791cf 104 { // Ball case
7d8eb699 105 Ball* pBall = dynamic_cast<Ball*>(ppe);
aa2791cf 106
7d8eb699 107 if( pBall != NULL )
aa2791cf 108 {
7d8eb699
PG
109 const float& xb = pBall->positionRaw().m_fX;
110 const float& yb = pBall->positionRaw().m_fY;
111 const float& rad = pBall->radius;
aa2791cf 112
7d8eb699
PG
113 vecMin.m_fX = xb - rad;
114 vecMin.m_fY = yb - rad;
115 vecMax.m_fX = xb + rad;
116 vecMax.m_fY = yb + rad;
aa2791cf
PG
117
118 goto start;
119 }
14597cf6
PG
120 }
121
aa2791cf 122 { // Polygon case
7d8eb699 123 Polygon* pPoly = dynamic_cast<Polygon*>(ppe);
14597cf6 124
aa2791cf
PG
125 if( pPoly != NULL )
126 {
7d8eb699
PG
127 vecMin = pPoly->minP;
128 vecMax = pPoly->maxP;
aa2791cf
PG
129
130 goto start;
131 }
132 }
133
134 DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!");
135 return;
136
137start:
7d8eb699
PG
138 for( int x = static_cast<int>( vecMin.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
139 x <= static_cast<int>( vecMax.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
14597cf6
PG
140 x++ )
141 {
7d8eb699 142 if(x < 0 || sc_ixDivisions <= x)
14597cf6
PG
143 break;
144
7d8eb699
PG
145 for( int y = static_cast<int>( vecMin.m_fY / (sc_iyScreenSize / sc_iyDivisions) );
146 y <= static_cast<int>( vecMax.m_fY / (sc_iyScreenSize / sc_iyDivisions) );
14597cf6
PG
147 y++ )
148 {
7d8eb699 149 if(y < 0 || sc_iyDivisions <= y)
14597cf6
PG
150 break;
151
7d8eb699 152 divisions[x][y].insert(ppe);
14597cf6
PG
153 }
154 }
54fe85c5
PG
155}
156
14597cf6
PG
157void updateEntities()
158{
159 for( int x = 0;
7d8eb699 160 x < sc_ixDivisions;
14597cf6
PG
161 x++ )
162 {
163 for( int y = 0;
7d8eb699 164 y < sc_iyDivisions;
14597cf6
PG
165 y++ )
166 {
167 for( setPhys::iterator it1 = divisions[x][y].begin();
168 it1 != divisions[x][y].end();
169 it1++ )
170 {
171 for( setPhys::iterator it2 = divisions[x][y].begin();
172 it2 != divisions[x][y].end();
173 it2++ )
174 {
1a4f2f30
PG
175 if( *it1 == *it2 )
176 break;
177
178 applyCollision(*it1, *it2);
14597cf6
PG
179 }
180 }
181 }
182 }
183}
54fe85c5 184
7d8eb699 185void applyCollision(PhysicsEntity* ppe1, PhysicsEntity* ppe2)
54fe85c5 186{
7d8eb699 187 DASSERT(ppe1 != NULL && ppe2 != NULL);
aa2791cf 188
7d8eb699
PG
189 {// Ball vs Ball
190 Ball* pb1 = dynamic_cast<Ball*>(ppe1);
191 Ball* pb2 = dynamic_cast<Ball*>(ppe2);
192
193 if( pb1 != NULL && pb2 != NULL )
aa2791cf 194 {
7d8eb699 195 applyCollision(pb1, pb2);
aa2791cf
PG
196 return;
197 }
198 }
54fe85c5 199
7d8eb699
PG
200 {// Polygon vs Ball
201 Polygon* pPoly = dynamic_cast<Polygon*>(ppe1);
202 Ball* pBall = dynamic_cast<Ball*>(ppe2);
aa2791cf
PG
203
204 if( pPoly != NULL && pBall != NULL )
205 {
206 applyCollision(pPoly, pBall);
207 return;
208 }
209 }
210
7d8eb699
PG
211 {// Ball vs Polygon
212 Polygon* pPoly = dynamic_cast<Polygon*>(ppe2);
213 Ball* pBall = dynamic_cast<Ball*>(ppe1);
aa2791cf
PG
214
215 if( pPoly != NULL && pBall != NULL )
216 {
217 applyCollision(pPoly, pBall);
218 return;
219 }
54fe85c5
PG
220 }
221
14597cf6 222 DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
54fe85c5
PG
223}
224
7d8eb699 225void applyCollision(Ball* pb1, Ball* pb2)
54fe85c5 226{
7d8eb699
PG
227 DASSERT(pb1 != NULL && pb2 != NULL);
228
39643454 229 CollisionInfo cInfo;
54fe85c5 230
7d8eb699 231 if(!getInfo(pb1, pb2, &cInfo))
54fe85c5
PG
232 return;
233
234 // a few values to simplify the equations
7d8eb699
PG
235 const Vector2& vecNormal = cInfo.m_vecNormal;
236 const Vector2& vecPoint = cInfo.m_vecPoint;
54fe85c5 237
7d8eb699
PG
238 float m1 = pb1->mass;
239 float m2 = pb2->mass;
54fe85c5 240
7d8eb699 241 float e = (pb1->CoR + pb2->CoR) / 2;
54fe85c5 242
7d8eb699
PG
243 Vector2 v1 = pb1->velocityRaw();
244 Vector2 v2 = pb2->velocityRaw();
54fe85c5
PG
245
246
7d8eb699 247 float iTop = -(e + 1) * (v1 - v2).dot(vecNormal);
54fe85c5
PG
248
249 // otherwise the collision happened and we do the math the below assumes
250 // collisions have no friction
251
7d8eb699 252 float impulse = iTop / (vecNormal.dot(vecNormal) * (1 / m1 + 1 / m2));
54fe85c5 253
7d8eb699
PG
254 pb1->applyImpulse(impulse / m1 * vecNormal, vecPoint);
255 pb2->applyImpulse(-impulse / m2 * vecNormal, vecPoint);
54fe85c5
PG
256}
257
aa2791cf
PG
258void applyCollision(Polygon* pPoly, Ball* pBall)
259{
7d8eb699
PG
260 DASSERT(pPoly != NULL && pBall != NULL);
261
aa2791cf
PG
262 CollisionInfo cInfo;
263
264 if(!getInfo(pPoly, pBall, &cInfo))
265 return;
266
7d8eb699
PG
267 // a few values to simplify the equations
268 const Vector2& vecNorm = cInfo.m_vecNormal;
aa2791cf 269
7d8eb699 270 float fCoR = pBall->CoR;
aa2791cf
PG
271 Vector2 vecVelo = pBall->velocityRaw();
272
273 // impulse divided by mass
274 float idm = (-(fCoR + 1) * vecVelo.dot(vecNorm))
275 / (vecNorm.dot(vecNorm));
276
277 pBall->applyImpulse(idm * vecNorm);
278
279 // HACK
7d8eb699 280 // CoR penetration fix, adds the polygon-ball jitters
aa2791cf
PG
281
282 // from center to point
19c2da50 283 Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius;
e86c86b3 284 pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw());
aa2791cf
PG
285}
286
7d8eb699 287bool getInfo(const Ball* pb1, const Ball* pb2, CollisionInfo* pcInfo)
54fe85c5
PG
288{
289 // a few values to simplify the equations
7d8eb699
PG
290 float r1 = pb1->radius;
291 float r2 = pb2->radius;
54fe85c5 292
7d8eb699
PG
293 Vector2 p1 = pb1->positionRaw();
294 Vector2 p2 = pb2->positionRaw();
295 Vector2 v1 = pb1->velocityRaw();
296 Vector2 v2 = pb2->velocityRaw();
54fe85c5
PG
297
298 // quick binding box check
9d6dea5f
PG
299 if (p1.m_fX - r1 > p2.m_fX + r2
300 || p1.m_fX + r1 < p2.m_fX - r2
301 || p1.m_fY - r1 > p2.m_fY + r2
302 || p1.m_fY + r1 < p2.m_fY - r2)
39643454 303 return false;
54fe85c5
PG
304
305 // test if not touching
306 if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
39643454 307 return false;
54fe85c5 308
7d8eb699 309 // test if they are moving apart in some way if they are it's likely
54fe85c5
PG
310 // that they collided last frame and are still overlapping
311
312 if ((v1 - v2).dot(p1 - p2) >= 0)
39643454
PG
313 return false;
314
e86c86b3
PG
315 pcInfo->m_vecPoint = p1 - (p1 - p2) * r1 / (r1 + r2);
316 pcInfo->m_vecNormal = p1 - p2;
54fe85c5 317
39643454 318 return true;
54fe85c5 319}
aa2791cf
PG
320
321bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* pcInfo)
322{
323 // a few values to simplify the equations
324 float fRad = pBall->radius;
325 Vector2 vecPos = pBall->positionRaw();
326 Vector2 vecVelo = pBall->velocityRaw();
327
9d6dea5f
PG
328 float fMaxX = pPoly->maxP.m_fX;
329 float fMinX = pPoly->minP.m_fX;
330 float fMaxY = pPoly->maxP.m_fY;
331 float fMinY = pPoly->minP.m_fY;
aa2791cf
PG
332
333 // quick binding box check
9d6dea5f
PG
334 if (vecPos.m_fX - fRad > fMaxX || vecPos.m_fX + fRad < fMinX ||
335 vecPos.m_fY - fRad > fMaxY || vecPos.m_fY + fRad < fMinY)
aa2791cf
PG
336 return false;
337
338
339 Vector2 vecTotVec;
340 int iTot = 0;
341
342 {
343 const vector<Vector2>& pts = pPoly->points;
344 unsigned int num = pts.size();
345
346 for (unsigned int i = 0; i < num; i++)
347 {
348 Vector2 vec = vectorToLine(vecPos,
9d6dea5f
PG
349 pts[i].m_fX,
350 pts[i].m_fY,
351 pts[(i + 1) % num].m_fX,
352 pts[(i + 1) % num].m_fY);
aa2791cf
PG
353
354 if (vec.sqrLength() <= fRad*fRad && 0 < vec.dot(vecVelo))
355 {
356 vecTotVec += vec;
357 iTot += 1;
358 }
359 }
360 }
361
362 if (iTot <= 0)
363 return false;
364
365
e86c86b3
PG
366 pcInfo->m_vecPoint = vecTotVec / iTot + vecPos;
367 pcInfo->m_vecNormal = vecPos - pcInfo->m_vecPoint;
aa2791cf
PG
368
369 return true;
370}