added suport for ballxpolygon collision ... VERY hacky

[physics.git] / src / collisionManager.cpp

diff --git a/src/collisionManager.cpp b/src/collisionManager.cpp
index 5d2dff0..8aa2dc7 100644 (file)
--- 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<Ball*>(p);
+    Vector2 minP;
+    Vector2 maxP;
 
-    if( b == NULL )
-    {
-        DPF(0, "ENTITY TYPE NOT SUPPORTED BY placeEntity()!!");
-        return;
+    { // Ball case
+        Ball* b = dynamic_cast<Ball*>(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;
+        }
     }
 
-    const float& xb     = b->positionRaw().x;
-    const float& yb     = b->positionRaw().y;
-    const float& rad    = b->radius;
+    { // Polygon case
+        Polygon* pPoly = dynamic_cast<Polygon*>(p);
 
-    for( int x =  static_cast<int>( (xb - rad) / (screenX / xDivisions) );
-             x <= static_cast<int>( (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<int>( minP.x / (screenX / xDivisions) );
+             x <= static_cast<int>( maxP.x / (screenX / xDivisions) );
              x++ )
     {
         if(x < 0 || xDivisions <= x)
             break;
 
-        for( int y =  static_cast<int>( (yb - rad) / (screenY / yDivisions) );
-                 y <= static_cast<int>( (yb + rad) / (screenY / yDivisions) );
+        for( int y =  static_cast<int>( minP.y / (screenY / yDivisions) );
+                 y <= static_cast<int>( maxP.y / (screenY / yDivisions) );
                  y++ )
         {
             if(y < 0 || yDivisions <= y)
@@ -154,13 +184,37 @@ void updateEntities()
 
 void applyCollision(PhysicsEntity* p1, PhysicsEntity* p2)
 {
-    Ball* b1 = dynamic_cast<Ball*>(p1);
-    Ball* b2 = dynamic_cast<Ball*>(p2);
+    {
+        Ball* b1 = dynamic_cast<Ball*>(p1);
+        Ball* b2 = dynamic_cast<Ball*>(p2);
+
+        if( b1 != NULL && b2 != NULL )
+        {
+            applyCollision(b1, b2);
+            return;
+        }
+    }
 
-    if( b1 != NULL && b2 != NULL )
     {
-        applyCollision(b1, b2);
-        return;
+        Polygon*    pPoly   = dynamic_cast<Polygon*>(p1);
+        Ball*       pBall   = dynamic_cast<Ball*>(p2);
+
+        if( pPoly != NULL && pBall != NULL )
+        {
+            applyCollision(pPoly, pBall);
+            return;
+        }
+    }
+
+    {
+        Polygon*    pPoly   = dynamic_cast<Polygon*>(p2);
+        Ball*       pBall   = dynamic_cast<Ball*>(p1);
+
+        if( pPoly != NULL && pBall != NULL )
+        {
+            applyCollision(pPoly, pBall);
+            return;
+        }
     }
 
     DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollision()!!");
@@ -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.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;
@@ -230,3 +310,54 @@ bool getInfo(Ball* b1, Ball* b2, CollisionInfo* pcInfo)
 
     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<Vector2>& 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 true;
+}