X-Git-Url: http://gitweb.pgornicz.com/gitweb.cgi?a=blobdiff_plain;f=src%2FcollisionManager.cpp;h=72dcd6bd9fe9895c4e4a9cc4522a9e85133e08f5;hb=e86c86b38b25bcb463943dc166b60f236aa4af10;hp=e03135afcbcb54a8cb1e6ede0ea802cf2f09a69f;hpb=39643454e15a57174037f04c7bb4f96520d36ad6;p=physics.git diff --git a/src/collisionManager.cpp b/src/collisionManager.cpp index e03135a..72dcd6b 100644 --- a/src/collisionManager.cpp +++ b/src/collisionManager.cpp @@ -21,21 +21,37 @@ #include "Vector2.h" #include "Entities/Ball.h" +#include "Entities/Polygon.h" #include "Entities/PhysicsEntity.h" #include "CollisionInfo.h" +#include "mathw.h" + /// ***** Private Class Header ***** /// ***** Private Method Headers ***** -void applyCollisionAt(PhysicsEntity* p1, PhysicsEntity* p2); -void applyCollisionAt(Ball* b1, Ball* b2); +void clearEntities(); +void placeEntity(PhysicsEntity* p); +void updateEntities(); + +void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2); +void applyCollision(Ball* b1, Ball* b2); +void applyCollision(Polygon* pPoly, Ball* pBall); -bool getInfoAt(Ball* b1, Ball* b2, CollisionInfo* cInfo); +bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* cInfo); +bool getInfo(const Polygon* pPoly, const Ball* pBall, CollisionInfo* cInfo); /// ***** Private Variables ***** +const int xDivisions = 20; +const int yDivisions = 16; +const int screenX = 800; +const int screenY = 600; + +setPhys divisions[xDivisions][yDivisions]; + /// ***** Initializers/Cleaners ***** void collision::init() @@ -51,49 +67,169 @@ void collision::clean() 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++ ) + { + placeEntity(*it); + } + + updateEntities(); +} + +/// ***** Private Methods ***** + +void clearEntities() +{ + for( int x = 0; + x < xDivisions; + x++ ) { - for( setPhys::iterator it2 = sp.begin(); - it2 != sp.end(); - it2++ ) + for( int y = 0; + y < yDivisions; + y++ ) { - if( *it1 != *it2 ) - { - applyCollisionAt(*it1, *it2); - } + divisions[x][y].clear(); + } + } +} + +void placeEntity(PhysicsEntity* p) +{ + Vector2 minP; + Vector2 maxP; + + { // Ball case + Ball* b = dynamic_cast(p); + + if( b != NULL ) + { + const float& xb = b->positionRaw().x; + const float& yb = b->positionRaw().y; + const float& rad = b->radius; + + minP.x = xb - rad; + minP.y = yb - rad; + maxP.x = xb + rad; + maxP.y = yb + rad; + + goto start; } } + { // Polygon case + Polygon* pPoly = dynamic_cast(p); + + if( pPoly != NULL ) + { + minP = pPoly->minP; + maxP = pPoly->maxP; + + goto start; + } + } + + DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!"); + return; + +start: + for( int x = static_cast( minP.x / (screenX / xDivisions) ); + x <= static_cast( maxP.x / (screenX / xDivisions) ); + x++ ) + { + if(x < 0 || xDivisions <= x) + break; + + for( int y = static_cast( minP.y / (screenY / yDivisions) ); + y <= static_cast( maxP.y / (screenY / yDivisions) ); + y++ ) + { + if(y < 0 || yDivisions <= y) + break; + + divisions[x][y].insert(p); + } + } } -/// ***** Private Methods ***** +void updateEntities() +{ + for( int x = 0; + x < xDivisions; + x++ ) + { + for( int y = 0; + y < yDivisions; + 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(PhysicsEntity* p1, PhysicsEntity* p2) +void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2) { - Ball* b1 = dynamic_cast(p1); - Ball* b2 = dynamic_cast(p2); + { + Ball* b1 = dynamic_cast(p1); + Ball* b2 = dynamic_cast(p2); + + if( b1 != NULL && b2 != NULL ) + { + applyCollision(b1, b2); + return; + } + } - if( b1 != NULL && b2 != NULL ) { - applyCollisionAt(b1, b2); - return; + Polygon* pPoly = dynamic_cast(p1); + Ball* pBall = dynamic_cast(p2); + + if( pPoly != NULL && pBall != NULL ) + { + applyCollision(pPoly, pBall); + return; + } } - DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollisionAt()!!"); + { + Polygon* pPoly = dynamic_cast(p2); + Ball* pBall = dynamic_cast(p1); + + if( pPoly != NULL && pBall != NULL ) + { + applyCollision(pPoly, pBall); + return; + } + } + + DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!"); } -void applyCollisionAt(Ball* b1, Ball* b2) +void applyCollision(Ball* b1, Ball* b2) { CollisionInfo cInfo; - if(!getInfoAt(b1, b2, &cInfo)) + if(!getInfo(b1, b2, &cInfo)) return; // a few values to simplify the equations - const Vector2& normal = cInfo.normal; - const Vector2& point = cInfo.point; + const Vector2& normal = cInfo.m_vecNormal; + const Vector2& point = cInfo.m_vecPoint; float m1 = b1->mass; float m2 = b2->mass; @@ -115,7 +251,33 @@ void applyCollisionAt(Ball* b1, Ball* b2) b2->applyImpulse(-impulse / m2 * normal, point); } -bool getInfoAt(Ball* b1, Ball* b2, CollisionInfo* pcInfo) +void applyCollision(Polygon* pPoly, Ball* pBall) +{ + CollisionInfo cInfo; + + if(!getInfo(pPoly, pBall, &cInfo)) + return; + + float fCoR = pBall->CoR; + Vector2 vecNorm = cInfo.m_vecNormal; + + 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 jitters + + // from center to point + Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius; + pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw()); +} + +bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* pcInfo) { // a few values to simplify the equations float r1 = b1->radius; @@ -143,8 +305,59 @@ bool getInfoAt(Ball* b1, Ball* b2, CollisionInfo* pcInfo) 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.x; + float fMinX = pPoly->minP.x; + float fMaxY = pPoly->maxP.y; + float fMinY = pPoly->minP.y; + + // quick binding box check + if (vecPos.x - fRad > fMaxX || vecPos.x + fRad < fMinX || + vecPos.y - fRad > fMaxY || vecPos.y + fRad < fMinY) + return false; + + + Vector2 vecTotVec; + int iTot = 0; + + { + const vector& pts = pPoly->points; + unsigned int num = pts.size(); + + for (unsigned int i = 0; i < num; i++) + { + Vector2 vec = vectorToLine(vecPos, + pts[i].x, + pts[i].y, + pts[(i + 1) % num].x, + pts[(i + 1) % num].y); + + 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; }