X-Git-Url: http://gitweb.pgornicz.com/gitweb.cgi?a=blobdiff_plain;f=src%2FcollisionManager.cpp;h=dc5a785986c44f4d15cd13c461b0c58e9a589145;hb=7d8eb69997a3000979f0b4cd73c9fbf9d1f10988;hp=39f63963f938af5d6816100eed33578dbd15a3da;hpb=dca497db1c716132a88dc430a86caa5b281162dc;p=physics.git

diff --git a/src/collisionManager.cpp b/src/collisionManager.cpp
index 39f6396..dc5a785 100644
--- a/src/collisionManager.cpp
+++ b/src/collisionManager.cpp
@@ -21,21 +21,35 @@
 #include "Vector2.h"
 
 #include "Entities/Ball.h"
+#include "Entities/Polygon.h"
 #include "Entities/PhysicsEntity.h"
 
 #include "CollisionInfo.h"
 
-/// ***** Private Class Header *****
+#include "mathw.h"
 
 /// ***** 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();
 
-CollisionInfo* getInfoAt(Ball* b1, Ball* b2);
+static void applyCollision(PhysicsEntity*, PhysicsEntity*);
+static void applyCollision(Ball*, Ball*);
+static void applyCollision(Polygon*, Ball*);
+
+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()
@@ -51,101 +65,306 @@ 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++ )
     {
-        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()
 {
-    Ball* b1 = dynamic_cast<Ball*>(p1);
-    Ball* b2 = dynamic_cast<Ball*>(p2);
+    for( int x = 0;
+             x < sc_ixDivisions;
+             x++ )
+    {
+        for( int y = 0;
+                 y < sc_iyDivisions;
+                 y++ )
+        {
+            divisions[x][y].clear();
+        }
+    }
+}
+
+void placeEntity(PhysicsEntity* ppe)
+{
+    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);
+        }
+    }
+}
+
+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 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;
+        }
     }
 
-    DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollisionAt()!!");
+    {// 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 applyCollisionAt(Ball* b1, Ball* b2)
+void applyCollision(Ball* pb1, Ball* pb2)
 {
-    // /*
-    CollisionInfo* info = getInfoAt(b1, b2);
+    DASSERT(pb1 != NULL && pb2 != NULL);
+
+    CollisionInfo   cInfo;
 
-    if(info == NULL)
+    if(!getInfo(pb1, pb2, &cInfo))
         return;
 
     // a few values to simplify the equations
-    Vector2 normal = info->normal;
-    Vector2 point = info->point;
+    const Vector2& vecNormal = cInfo.m_vecNormal;
+    const Vector2& vecPoint  = cInfo.m_vecPoint;
 
-    delete info;
+    float m1 = pb1->mass;
+    float m2 = pb2->mass;
 
-    float m1 = b1->mass;
-    float m2 = b2->mass;
+    float e = (pb1->CoR + pb2->CoR) / 2;
 
-    float e = (b1->CoR + b2->CoR) / 2;
+    Vector2 v1 = pb1->velocityRaw();
+    Vector2 v2 = pb2->velocityRaw();
 
-    Vector2 v1 = b1->velocityRaw();
-    Vector2 v2 = b2->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));
 
-    b1->applyImpulse(impulse / m1 * normal, point);
-    b2->applyImpulse(-impulse / m2 * normal, point);
-    // */
+    pBall->applyImpulse(idm * vecNorm);
+
+    // HACK
+    // CoR penetration fix, adds the polygon-ball jitters
+
+    // from center to point
+    Vector2 vecCollP = vecNorm / vecNorm.length() * pBall->radius;
+    pBall->applyNudge(cInfo.m_vecPoint + vecCollP - pBall->positionRaw());
 }
 
-CollisionInfo* getInfoAt(Ball* b1, Ball* b2)
+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)
-        return NULL;
+    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 NULL;
+        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 NULL;
+        return false;
+
+    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 new CollisionInfo(p1 - (p1 - p2) * r1 / (r1 + r2), p1 - p2);
+    return true;
 }