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
  20 #include <bear/debug.h>
  21 #include <bear/Vector2.h>
  22 #include <bear/mathw.h>
  23 using namespace bear;
  24
  25 #include "Entities/Ball.h"
  26 #include "Entities/Polygon.h"
  27 #include "Entities/PhysicsEntity.h"
  28
  29 #include "CollisionInfo.h"
  30
  31 /// ***** Private Method Headers *****
  32
  33 static void clearEntities();
  34 static void placeEntity(PhysicsEntity*);
  35 static void updateEntities();
  36
  37 static void applyCollision(PhysicsEntity*, PhysicsEntity*);
  38 static void applyCollision(Ball*, Ball*);
  39 static void applyCollision(Polygon*, Ball*);
  40
  41 static bool getInfo(const Ball*, const Ball*, CollisionInfo*);
  42 static bool getInfo(const Polygon*, const Ball*, CollisionInfo*);
  43
  44 /// ***** Private Variables *****
  45
  46 static const int sc_ixDivisions     = 20;
  47 static const int sc_iyDivisions     = 16;
  48 static const int sc_ixScreenSize    = 800;
  49 static const int sc_iyScreenSize    = 600;
  50
  51 setPhys divisions[sc_ixDivisions][sc_iyDivisions];
  52
  53 /// ***** Initializers/Cleaners *****
  54
  55 void collision::init()
  56 {
  57
  58 }
  59 void collision::clean()
  60 {
  61
  62 }
  63
  64 /// ***** Public Methods *****
  65
  66 void collision::update(setPhys& sp)
  67 {
  68     clearEntities();
  69
  70     for( setPhys::iterator it = sp.begin();
  71          it != sp.end();
  72          it++ )
  73     {
  74         placeEntity(*it);
  75     }
  76
  77     updateEntities();
  78 }
  79
  80 /// ***** Private Methods *****
  81
  82 void clearEntities()
  83 {
  84     for( int x = 0;
  85              x < sc_ixDivisions;
  86              x++ )
  87     {
  88         for( int y = 0;
  89                  y < sc_iyDivisions;
  90                  y++ )
  91         {
  92             divisions[x][y].clear();
  93         }
  94     }
  95 }
  96
  97 void placeEntity(PhysicsEntity* ppe)
  98 {
  99     DASSERT(ppe != NULL);
 100
 101     Vector2 vecMin;
 102     Vector2 vecMax;
 103
 104     { // Ball case
 105         Ball* pBall = dynamic_cast<Ball*>(ppe);
 106
 107         if( pBall != NULL )
 108         {
 109             const float& xb     = pBall->positionRaw().m_fX;
 110             const float& yb     = pBall->positionRaw().m_fY;
 111             const float& rad    = pBall->m_radius;
 112
 113             vecMin.m_fX = xb - rad;
 114             vecMin.m_fY = yb - rad;
 115             vecMax.m_fX = xb + rad;
 116             vecMax.m_fY = yb + rad;
 117
 118             goto start;
 119         }
 120     }
 121
 122     { // Polygon case
 123         Polygon* pPoly = dynamic_cast<Polygon*>(ppe);
 124
 125         if( pPoly != NULL )
 126         {
 127             vecMin = pPoly->m_minP;
 128             vecMax = pPoly->m_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>( vecMin.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
 139              x <= static_cast<int>( vecMax.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
 140              x++ )
 141     {
 142         if(x < 0 || sc_ixDivisions <= x)
 143             break;
 144
 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) );
 147                  y++ )
 148         {
 149             if(y < 0 || sc_iyDivisions <= y)
 150                 break;
 151
 152             divisions[x][y].insert(ppe);
 153         }
 154     }
 155 }
 156
 157 void updateEntities()
 158 {
 159     for( int x = 0;
 160              x < sc_ixDivisions;
 161              x++ )
 162     {
 163         for( int y = 0;
 164                  y < sc_iyDivisions;
 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* ppe1, PhysicsEntity* ppe2)
 186 {
 187     DASSERT(ppe1 != NULL && ppe2 != NULL);
 188
 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 )
 194         {
 195             applyCollision(pb1, pb2);
 196             return;
 197         }
 198     }
 199
 200     {// Polygon vs Ball
 201         Polygon*    pPoly   = dynamic_cast<Polygon*>(ppe1);
 202         Ball*       pBall   = dynamic_cast<Ball*>(ppe2);
 203
 204         if( pPoly != NULL && pBall != NULL )
 205         {
 206             applyCollision(pPoly, pBall);
 207             return;
 208         }
 209     }
 210
 211     {// Ball vs Polygon
 212         Polygon*    pPoly   = dynamic_cast<Polygon*>(ppe2);
 213         Ball*       pBall   = dynamic_cast<Ball*>(ppe1);
 214
 215         if( pPoly != NULL && pBall != NULL )
 216         {
 217             applyCollision(pPoly, pBall);
 218             return;
 219         }
 220     }
 221
 222     DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
 223 }
 224
 225 void applyCollision(Ball* pb1, Ball* pb2)
 226 {
 227     DASSERT(pb1 != NULL && pb2 != NULL);
 228
 229     CollisionInfo   cInfo;
 230
 231     if(!getInfo(pb1, pb2, &cInfo))
 232         return;
 233
 234     // a few values to simplify the equations
 235     const Vector2& vecNormal = cInfo.m_vecNormal;
 236     const Vector2& vecPoint  = cInfo.m_vecPoint;
 237
 238     float m1 = pb1->m_mass;
 239     float m2 = pb2->m_mass;
 240
 241     float e = (pb1->m_CoR + pb2->m_CoR) / 2;
 242
 243     Vector2 v1 = pb1->velocityRaw();
 244     Vector2 v2 = pb2->velocityRaw();
 245
 246
 247     float iTop = -(e + 1) * (v1 - v2).dot(vecNormal);
 248
 249     // otherwise the collision happened and we do the math the below assumes
 250     // collisions have no friction
 251
 252     float impulse = iTop / (vecNormal.dot(vecNormal) * (1 / m1 + 1 / m2));
 253
 254     pb1->applyImpulse(impulse / m1 * vecNormal, vecPoint);
 255     pb2->applyImpulse(-impulse / m2 * vecNormal, vecPoint);
 256 }
 257
 258 void applyCollision(Polygon* pPoly, Ball* pBall)
 259 {
 260     DASSERT(pPoly != NULL && pBall != NULL);
 261
 262     CollisionInfo   cInfo;
 263
 264     if(!getInfo(pPoly, pBall, &cInfo))
 265         return;
 266
 267     // a few values to simplify the equations
 268     const Vector2& vecNorm = cInfo.m_vecNormal;
 269
 270     float fCoR = pBall->m_CoR;
 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
 280     // CoR penetration fix, adds the polygon-ball jitters
 281
 282     // from center to point
 283     Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->m_radius;
 284     pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw());
 285 }
 286
 287 bool getInfo(const Ball* pb1, const Ball* pb2, CollisionInfo* pcInfo)
 288 {
 289     // a few values to simplify the equations
 290     float r1 = pb1->m_radius;
 291     float r2 = pb2->m_radius;
 292
 293     Vector2 p1 = pb1->positionRaw();
 294     Vector2 p2 = pb2->positionRaw();
 295     Vector2 v1 = pb1->velocityRaw();
 296     Vector2 v2 = pb2->velocityRaw();
 297
 298     // quick binding box check
 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)
 303         return false;
 304
 305     // test if not touching
 306     if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
 307         return false;
 308
 309     // test if they are moving apart in some way if they are it's likely
 310     // that they collided last frame and are still overlapping
 311
 312     if ((v1 - v2).dot(p1 - p2) >= 0)
 313         return false;
 314
 315     pcInfo->m_vecPoint   = p1 - (p1 - p2) * r1 / (r1 + r2);
 316     pcInfo->m_vecNormal  = p1 - p2;
 317
 318     return true;
 319 }
 320
 321 bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* pcInfo)
 322 {
 323     // a few values to simplify the equations
 324     float   fRad    = pBall->m_radius;
 325     Vector2 vecPos  = pBall->positionRaw();
 326     Vector2 vecVelo = pBall->velocityRaw();
 327
 328     float fMaxX = pPoly->m_maxP.m_fX;
 329     float fMinX = pPoly->m_minP.m_fX;
 330     float fMaxY = pPoly->m_maxP.m_fY;
 331     float fMinY = pPoly->m_minP.m_fY;
 332
 333     // quick binding box check
 334     if (vecPos.m_fX - fRad > fMaxX || vecPos.m_fX + fRad < fMinX ||
 335         vecPos.m_fY - fRad > fMaxY || vecPos.m_fY + fRad < fMinY)
 336         return false;
 337
 338
 339     Vector2 vecTotVec;
 340     int iTot = 0;
 341
 342     {
 343         const vector<Vector2>& pts = pPoly->m_points;
 344         unsigned int num = pts.size();
 345
 346         for (unsigned int i = 0; i < num; i++)
 347         {
 348             Vector2 vec = vectorToLine(vecPos,
 349                                        pts[i].m_fX,
 350                                        pts[i].m_fY,
 351                                        pts[(i + 1) % num].m_fX,
 352                                        pts[(i + 1) % num].m_fY);
 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
 366     pcInfo->m_vecPoint   = vecTotVec / iTot + vecPos;
 367     pcInfo->m_vecNormal  = vecPos - pcInfo->m_vecPoint;
 368
 369     return true;
 370 }