more debug changes

[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 "debug.h"

#include "Vector2.h"

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

#include "CollisionInfo.h"

/// ***** Private Class Header *****

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

void applyCollisionAt(PhysicsEntity* p1, PhysicsEntity* p2);
void applyCollisionAt(Ball* b1, Ball* b2);

CollisionInfo* getInfoAt(Ball* b1, Ball* b2);

/// ***** Private Variables *****

/// ***** Initializers/Cleaners *****

void collision::init()
{

}
void collision::clean()
{

}

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

void collision::update(setPhys& sp)
{
    for( setPhys::iterator it1 = sp.begin();
         it1 != sp.end();
         it1++ )
    {
        for( setPhys::iterator it2 = sp.begin();
                it2 != sp.end();
                it2++ )
        {
            if( *it1 != *it2 )
            {
                applyCollisionAt(*it1, *it2);
            }
        }
    }

}

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

void applyCollisionAt(PhysicsEntity* p1, PhysicsEntity* p2)
{
    Ball* b1 = dynamic_cast<Ball*>(p1);
    Ball* b2 = dynamic_cast<Ball*>(p2);

    if( b1 != NULL && b2 != NULL )
    {
        applyCollisionAt(b1, b2);
        return;
    }

    DPF(0, "ENTITY TYPE NOT SUPPORTED BY applyCollisionAt()!!");
}

void applyCollisionAt(Ball* b1, Ball* b2)
{
    // /*
    CollisionInfo* info = getInfoAt(b1, b2);

    if(info == NULL)
        return;

    // a few values to simplify the equations
    Vector2 normal = info->normal;
    Vector2 point = info->point;

    delete info;

    float m1 = b1->mass;
    float m2 = b2->mass;

    float e = (b1->CoR + b2->CoR) / 2;

    Vector2 v1 = b1->velocityRaw();
    Vector2 v2 = b2->velocityRaw();


    float iTop = -(e + 1) * (v1 - v2).dot(normal);

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

    b1->applyImpulse(impulse / m1 * normal, point);
    b2->applyImpulse(-impulse / m2 * normal, point);
    // */
}

CollisionInfo* getInfoAt(Ball* b1, Ball* b2)
{
    // a few values to simplify the equations
    float r1 = b1->radius;
    float r2 = b2->radius;

    Vector2 p1 = b1->positionRaw();
    Vector2 p2 = b2->positionRaw();
    Vector2 v1 = b1->velocityRaw();
    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)
        return NULL;

    // test if not touching
    if ((p1 - p2).sqrLength() >= (r1 + r2)*(r1 + r2))
        return NULL;

    // 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 new CollisionInfo(p1 - (p1 - p2) * r1 / (r1 + r2), p1 - p2);
}