2 * Copyright (C) 2008 Patrik Gornicz, Gornicz_P (at) hotmail (dot) com.
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.
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.
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/>.
18 #include "collisionManager.h"
23 #include "Entities/Ball.h"
24 #include "Entities/Polygon.h"
25 #include "Entities/PhysicsEntity.h"
27 #include "CollisionInfo.h"
31 /// ***** Private Class Header *****
33 /// ***** Private Method Headers *****
36 void placeEntity(PhysicsEntity* p);
37 void updateEntities();
39 void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2);
40 void applyCollision(Ball* b1, Ball* b2);
41 void applyCollision(Polygon* pPoly, Ball* pBall);
43 bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* cInfo);
44 bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* cInfo);
46 /// ***** Private Variables *****
48 const int xDivisions = 20;
49 const int yDivisions = 16;
50 const int screenX = 800;
51 const int screenY = 600;
53 setPhys divisions[xDivisions][yDivisions];
55 /// ***** Initializers/Cleaners *****
57 void collision::init()
61 void collision::clean()
66 /// ***** Public Methods *****
68 void collision::update(setPhys& sp)
72 for( setPhys::iterator it = sp.begin();
82 /// ***** Private Methods *****
94 divisions[x][y].clear();
99 void placeEntity(PhysicsEntity* p)
105 Ball* b = dynamic_cast<Ball*>(p);
109 const float& xb = b->positionRaw().m_fX;
110 const float& yb = b->positionRaw().m_fY;
111 const float& rad = b->radius;
113 minP.m_fX = xb - rad;
114 minP.m_fY = yb - rad;
115 maxP.m_fX = xb + rad;
116 maxP.m_fY = yb + rad;
123 Polygon* pPoly = dynamic_cast<Polygon*>(p);
134 DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!");
138 for( int x = static_cast<int>( minP.m_fX / (screenX / xDivisions) );
139 x <= static_cast<int>( maxP.m_fX / (screenX / xDivisions) );
142 if(x < 0 || xDivisions <= x)
145 for( int y = static_cast<int>( minP.m_fY / (screenY / yDivisions) );
146 y <= static_cast<int>( maxP.m_fY / (screenY / yDivisions) );
149 if(y < 0 || yDivisions <= y)
152 divisions[x][y].insert(p);
157 void updateEntities()
167 for( setPhys::iterator it1 = divisions[x][y].begin();
168 it1 != divisions[x][y].end();
171 for( setPhys::iterator it2 = divisions[x][y].begin();
172 it2 != divisions[x][y].end();
178 applyCollision(*it1, *it2);
185 void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2)
188 Ball* b1 = dynamic_cast<Ball*>(p1);
189 Ball* b2 = dynamic_cast<Ball*>(p2);
191 if( b1 != NULL && b2 != NULL )
193 applyCollision(b1, b2);
199 Polygon* pPoly = dynamic_cast<Polygon*>(p1);
200 Ball* pBall = dynamic_cast<Ball*>(p2);
202 if( pPoly != NULL && pBall != NULL )
204 applyCollision(pPoly, pBall);
210 Polygon* pPoly = dynamic_cast<Polygon*>(p2);
211 Ball* pBall = dynamic_cast<Ball*>(p1);
213 if( pPoly != NULL && pBall != NULL )
215 applyCollision(pPoly, pBall);
220 DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
223 void applyCollision(Ball* b1, Ball* b2)
227 if(!getInfo(b1, b2, &cInfo))
230 // a few values to simplify the equations
231 const Vector2& normal = cInfo.m_vecNormal;
232 const Vector2& point = cInfo.m_vecPoint;
237 float e = (b1->CoR + b2->CoR) / 2;
239 Vector2 v1 = b1->velocityRaw();
240 Vector2 v2 = b2->velocityRaw();
243 float iTop = -(e + 1) * (v1 - v2).dot(normal);
245 // otherwise the collision happened and we do the math the below assumes
246 // collisions have no friction
248 float impulse = iTop / (normal.dot(normal) * (1 / m1 + 1 / m2));
250 b1->applyImpulse(impulse / m1 * normal, point);
251 b2->applyImpulse(-impulse / m2 * normal, point);
254 void applyCollision(Polygon* pPoly, Ball* pBall)
258 if(!getInfo(pPoly, pBall, &cInfo))
261 float fCoR = pBall->CoR;
262 Vector2 vecNorm = cInfo.m_vecNormal;
264 Vector2 vecVelo = pBall->velocityRaw();
266 // impulse divided by mass
267 float idm = (-(fCoR + 1) * vecVelo.dot(vecNorm))
268 / (vecNorm.dot(vecNorm));
270 pBall->applyImpulse(idm * vecNorm);
273 // CoR penetration fix, adds the jitters
275 // from center to point
276 Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius;
277 pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw());
280 bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* pcInfo)
282 // a few values to simplify the equations
283 float r1 = b1->radius;
284 float r2 = b2->radius;
286 Vector2 p1 = b1->positionRaw();
287 Vector2 p2 = b2->positionRaw();
288 Vector2 v1 = b1->velocityRaw();
289 Vector2 v2 = b2->velocityRaw();
291 // quick binding box check
292 if (p1.m_fX - r1 > p2.m_fX + r2
293 || p1.m_fX + r1 < p2.m_fX - r2
294 || p1.m_fY - r1 > p2.m_fY + r2
295 || p1.m_fY + r1 < p2.m_fY - r2)
298 // test if not touching
299 if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
302 // test if they are moving apart in some way if they aren't it's likely
303 // that they collided last frame and are still overlapping
305 if ((v1 - v2).dot(p1 - p2) >= 0)
308 pcInfo->m_vecPoint = p1 - (p1 - p2) * r1 / (r1 + r2);
309 pcInfo->m_vecNormal = p1 - p2;
314 bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* pcInfo)
316 // a few values to simplify the equations
317 float fRad = pBall->radius;
318 Vector2 vecPos = pBall->positionRaw();
319 Vector2 vecVelo = pBall->velocityRaw();
321 float fMaxX = pPoly->maxP.m_fX;
322 float fMinX = pPoly->minP.m_fX;
323 float fMaxY = pPoly->maxP.m_fY;
324 float fMinY = pPoly->minP.m_fY;
326 // quick binding box check
327 if (vecPos.m_fX - fRad > fMaxX || vecPos.m_fX + fRad < fMinX ||
328 vecPos.m_fY - fRad > fMaxY || vecPos.m_fY + fRad < fMinY)
336 const vector<Vector2>& pts = pPoly->points;
337 unsigned int num = pts.size();
339 for (unsigned int i = 0; i < num; i++)
341 Vector2 vec = vectorToLine(vecPos,
344 pts[(i + 1) % num].m_fX,
345 pts[(i + 1) % num].m_fY);
347 if (vec.sqrLength() <= fRad*fRad && 0 < vec.dot(vecVelo))
359 pcInfo->m_vecPoint = vecTotVec / iTot + vecPos;
360 pcInfo->m_vecNormal = vecPos - pcInfo->m_vecPoint;