X-Git-Url: http://gitweb.pgornicz.com/gitweb.cgi?a=blobdiff_plain;f=src%2FcollisionManager.cpp;h=8aa2dc78ba2c3380cc4bf03ea41f84a9c16c0762;hb=aa2791cf43a9ddd3a288e504db08e11d03439653;hp=39f63963f938af5d6816100eed33578dbd15a3da;hpb=ca2d526eb092a673f37a906020f1f32d91a608d3;p=physics.git diff --git a/src/collisionManager.cpp b/src/collisionManager.cpp index 39f6396..8aa2dc7 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); -CollisionInfo* getInfoAt(Ball* b1, Ball* b2); +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,52 +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; + } + } + + { + 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 applyCollisionAt()!!"); + DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!"); } -void applyCollisionAt(Ball* b1, Ball* b2) +void applyCollision(Ball* b1, Ball* b2) { - // /* - CollisionInfo* info = getInfoAt(b1, b2); + CollisionInfo cInfo; - if(info == NULL) + if(!getInfo(b1, b2, &cInfo)) return; // a few values to simplify the equations - Vector2 normal = info->normal; - Vector2 point = info->point; - - delete info; + const Vector2& normal = cInfo.normal; + const Vector2& point = cInfo.point; float m1 = b1->mass; float m2 = b2->mass; @@ -116,10 +249,35 @@ void applyCollisionAt(Ball* b1, Ball* b2) b1->applyImpulse(impulse / m1 * normal, point); b2->applyImpulse(-impulse / m2 * normal, point); - // */ } -CollisionInfo* getInfoAt(Ball* b1, Ball* b2) +void applyCollision(Polygon* pPoly, Ball* pBall) +{ + CollisionInfo cInfo; + + if(!getInfo(pPoly, pBall, &cInfo)) + return; + + float fCoR = pBall->CoR; + Vector2 vecNorm = cInfo.normal; + + 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 CollP = normal / normal.Length * Ball.Radius; + //Ball.Position = Info.Point + CollP; +} + +bool getInfo(const Ball* b1, const Ball* b2, CollisionInfo* pcInfo) { // a few values to simplify the equations float r1 = b1->radius; @@ -135,17 +293,71 @@ CollisionInfo* getInfoAt(Ball* b1, Ball* b2) || p1.x + r1 < p2.x - r2 || p1.y - r1 > p2.y + r2 || p1.y + r1 < p2.y - r2) - return NULL; + return false; // test if not touching if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2)) - return NULL; + return false; // test if they are moving apart in some way if they aren't it's likely // that they collided last frame and are still overlapping if ((v1 - v2).dot(p1 - p2) >= 0) - return NULL; + return false; + + pcInfo->point = p1 - (p1 - p2) * r1 / (r1 + r2); + pcInfo->normal = 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->point = vecTotVec / iTot + vecPos; + pcInfo->normal = vecPos - pcInfo->point; - return new CollisionInfo(p1 - (p1 - p2) * r1 / (r1 + r2), p1 - p2); + return true; }