changed src so the libpg headers are now used

[physics.git] / src / collisionManager.cpp

/*
 *  Copyright (C) 2008 Patrik Gornicz, Gornicz_P (at) hotmail (dot) com.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "collisionManager.h"

#include <pg/debug.h>
#include <pg/Vector2.h>
#include <pg/mathw.h>

#include "Entities/Ball.h"
#include "Entities/Polygon.h"
#include "Entities/PhysicsEntity.h"

#include "CollisionInfo.h"

/// ***** Private Method Headers *****

static void clearEntities();
static void placeEntity(PhysicsEntity*);
static void updateEntities();

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()
{

}
void collision::clean()
{

}

/// ***** Public Methods *****

void collision::update(setPhys& sp)
{
    clearEntities();

    for( setPhys::iterator it = sp.begin();
         it != sp.end();
         it++ )
    {
        placeEntity(*it);
    }

    updateEntities();
}

/// ***** Private Methods *****

void clearEntities()
{
    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;
        }
    }

    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++ )
    {
        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;
        }
    }

    {// 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 applyCollision(Ball* pb1, Ball* pb2)
{
    DASSERT(pb1 != NULL && pb2 != NULL);

    CollisionInfo   cInfo;

    if(!getInfo(pb1, pb2, &cInfo))
        return;

    // a few values to simplify the equations
    const Vector2& vecNormal = cInfo.m_vecNormal;
    const Vector2& vecPoint  = cInfo.m_vecPoint;

    float m1 = pb1->mass;
    float m2 = pb2->mass;

    float e = (pb1->CoR + pb2->CoR) / 2;

    Vector2 v1 = pb1->velocityRaw();
    Vector2 v2 = pb2->velocityRaw();


    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 / (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));

    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());
}

bool getInfo(const Ball* pb1, const Ball* pb2, CollisionInfo* pcInfo)
{
    // a few values to simplify the equations
    float r1 = pb1->radius;
    float r2 = pb2->radius;

    Vector2 p1 = pb1->positionRaw();
    Vector2 p2 = pb2->positionRaw();
    Vector2 v1 = pb1->velocityRaw();
    Vector2 v2 = pb2->velocityRaw();

    // quick binding box check
    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 false;

    // 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 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 true;
}