src/collisionManager.cpp

   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
  18 #include "collisionManager.h"
  19 #include "debug.h"
  20
  21 #include "Vector2.h"
  22
  23 #include "Entities/Ball.h"
  24 #include "Entities/Polygon.h"
  25 #include "Entities/PhysicsEntity.h"
  26
  27 #include "CollisionInfo.h"
  28
  29 #include "mathw.h"
  30
  31 /// ***** Private Class Header *****
  32
  33 /// ***** Private Method Headers *****
  34
  35 void clearEntities();
  36 void placeEntity(PhysicsEntity* p);
  37 void updateEntities();
  38
  39 void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2);
  40 void applyCollision(Ball* b1, Ball* b2);
  41 void applyCollision(Polygon* pPoly, Ball* pBall);
  42
  43 bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* cInfo);
  44 bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* cInfo);
  45
  46 /// ***** Private Variables *****
  47
  48 const int xDivisions = 20;
  49 const int yDivisions = 16;
  50 const int screenX    = 800;
  51 const int screenY    = 600;
  52
  53 setPhys divisions[xDivisions][yDivisions];
  54
  55 /// ***** Initializers/Cleaners *****
  56
  57 void collision::init()
  58 {
  59
  60 }
  61 void collision::clean()
  62 {
  63
  64 }
  65
  66 /// ***** Public Methods *****
  67
  68 void collision::update(setPhys& sp)
  69 {
  70     clearEntities();
  71
  72     for( setPhys::iterator it = sp.begin();
  73          it != sp.end();
  74          it++ )
  75     {
  76         placeEntity(*it);
  77     }
  78
  79     updateEntities();
  80 }
  81
  82 /// ***** Private Methods *****
  83
  84 void clearEntities()
  85 {
  86     for( int x = 0;
  87              x < xDivisions;
  88              x++ )
  89     {
  90         for( int y = 0;
  91                  y < yDivisions;
  92                  y++ )
  93         {
  94             divisions[x][y].clear();
  95         }
  96     }
  97 }
  98
  99 void placeEntity(PhysicsEntity* p)
 100 {
 101     Vector2 minP;
 102     Vector2 maxP;
 103
 104     { // Ball case
 105         Ball* b = dynamic_cast<Ball*>(p);
 106
 107         if( b != NULL )
 108         {
 109             const float& xb     = b->positionRaw().x;
 110             const float& yb     = b->positionRaw().y;
 111             const float& rad    = b->radius;
 112
 113             minP.x = xb - rad;
 114             minP.y = yb - rad;
 115             maxP.x = xb + rad;
 116             maxP.y = yb + rad;
 117
 118             goto start;
 119         }
 120     }
 121
 122     { // Polygon case
 123         Polygon* pPoly = dynamic_cast<Polygon*>(p);
 124
 125         if( pPoly != NULL )
 126         {
 127             minP = pPoly->minP;
 128             maxP = pPoly->maxP;
 129
 130             goto start;
 131         }
 132     }
 133
 134     DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!");
 135     return;
 136
 137 start:
 138     for( int x =  static_cast<int>( minP.x / (screenX / xDivisions) );
 139              x <= static_cast<int>( maxP.x / (screenX / xDivisions) );
 140              x++ )
 141     {
 142         if(x < 0 || xDivisions <= x)
 143             break;
 144
 145         for( int y =  static_cast<int>( minP.y / (screenY / yDivisions) );
 146                  y <= static_cast<int>( maxP.y / (screenY / yDivisions) );
 147                  y++ )
 148         {
 149             if(y < 0 || yDivisions <= y)
 150                 break;
 151
 152             divisions[x][y].insert(p);
 153         }
 154     }
 155 }
 156
 157 void updateEntities()
 158 {
 159     for( int x = 0;
 160              x < xDivisions;
 161              x++ )
 162     {
 163         for( int y = 0;
 164                  y < yDivisions;
 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                 {
 175                     if( *it1 == *it2 )
 176                         break;
 177
 178                     applyCollision(*it1, *it2);
 179                 }
 180             }
 181         }
 182     }
 183 }
 184
 185 void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2)
 186 {
 187     {
 188         Ball* b1 = dynamic_cast<Ball*>(p1);
 189         Ball* b2 = dynamic_cast<Ball*>(p2);
 190
 191         if( b1 != NULL && b2 != NULL )
 192         {
 193             applyCollision(b1, b2);
 194             return;
 195         }
 196     }
 197
 198     {
 199         Polygon*    pPoly   = dynamic_cast<Polygon*>(p1);
 200         Ball*       pBall   = dynamic_cast<Ball*>(p2);
 201
 202         if( pPoly != NULL && pBall != NULL )
 203         {
 204             applyCollision(pPoly, pBall);
 205             return;
 206         }
 207     }
 208
 209     {
 210         Polygon*    pPoly   = dynamic_cast<Polygon*>(p2);
 211         Ball*       pBall   = dynamic_cast<Ball*>(p1);
 212
 213         if( pPoly != NULL && pBall != NULL )
 214         {
 215             applyCollision(pPoly, pBall);
 216             return;
 217         }
 218     }
 219
 220     DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
 221 }
 222
 223 void applyCollision(Ball* b1, Ball* b2)
 224 {
 225     CollisionInfo   cInfo;
 226
 227     if(!getInfo(b1, b2, &cInfo))
 228         return;
 229
 230     // a few values to simplify the equations
 231     const Vector2& normal = cInfo.normal;
 232     const Vector2& point  = cInfo.point;
 233
 234     float m1 = b1->mass;
 235     float m2 = b2->mass;
 236
 237     float e = (b1->CoR + b2->CoR) / 2;
 238
 239     Vector2 v1 = b1->velocityRaw();
 240     Vector2 v2 = b2->velocityRaw();
 241
 242
 243     float iTop = -(e + 1) * (v1 - v2).dot(normal);
 244
 245     // otherwise the collision happened and we do the math the below assumes
 246     // collisions have no friction
 247
 248     float impulse = iTop / (normal.dot(normal) * (1 / m1 + 1 / m2));
 249
 250     b1->applyImpulse(impulse / m1 * normal, point);
 251     b2->applyImpulse(-impulse / m2 * normal, point);
 252 }
 253
 254 void applyCollision(Polygon* pPoly, Ball* pBall)
 255 {
 256     CollisionInfo   cInfo;
 257
 258     if(!getInfo(pPoly, pBall, &cInfo))
 259         return;
 260
 261     float fCoR = pBall->CoR;
 262     Vector2 vecNorm = cInfo.normal;
 263
 264     Vector2 vecVelo = pBall->velocityRaw();
 265
 266     // impulse divided by mass
 267     float idm = (-(fCoR + 1) * vecVelo.dot(vecNorm))
 268               / (vecNorm.dot(vecNorm));
 269
 270     pBall->applyImpulse(idm * vecNorm);
 271
 272     // HACK
 273     // CoR penetration fix, adds the jitters
 274
 275     // from center to point
 276     Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius;
 277     pBall->applyNudge(cInfo.point + vecCollP - pBall->positionRaw());
 278 }
 279
 280 bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* pcInfo)
 281 {
 282     // a few values to simplify the equations
 283     float r1 = b1->radius;
 284     float r2 = b2->radius;
 285
 286     Vector2 p1 = b1->positionRaw();
 287     Vector2 p2 = b2->positionRaw();
 288     Vector2 v1 = b1->velocityRaw();
 289     Vector2 v2 = b2->velocityRaw();
 290
 291     // quick binding box check
 292     if (p1.x - r1 > p2.x + r2
 293      || p1.x + r1 < p2.x - r2
 294      || p1.y - r1 > p2.y + r2
 295      || p1.y + r1 < p2.y - r2)
 296         return false;
 297
 298     // test if not touching
 299     if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
 300         return false;
 301
 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
 304
 305     if ((v1 - v2).dot(p1 - p2) >= 0)
 306         return false;
 307
 308     pcInfo->point   = p1 - (p1 - p2) * r1 / (r1 + r2);
 309     pcInfo->normal  = p1 - p2;
 310
 311     return true;
 312 }
 313
 314 bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* pcInfo)
 315 {
 316     // a few values to simplify the equations
 317     float   fRad    = pBall->radius;
 318     Vector2 vecPos  = pBall->positionRaw();
 319     Vector2 vecVelo = pBall->velocityRaw();
 320
 321     float fMaxX = pPoly->maxP.x;
 322     float fMinX = pPoly->minP.x;
 323     float fMaxY = pPoly->maxP.y;
 324     float fMinY = pPoly->minP.y;
 325
 326     // quick binding box check
 327     if (vecPos.x - fRad > fMaxX || vecPos.x + fRad < fMinX ||
 328         vecPos.y - fRad > fMaxY || vecPos.y + fRad < fMinY)
 329         return false;
 330
 331
 332     Vector2 vecTotVec;
 333     int iTot = 0;
 334
 335     {
 336         const vector<Vector2>& pts = pPoly->points;
 337         unsigned int num = pts.size();
 338
 339         for (unsigned int i = 0; i < num; i++)
 340         {
 341             Vector2 vec = vectorToLine(vecPos,
 342                                        pts[i].x,
 343                                        pts[i].y,
 344                                        pts[(i + 1) % num].x,
 345                                        pts[(i + 1) % num].y);
 346
 347             if (vec.sqrLength() <= fRad*fRad && 0 < vec.dot(vecVelo))
 348             {
 349                 vecTotVec += vec;
 350                 iTot += 1;
 351             }
 352         }
 353     }
 354
 355     if (iTot <= 0)
 356         return false;
 357
 358
 359     pcInfo->point   = vecTotVec / iTot + vecPos;
 360     pcInfo->normal  = vecPos - pcInfo->point;
 361
 362     return true;
 363 }