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