*/
#include "collisionManager.h"
-#include "debug.h"
-#include "Vector2.h"
+#include <bear/debug.h>
+#include <bear/Vector2.h>
+#include <bear/mathw.h>
+using namespace bear;
#include "Entities/Ball.h"
+#include "Entities/Polygon.h"
#include "Entities/PhysicsEntity.h"
#include "CollisionInfo.h"
-/// ***** Private Class Header *****
-
/// ***** Private Method Headers *****
-void applyCollisionAt(PhysicsEntity* p1, PhysicsEntity* p2);
-void applyCollisionAt(Ball* b1, Ball* b2);
+static void clearEntities();
+static void placeEntity(PhysicsEntity*);
+static void updateEntities();
+
+static void applyCollision(PhysicsEntity*, PhysicsEntity*);
+static void applyCollision(Ball*, Ball*);
+static void applyCollision(Polygon*, Ball*);
-bool getInfoAt(Ball* b1, Ball* b2, CollisionInfo* cInfo);
+static bool getInfo(const Ball*, const Ball*, CollisionInfo*);
+static bool getInfo(const Polygon*, const Ball*, CollisionInfo*);
/// ***** Private Variables *****
+static const int sc_ixDivisions = 20;
+static const int sc_iyDivisions = 16;
+static const int sc_ixScreenSize = 800;
+static const int sc_iyScreenSize = 600;
+
+setPhys divisions[sc_ixDivisions][sc_iyDivisions];
+
/// ***** Initializers/Cleaners *****
void collision::init()
void collision::update(setPhys& sp)
{
- for( setPhys::iterator it1 = sp.begin();
- it1 != sp.end();
- it1++ )
+ clearEntities();
+
+ for( setPhys::iterator it = sp.begin();
+ it != sp.end();
+ it++ )
{
- for( setPhys::iterator it2 = sp.begin();
- it2 != sp.end();
- it2++ )
- {
- if( *it1 != *it2 )
- {
- applyCollisionAt(*it1, *it2);
- }
- }
+ placeEntity(*it);
}
+ updateEntities();
}
/// ***** Private Methods *****
-void applyCollisionAt(PhysicsEntity* p1, PhysicsEntity* p2)
+void clearEntities()
+{
+ for( int x = 0;
+ x < sc_ixDivisions;
+ x++ )
+ {
+ for( int y = 0;
+ y < sc_iyDivisions;
+ y++ )
+ {
+ divisions[x][y].clear();
+ }
+ }
+}
+
+void placeEntity(PhysicsEntity* ppe)
{
- Ball* b1 = dynamic_cast<Ball*>(p1);
- Ball* b2 = dynamic_cast<Ball*>(p2);
+ DASSERT(ppe != NULL);
+
+ Vector2 vecMin;
+ Vector2 vecMax;
+
+ { // Ball case
+ Ball* pBall = dynamic_cast<Ball*>(ppe);
+
+ if( pBall != NULL )
+ {
+ const float& xb = pBall->positionRaw().m_fX;
+ const float& yb = pBall->positionRaw().m_fY;
+ const float& rad = pBall->radius;
+
+ vecMin.m_fX = xb - rad;
+ vecMin.m_fY = yb - rad;
+ vecMax.m_fX = xb + rad;
+ vecMax.m_fY = yb + rad;
+
+ goto start;
+ }
+ }
+
+ { // Polygon case
+ Polygon* pPoly = dynamic_cast<Polygon*>(ppe);
+
+ if( pPoly != NULL )
+ {
+ vecMin = pPoly->minP;
+ vecMax = pPoly->maxP;
+
+ goto start;
+ }
+ }
- if( b1 != NULL && b2 != NULL )
+ DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!");
+ return;
+
+start:
+ for( int x = static_cast<int>( vecMin.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
+ x <= static_cast<int>( vecMax.m_fX / (sc_ixScreenSize / sc_ixDivisions) );
+ x++ )
{
- applyCollisionAt(b1, b2);
- return;
+ if(x < 0 || sc_ixDivisions <= x)
+ break;
+
+ for( int y = static_cast<int>( vecMin.m_fY / (sc_iyScreenSize / sc_iyDivisions) );
+ y <= static_cast<int>( vecMax.m_fY / (sc_iyScreenSize / sc_iyDivisions) );
+ y++ )
+ {
+ if(y < 0 || sc_iyDivisions <= y)
+ break;
+
+ divisions[x][y].insert(ppe);
+ }
}
+}
- DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollisionAt()!!");
+void updateEntities()
+{
+ for( int x = 0;
+ x < sc_ixDivisions;
+ x++ )
+ {
+ for( int y = 0;
+ y < sc_iyDivisions;
+ y++ )
+ {
+ for( setPhys::iterator it1 = divisions[x][y].begin();
+ it1 != divisions[x][y].end();
+ it1++ )
+ {
+ for( setPhys::iterator it2 = divisions[x][y].begin();
+ it2 != divisions[x][y].end();
+ it2++ )
+ {
+ if( *it1 == *it2 )
+ break;
+
+ applyCollision(*it1, *it2);
+ }
+ }
+ }
+ }
}
-void applyCollisionAt(Ball* b1, Ball* b2)
+void applyCollision(PhysicsEntity* ppe1, PhysicsEntity* ppe2)
{
+ DASSERT(ppe1 != NULL && ppe2 != NULL);
+
+ {// Ball vs Ball
+ Ball* pb1 = dynamic_cast<Ball*>(ppe1);
+ Ball* pb2 = dynamic_cast<Ball*>(ppe2);
+
+ if( pb1 != NULL && pb2 != NULL )
+ {
+ applyCollision(pb1, pb2);
+ return;
+ }
+ }
+
+ {// Polygon vs Ball
+ Polygon* pPoly = dynamic_cast<Polygon*>(ppe1);
+ Ball* pBall = dynamic_cast<Ball*>(ppe2);
+
+ if( pPoly != NULL && pBall != NULL )
+ {
+ applyCollision(pPoly, pBall);
+ return;
+ }
+ }
+
+ {// Ball vs Polygon
+ Polygon* pPoly = dynamic_cast<Polygon*>(ppe2);
+ Ball* pBall = dynamic_cast<Ball*>(ppe1);
+
+ if( pPoly != NULL && pBall != NULL )
+ {
+ applyCollision(pPoly, pBall);
+ return;
+ }
+ }
+
+ DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
+}
+
+void applyCollision(Ball* pb1, Ball* pb2)
+{
+ DASSERT(pb1 != NULL && pb2 != NULL);
+
CollisionInfo cInfo;
- if(!getInfoAt(b1, b2, &cInfo))
+ if(!getInfo(pb1, pb2, &cInfo))
return;
// a few values to simplify the equations
- const Vector2& normal = cInfo.normal;
- const Vector2& point = cInfo.point;
+ const Vector2& vecNormal = cInfo.m_vecNormal;
+ const Vector2& vecPoint = cInfo.m_vecPoint;
- float m1 = b1->mass;
- float m2 = b2->mass;
+ float m1 = pb1->mass;
+ float m2 = pb2->mass;
- float e = (b1->CoR + b2->CoR) / 2;
+ float e = (pb1->CoR + pb2->CoR) / 2;
- Vector2 v1 = b1->velocityRaw();
- Vector2 v2 = b2->velocityRaw();
+ Vector2 v1 = pb1->velocityRaw();
+ Vector2 v2 = pb2->velocityRaw();
- float iTop = -(e + 1) * (v1 - v2).dot(normal);
+ float iTop = -(e + 1) * (v1 - v2).dot(vecNormal);
// otherwise the collision happened and we do the math the below assumes
// collisions have no friction
- float impulse = iTop / (normal.dot(normal) * (1 / m1 + 1 / m2));
+ float impulse = iTop / (vecNormal.dot(vecNormal) * (1 / m1 + 1 / m2));
+
+ pb1->applyImpulse(impulse / m1 * vecNormal, vecPoint);
+ pb2->applyImpulse(-impulse / m2 * vecNormal, vecPoint);
+}
+
+void applyCollision(Polygon* pPoly, Ball* pBall)
+{
+ DASSERT(pPoly != NULL && pBall != NULL);
+
+ CollisionInfo cInfo;
+
+ if(!getInfo(pPoly, pBall, &cInfo))
+ return;
+
+ // a few values to simplify the equations
+ const Vector2& vecNorm = cInfo.m_vecNormal;
+
+ float fCoR = pBall->CoR;
+ Vector2 vecVelo = pBall->velocityRaw();
+
+ // impulse divided by mass
+ float idm = (-(fCoR + 1) * vecVelo.dot(vecNorm))
+ / (vecNorm.dot(vecNorm));
+
+ pBall->applyImpulse(idm * vecNorm);
+
+ // HACK
+ // CoR penetration fix, adds the polygon-ball jitters
- b1->applyImpulse(impulse / m1 * normal, point);
- b2->applyImpulse(-impulse / m2 * normal, point);
+ // from center to point
+ Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius;
+ pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw());
}
-bool getInfoAt(Ball* b1, Ball* b2, CollisionInfo* pcInfo)
+bool getInfo(const Ball* pb1, const Ball* pb2, CollisionInfo* pcInfo)
{
// a few values to simplify the equations
- float r1 = b1->radius;
- float r2 = b2->radius;
+ float r1 = pb1->radius;
+ float r2 = pb2->radius;
- Vector2 p1 = b1->positionRaw();
- Vector2 p2 = b2->positionRaw();
- Vector2 v1 = b1->velocityRaw();
- Vector2 v2 = b2->velocityRaw();
+ Vector2 p1 = pb1->positionRaw();
+ Vector2 p2 = pb2->positionRaw();
+ Vector2 v1 = pb1->velocityRaw();
+ Vector2 v2 = pb2->velocityRaw();
// quick binding box check
- if (p1.x - r1 > p2.x + r2
- || p1.x + r1 < p2.x - r2
- || p1.y - r1 > p2.y + r2
- || p1.y + r1 < p2.y - r2)
+ if (p1.m_fX - r1 > p2.m_fX + r2
+ || p1.m_fX + r1 < p2.m_fX - r2
+ || p1.m_fY - r1 > p2.m_fY + r2
+ || p1.m_fY + r1 < p2.m_fY - r2)
return false;
// test if not touching
if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
return false;
- // test if they are moving apart in some way if they aren't it's likely
+ // test if they are moving apart in some way if they are it's likely
// that they collided last frame and are still overlapping
if ((v1 - v2).dot(p1 - p2) >= 0)
return false;
- pcInfo->point = p1 - (p1 - p2) * r1 / (r1 + r2);
- pcInfo->normal = p1 - p2;
+ pcInfo->m_vecPoint = p1 - (p1 - p2) * r1 / (r1 + r2);
+ pcInfo->m_vecNormal = p1 - p2;
+
+ return true;
+}
+
+bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* pcInfo)
+{
+ // a few values to simplify the equations
+ float fRad = pBall->radius;
+ Vector2 vecPos = pBall->positionRaw();
+ Vector2 vecVelo = pBall->velocityRaw();
+
+ float fMaxX = pPoly->maxP.m_fX;
+ float fMinX = pPoly->minP.m_fX;
+ float fMaxY = pPoly->maxP.m_fY;
+ float fMinY = pPoly->minP.m_fY;
+
+ // quick binding box check
+ if (vecPos.m_fX - fRad > fMaxX || vecPos.m_fX + fRad < fMinX ||
+ vecPos.m_fY - fRad > fMaxY || vecPos.m_fY + fRad < fMinY)
+ return false;
+
+
+ Vector2 vecTotVec;
+ int iTot = 0;
+
+ {
+ const vector<Vector2>& pts = pPoly->points;
+ unsigned int num = pts.size();
+
+ for (unsigned int i = 0; i < num; i++)
+ {
+ Vector2 vec = vectorToLine(vecPos,
+ pts[i].m_fX,
+ pts[i].m_fY,
+ pts[(i + 1) % num].m_fX,
+ pts[(i + 1) % num].m_fY);
+
+ if (vec.sqrLength() <= fRad*fRad && 0 < vec.dot(vecVelo))
+ {
+ vecTotVec += vec;
+ iTot += 1;
+ }
+ }
+ }
+
+ if (iTot <= 0)
+ return false;
+
+
+ pcInfo->m_vecPoint = vecTotVec / iTot + vecPos;
+ pcInfo->m_vecNormal = vecPos - pcInfo->m_vecPoint;
return true;
}