X-Git-Url: http://gitweb.pgornicz.com/gitweb.cgi?a=blobdiff_plain;f=src%2FcollisionManager.cpp;h=81d5e052a64728ad5ba36c2b8b532d30b237a19a;hb=9d6dea5f1cfd43cfa136241b352830371acfdc88;hp=5d2dff0872e65def4dd9db2b134468438b289640;hpb=1a4f2f30e4582501500f448fb7b1f0b87f4094f9;p=physics.git diff --git a/src/collisionManager.cpp b/src/collisionManager.cpp index 5d2dff0..81d5e05 100644 --- a/src/collisionManager.cpp +++ b/src/collisionManager.cpp @@ -21,10 +21,13 @@ #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 ***** @@ -35,8 +38,10 @@ void updateEntities(); void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2); void applyCollision(Ball* b1, Ball* b2); +void applyCollision(Polygon* pPoly, Ball* pBall); -bool getInfo(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 ***** @@ -93,27 +98,52 @@ void clearEntities() void placeEntity(PhysicsEntity* p) { - Ball* b = dynamic_cast(p); + Vector2 minP; + Vector2 maxP; - if( b == NULL ) - { - DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!"); - return; + { // Ball case + Ball* b = dynamic_cast(p); + + if( b != NULL ) + { + const float& xb = b->positionRaw().m_fX; + const float& yb = b->positionRaw().m_fY; + const float& rad = b->radius; + + minP.m_fX = xb - rad; + minP.m_fY = yb - rad; + maxP.m_fX = xb + rad; + maxP.m_fY = yb + rad; + + goto start; + } } - const float& xb = b->positionRaw().x; - const float& yb = b->positionRaw().y; - const float& rad = b->radius; + { // Polygon case + Polygon* pPoly = dynamic_cast(p); - for( int x = static_cast( (xb - rad) / (screenX / xDivisions) ); - x <= static_cast( (xb + rad) / (screenX / xDivisions) ); + 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.m_fX / (screenX / xDivisions) ); + x <= static_cast( maxP.m_fX / (screenX / xDivisions) ); x++ ) { if(x < 0 || xDivisions <= x) break; - for( int y = static_cast( (yb - rad) / (screenY / yDivisions) ); - y <= static_cast( (yb + rad) / (screenY / yDivisions) ); + for( int y = static_cast( minP.m_fY / (screenY / yDivisions) ); + y <= static_cast( maxP.m_fY / (screenY / yDivisions) ); y++ ) { if(y < 0 || yDivisions <= y) @@ -154,13 +184,37 @@ void updateEntities() 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 ) { - applyCollision(b1, b2); - return; + Polygon* pPoly = dynamic_cast(p1); + Ball* pBall = dynamic_cast(p2); + + if( pPoly != NULL && pBall != NULL ) + { + applyCollision(pPoly, pBall); + return; + } + } + + { + 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()!!"); @@ -174,8 +228,8 @@ void applyCollision(Ball* b1, Ball* b2) 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; @@ -197,7 +251,33 @@ void applyCollision(Ball* b1, Ball* b2) b2->applyImpulse(-impulse / m2 * normal, point); } -bool getInfo(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; @@ -209,10 +289,10 @@ bool getInfo(Ball* b1, Ball* b2, CollisionInfo* pcInfo) Vector2 v2 = b2->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 @@ -225,8 +305,59 @@ bool getInfo(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.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& 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; }