GamePlay/gameplay/src/PhysicsController.cpp

#include "Base.h"
#include "PhysicsController.h"
#include "PhysicsRigidBody.h"
#include "PhysicsCharacter.h"
#include "Game.h"
#include "MeshPart.h"
#include "Bundle.h"
#include "ScriptListener.h"

#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"

// The initial capacity of the Bullet debug drawer's vertex batch.
#define INITIAL_CAPACITY 280

namespace gameplay
{

const int PhysicsController::DIRTY         = 0x01;
const int PhysicsController::COLLISION     = 0x02;
const int PhysicsController::REGISTERED    = 0x04;
const int PhysicsController::REMOVE        = 0x08;

PhysicsController::PhysicsController()
  : _collisionConfiguration(NULL), _dispatcher(NULL),
    _overlappingPairCache(NULL), _solver(NULL), _world(NULL), _ghostPairCallback(NULL),
    _debugDrawer(NULL), _status(PhysicsController::Listener::DEACTIVATED), _listeners(NULL),
    _gravity(btScalar(0.0), btScalar(-9.8), btScalar(0.0)), _collisionCallback(NULL), _scriptListeners(NULL)
{
    // Default gravity is 9.8 along the negative Y axis.
    _collisionCallback = new CollisionCallback(this);
}

PhysicsController::~PhysicsController()
{
    SAFE_DELETE(_collisionCallback);
    SAFE_DELETE(_ghostPairCallback);
    SAFE_DELETE(_debugDrawer);
    SAFE_DELETE(_listeners);

    if (_scriptListeners)
    {
        for (unsigned int i = 0; i < _scriptListeners->size(); i++)
        {
            SAFE_DELETE((*_scriptListeners)[i]);
        }
        SAFE_DELETE(_scriptListeners);
    }
}

void PhysicsController::addStatusListener(Listener* listener)
{
    GP_ASSERT(listener);
    if (!_listeners)
        _listeners = new std::vector<Listener*>();

    _listeners->push_back(listener);
}

void PhysicsController::removeStatusListener(Listener* listener)
{
    GP_ASSERT(listener);
    if (!_listeners)
        return;

    for (std::vector<Listener*>::iterator iter = _listeners->begin(); iter != _listeners->end(); iter++)
    {
        if (*iter == listener)
        {
            _listeners->erase(iter);
            return;
        }
    }
}

void PhysicsController::addStatusListener(const char* function)
{
    if (!_scriptListeners)
        _scriptListeners = new std::vector<ScriptListener*>();

    ScriptListener* listener = new ScriptListener(function);
    _scriptListeners->push_back(listener);
    addStatusListener(listener);
}

void PhysicsController::removeStatusListener(const char* function)
{
    if (!_scriptListeners)
        return;

    std::string functionStr = function;
    for (unsigned int i = 0; i < _scriptListeners->size(); i++)
    {
        if ((*_scriptListeners)[i]->_function == functionStr)
        {
            removeStatusListener((*_scriptListeners)[i]);
            SAFE_DELETE((*_scriptListeners)[i]);
            _scriptListeners->erase(_scriptListeners->begin() + i);
            return;
        }
    }
}

PhysicsFixedConstraint* PhysicsController::createFixedConstraint(PhysicsRigidBody* a, PhysicsRigidBody* b)
{
    checkConstraintRigidBodies(a, b);
    PhysicsFixedConstraint* constraint = new PhysicsFixedConstraint(a, b);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsGenericConstraint* PhysicsController::createGenericConstraint(PhysicsRigidBody* a, PhysicsRigidBody* b)
{
    checkConstraintRigidBodies(a, b);
    PhysicsGenericConstraint* constraint = new PhysicsGenericConstraint(a, b);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsGenericConstraint* PhysicsController::createGenericConstraint(PhysicsRigidBody* a,
    const Quaternion& rotationOffsetA, const Vector3& translationOffsetA, PhysicsRigidBody* b,
    const Quaternion& rotationOffsetB, const Vector3& translationOffsetB)
{
    checkConstraintRigidBodies(a, b);
    PhysicsGenericConstraint* constraint = new PhysicsGenericConstraint(a, rotationOffsetA, translationOffsetA, b, rotationOffsetB, translationOffsetB);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsHingeConstraint* PhysicsController::createHingeConstraint(PhysicsRigidBody* a,
    const Quaternion& rotationOffsetA, const Vector3& translationOffsetA, PhysicsRigidBody* b, 
    const Quaternion& rotationOffsetB, const Vector3& translationOffsetB)
{
    checkConstraintRigidBodies(a, b);
    PhysicsHingeConstraint* constraint = new PhysicsHingeConstraint(a, rotationOffsetA, translationOffsetA, b, rotationOffsetB, translationOffsetB);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsSocketConstraint* PhysicsController::createSocketConstraint(PhysicsRigidBody* a, PhysicsRigidBody* b)
{
    checkConstraintRigidBodies(a, b);
    PhysicsSocketConstraint* constraint = new PhysicsSocketConstraint(a, b);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsSocketConstraint* PhysicsController::createSocketConstraint(PhysicsRigidBody* a,
    const Vector3& translationOffsetA, PhysicsRigidBody* b, const Vector3& translationOffsetB)
{
    checkConstraintRigidBodies(a, b);
    PhysicsSocketConstraint* constraint = new PhysicsSocketConstraint(a,translationOffsetA, b, translationOffsetB);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsSpringConstraint* PhysicsController::createSpringConstraint(PhysicsRigidBody* a, PhysicsRigidBody* b)
{
    checkConstraintRigidBodies(a, b);
    PhysicsSpringConstraint* constraint = new PhysicsSpringConstraint(a, b);
    addConstraint(a, b, constraint);
    return constraint;
}

PhysicsSpringConstraint* PhysicsController::createSpringConstraint(PhysicsRigidBody* a, const Quaternion& rotationOffsetA, const Vector3& translationOffsetA,           
                                                                   PhysicsRigidBody* b, const Quaternion& rotationOffsetB, const Vector3& translationOffsetB)
{
    checkConstraintRigidBodies(a, b);
    PhysicsSpringConstraint* constraint = new PhysicsSpringConstraint(a, rotationOffsetA, translationOffsetA, b, rotationOffsetB, translationOffsetB);
    addConstraint(a, b, constraint);
    return constraint;
}

const Vector3& PhysicsController::getGravity() const
{
    return _gravity;
}

void PhysicsController::setGravity(const Vector3& gravity)
{
    _gravity = gravity;

    if (_world)
        _world->setGravity(BV(_gravity));
}

void PhysicsController::drawDebug(const Matrix& viewProjection)
{
    GP_ASSERT(_debugDrawer);
    GP_ASSERT(_world);

    _debugDrawer->begin(viewProjection);
    _world->debugDrawWorld();
    _debugDrawer->end();
}

bool PhysicsController::rayTest(const Ray& ray, float distance, PhysicsController::HitResult* result, PhysicsController::HitFilter* filter)
{
    class RayTestCallback : public btCollisionWorld::ClosestRayResultCallback
    {
    private:

        HitFilter* filter;
        HitResult hitResult;

    public:

        RayTestCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, PhysicsController::HitFilter* filter)
            : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), filter(filter)
        {
        }

		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
		{
            if (!btCollisionWorld::ClosestRayResultCallback::needsCollision(proxy0))
                return false;

            btCollisionObject* co = reinterpret_cast<btCollisionObject*>(proxy0->m_clientObject);
            PhysicsCollisionObject* object = reinterpret_cast<PhysicsCollisionObject*>(co->getUserPointer());
            if (object == NULL)
                return false;

            return filter ? !filter->filter(object) : true;
        }

        btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
        {
            GP_ASSERT(rayResult.m_collisionObject);
            PhysicsCollisionObject* object = reinterpret_cast<PhysicsCollisionObject*>(rayResult.m_collisionObject->getUserPointer());

            if (object == NULL)
                return 1.0f; // ignore

            float result = btCollisionWorld::ClosestRayResultCallback::addSingleResult(rayResult, normalInWorldSpace);

            hitResult.object = object;
            hitResult.point.set(m_hitPointWorld.x(), m_hitPointWorld.y(), m_hitPointWorld.z());
            hitResult.fraction = m_closestHitFraction;
            hitResult.normal.set(m_hitNormalWorld.x(), m_hitNormalWorld.y(), m_hitNormalWorld.z());

            if (filter && !filter->hit(hitResult))
                return 1.0f; // process next collision

            return result; // continue normally
        }
    };

    GP_ASSERT(_world);

    btVector3 rayFromWorld(BV(ray.getOrigin()));
    btVector3 rayToWorld(rayFromWorld + BV(ray.getDirection() * distance));

    RayTestCallback callback(rayFromWorld, rayToWorld, filter);
    _world->rayTest(rayFromWorld, rayToWorld, callback);
    if (callback.hasHit())
    {
        if (result)
        {
            result->object = getCollisionObject(callback.m_collisionObject);
            result->point.set(callback.m_hitPointWorld.x(), callback.m_hitPointWorld.y(), callback.m_hitPointWorld.z());
            result->fraction = callback.m_closestHitFraction;
            result->normal.set(callback.m_hitNormalWorld.x(), callback.m_hitNormalWorld.y(), callback.m_hitNormalWorld.z());
        }

        return true;
    }

    return false;
}

bool PhysicsController::sweepTest(PhysicsCollisionObject* object, const Vector3& endPosition, PhysicsController::HitResult* result, PhysicsController::HitFilter* filter)
{
    class SweepTestCallback : public btCollisionWorld::ClosestConvexResultCallback
    {
    private:

        PhysicsCollisionObject* me;
        PhysicsController::HitFilter* filter;
        PhysicsController::HitResult hitResult;

    public:

        SweepTestCallback(PhysicsCollisionObject* me, PhysicsController::HitFilter* filter)
            : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)), me(me), filter(filter)
        {
        }

		virtual bool needsCollision(btBroadphaseProxy* proxy0) const
		{
            if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
                return false;

            btCollisionObject* co = reinterpret_cast<btCollisionObject*>(proxy0->m_clientObject);
            PhysicsCollisionObject* object = reinterpret_cast<PhysicsCollisionObject*>(co->getUserPointer());
            if (object == NULL || object == me)
                return false;

            return filter ? !filter->filter(object) : true;
        }

        btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace)
        {
            GP_ASSERT(convexResult.m_hitCollisionObject);
            PhysicsCollisionObject* object = reinterpret_cast<PhysicsCollisionObject*>(convexResult.m_hitCollisionObject->getUserPointer());

            if (object == NULL)
                return 1.0f;

            float result = ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace);

            hitResult.object = object;
            hitResult.point.set(m_hitPointWorld.x(), m_hitPointWorld.y(), m_hitPointWorld.z());
            hitResult.fraction = m_closestHitFraction;
            hitResult.normal.set(m_hitNormalWorld.x(), m_hitNormalWorld.y(), m_hitNormalWorld.z());

            if (filter && !filter->hit(hitResult))
                return 1.0f;

            return result;
        }
    };

    GP_ASSERT(object && object->getCollisionShape());
    PhysicsCollisionShape* shape = object->getCollisionShape();
    PhysicsCollisionShape::Type type = shape->getType();
    if (type != PhysicsCollisionShape::SHAPE_BOX && type != PhysicsCollisionShape::SHAPE_SPHERE && type != PhysicsCollisionShape::SHAPE_CAPSULE)
        return false; // unsupported type

    // Define the start transform.
    btTransform start;
    start.setIdentity();
    if (object->getNode())
    {
        Vector3 translation;
        Quaternion rotation;
        const Matrix& m = object->getNode()->getWorldMatrix();
        m.getTranslation(&translation);
        m.getRotation(&rotation);

        start.setIdentity();
        start.setOrigin(BV(translation));
        start.setRotation(BQ(rotation));
    }

    // Define the end transform.
    btTransform end(start);
    end.setOrigin(BV(endPosition));

    // Perform bullet convex sweep test.
    SweepTestCallback callback(object, filter);

    // If the object is represented by a ghost object, use the ghost object's convex sweep test
    // since it is much faster than the world's version.
    // NOTE: Unfortunately the ghost object sweep test does not seem reliable here currently, so using world's version instead.
    /*switch (object->getType())
    {
    case PhysicsCollisionObject::GHOST_OBJECT:
    case PhysicsCollisionObject::CHARACTER:
        static_cast<PhysicsGhostObject*>(object)->_ghostObject->convexSweepTest(static_cast<btConvexShape*>(shape->getShape()), start, end, callback, _world->getDispatchInfo().m_allowedCcdPenetration);
        break;

    default:
        _world->convexSweepTest(static_cast<btConvexShape*>(shape->getShape()), start, end, callback, _world->getDispatchInfo().m_allowedCcdPenetration);
        break;
    }*/

    GP_ASSERT(_world);
    _world->convexSweepTest(static_cast<btConvexShape*>(shape->getShape()), start, end, callback, _world->getDispatchInfo().m_allowedCcdPenetration);

    // Check for hits and store results.
    if (callback.hasHit())
    {
        if (result)
        {
            result->object = getCollisionObject(callback.m_hitCollisionObject);
            result->point.set(callback.m_hitPointWorld.x(), callback.m_hitPointWorld.y(), callback.m_hitPointWorld.z());
            result->fraction = callback.m_closestHitFraction;
            result->normal.set(callback.m_hitNormalWorld.x(), callback.m_hitNormalWorld.y(), callback.m_hitNormalWorld.z());
        }

        return true;
    }

    return false;
}

btScalar PhysicsController::CollisionCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObject* a, int partIdA, int indexA, 
    const btCollisionObject* b, int partIdB, int indexB)
{
    GP_ASSERT(_pc);

    // Get pointers to the PhysicsCollisionObject objects.
    PhysicsCollisionObject* objectA = _pc->getCollisionObject(a);
    PhysicsCollisionObject* objectB = _pc->getCollisionObject(b);

    // If the given collision object pair has collided in the past, then
    // we notify the listeners only if the pair was not colliding
    // during the previous frame. Otherwise, it's a new pair, so add a
    // new entry to the cache with the appropriate listeners and notify them.
    PhysicsCollisionObject::CollisionPair pair(objectA, objectB);

    CollisionInfo* collisionInfo;
    if (_pc->_collisionStatus.count(pair) > 0)
    {
        collisionInfo = &_pc->_collisionStatus[pair];
    }
    else
    {
        // Add a new collision pair for these objects.
        collisionInfo = &_pc->_collisionStatus[pair];

        // Add the appropriate listeners.
        PhysicsCollisionObject::CollisionPair p1(pair.objectA, NULL);
        if (_pc->_collisionStatus.count(p1) > 0)
        {
            const CollisionInfo& ci = _pc->_collisionStatus[p1];
            std::vector<PhysicsCollisionObject::CollisionListener*>::const_iterator iter = ci._listeners.begin();
            for (; iter != ci._listeners.end(); iter++)
            {
                GP_ASSERT(*iter);
                collisionInfo->_listeners.push_back(*iter);
            }
        }
        PhysicsCollisionObject::CollisionPair p2(pair.objectB, NULL);
        if (_pc->_collisionStatus.count(p2) > 0)
        {
            const CollisionInfo& ci = _pc->_collisionStatus[p2];
            std::vector<PhysicsCollisionObject::CollisionListener*>::const_iterator iter = ci._listeners.begin();
            for (; iter != ci._listeners.end(); iter++)
            {
                GP_ASSERT(*iter);
                collisionInfo->_listeners.push_back(*iter);
            }
        }
    }

    // Fire collision event.
    if ((collisionInfo->_status & COLLISION) == 0)
    {
        std::vector<PhysicsCollisionObject::CollisionListener*>::const_iterator iter = collisionInfo->_listeners.begin();
        for (; iter != collisionInfo->_listeners.end(); iter++)
        {
            GP_ASSERT(*iter);
            if ((collisionInfo->_status & REMOVE) == 0)
            {
                (*iter)->collisionEvent(PhysicsCollisionObject::CollisionListener::COLLIDING, pair, Vector3(cp.getPositionWorldOnA().x(), cp.getPositionWorldOnA().y(), cp.getPositionWorldOnA().z()),
                    Vector3(cp.getPositionWorldOnB().x(), cp.getPositionWorldOnB().y(), cp.getPositionWorldOnB().z()));
            }
        }
    }

    // Update the collision status cache (we remove the dirty bit
    // set in the controller's update so that this particular collision pair's
    // status is not reset to 'no collision' when the controller's update completes).
    collisionInfo->_status &= ~DIRTY;
    collisionInfo->_status |= COLLISION;
    return 0.0f;
}

void PhysicsController::initialize()
{
    _collisionConfiguration = new btDefaultCollisionConfiguration();
    _dispatcher = new btCollisionDispatcher(_collisionConfiguration);
    _overlappingPairCache = new btDbvtBroadphase();
    _solver = new btSequentialImpulseConstraintSolver();

    // Create the world.
    _world = new btDiscreteDynamicsWorld(_dispatcher, _overlappingPairCache, _solver, _collisionConfiguration);
    _world->setGravity(BV(_gravity));

    // Register ghost pair callback so bullet detects collisions with ghost objects (used for character collisions).
    GP_ASSERT(_world->getPairCache());
    _ghostPairCallback = bullet_new<btGhostPairCallback>();
    _world->getPairCache()->setInternalGhostPairCallback(_ghostPairCallback);
    _world->getDispatchInfo().m_allowedCcdPenetration = 0.0001f;

    // Set up debug drawing.
    _debugDrawer = new DebugDrawer();
    _world->setDebugDrawer(_debugDrawer);
}

void PhysicsController::finalize()
{
    // Clean up the world and its various components.
    SAFE_DELETE(_world);
    SAFE_DELETE(_ghostPairCallback);
    SAFE_DELETE(_solver);
    SAFE_DELETE(_overlappingPairCache);
    SAFE_DELETE(_dispatcher);
    SAFE_DELETE(_collisionConfiguration);
}

void PhysicsController::pause()
{
    // Unused
}

void PhysicsController::resume()
{
    // Unused
}

void PhysicsController::update(float elapsedTime)
{
    GP_ASSERT(_world);

    // Update the physics simulation, with a maximum
    // of 10 simulation steps being performed in a given frame.
    //
    // Note that stepSimulation takes elapsed time in seconds
    // so we divide by 1000 to convert from milliseconds.
    _world->stepSimulation(elapsedTime * 0.001f, 10);

    // If we have status listeners, then check if our status has changed.
    if (_listeners)
    {
        Listener::EventType oldStatus = _status;

        if (_status == Listener::DEACTIVATED)
        {
            for (int i = 0; i < _world->getNumCollisionObjects(); i++)
            {
                GP_ASSERT(_world->getCollisionObjectArray()[i]);
                if (_world->getCollisionObjectArray()[i]->isActive())
                {
                    _status = Listener::ACTIVATED;
                    break;
                }
            }
        }
        else
        {
            bool allInactive = true;
            for (int i = 0; i < _world->getNumCollisionObjects(); i++)
            {
                GP_ASSERT(_world->getCollisionObjectArray()[i]);
                if (_world->getCollisionObjectArray()[i]->isActive())
                {
                    allInactive = false;
                    break;
                }
            }

            if (allInactive)
                _status = Listener::DEACTIVATED;
        }

        // If the status has changed, notify our listeners.
        if (oldStatus != _status)
        {
            for (unsigned int k = 0; k < _listeners->size(); k++)
            {
                GP_ASSERT((*_listeners)[k]);
                (*_listeners)[k]->statusEvent(_status);
            }
        }
    }

    // All statuses are set with the DIRTY bit before collision processing occurs.
    // During collision processing, if a collision occurs, the status is 
    // set to COLLISION and the DIRTY bit is cleared. Then, after collision processing 
    // is finished, if a given status is still dirty, the COLLISION bit is cleared.
    //
    // If an entry was marked for removal in the last frame, remove it now.

    // Dirty the collision status cache entries.
    std::map<PhysicsCollisionObject::CollisionPair, CollisionInfo>::iterator iter = _collisionStatus.begin();
    for (; iter != _collisionStatus.end();)
    {
        if ((iter->second._status & REMOVE) != 0)
        {
            std::map<PhysicsCollisionObject::CollisionPair, CollisionInfo>::iterator eraseIter = iter;
            iter++;
            _collisionStatus.erase(eraseIter);
        }
        else
        {
            iter->second._status |= DIRTY;
            iter++;
        }
    }

    // Go through the collision status cache and perform all registered collision tests.
    iter = _collisionStatus.begin();
    for (; iter != _collisionStatus.end(); iter++)
    {
        // If this collision pair was one that was registered for listening, then perform the collision test.
        // (In the case where we register for all collisions with a rigid body, there will be a lot
        // of collision pairs in the status cache that we did not explicitly register for.)
        if ((iter->second._status & REGISTERED) != 0 && (iter->second._status & REMOVE) == 0)
        {
            if (iter->first.objectB)
                _world->contactPairTest(iter->first.objectA->getCollisionObject(), iter->first.objectB->getCollisionObject(), *_collisionCallback);
            else
                _world->contactTest(iter->first.objectA->getCollisionObject(), *_collisionCallback);
        }
    }

    // Update all the collision status cache entries.
    iter = _collisionStatus.begin();
    for (; iter != _collisionStatus.end(); iter++)
    {
        if ((iter->second._status & DIRTY) != 0)
        {
            if ((iter->second._status & COLLISION) != 0 && iter->first.objectB)
            {
                unsigned int size = iter->second._listeners.size();
                for (unsigned int i = 0; i < size; i++)
                {
                    iter->second._listeners[i]->collisionEvent(PhysicsCollisionObject::CollisionListener::NOT_COLLIDING, iter->first);
                }
            }

            iter->second._status &= ~COLLISION;
        }
    }
}

void PhysicsController::addCollisionListener(PhysicsCollisionObject::CollisionListener* listener, PhysicsCollisionObject* objectA, PhysicsCollisionObject* objectB)
{
    GP_ASSERT(listener);
    
    // One of the collision objects in the pair must be non-null.
    GP_ASSERT(objectA || objectB);
    PhysicsCollisionObject::CollisionPair pair(objectA, objectB);

    // Add the listener and ensure the status includes that this collision pair is registered.
    CollisionInfo& info = _collisionStatus[pair];
    info._listeners.push_back(listener);
    info._status |= PhysicsController::REGISTERED;
}

void PhysicsController::removeCollisionListener(PhysicsCollisionObject::CollisionListener* listener, PhysicsCollisionObject* objectA, PhysicsCollisionObject* objectB)
{
    // One of the collision objects in the pair must be non-null.
    GP_ASSERT(objectA || objectB);
    PhysicsCollisionObject::CollisionPair pair(objectA, objectB);

    // Mark the collision pair for these objects for removal.
    if (_collisionStatus.count(pair) > 0)
    {
        _collisionStatus[pair]._status |= REMOVE;
    }
}

void PhysicsController::addCollisionObject(PhysicsCollisionObject* object)
{
    GP_ASSERT(object && object->getCollisionObject());
    GP_ASSERT(_world);

    // Assign user pointer for the bullet collision object to allow efficient
    // lookups of bullet objects -> gameplay objects.
    object->getCollisionObject()->setUserPointer(object);

    // Add the object to the physics world.
    switch (object->getType())
    {
    case PhysicsCollisionObject::RIGID_BODY:
        _world->addRigidBody(static_cast<btRigidBody*>(object->getCollisionObject()), btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::DefaultFilter | btBroadphaseProxy::StaticFilter | btBroadphaseProxy::CharacterFilter | btBroadphaseProxy::AllFilter);
        break;

    case PhysicsCollisionObject::CHARACTER:
        _world->addCollisionObject(object->getCollisionObject(), btBroadphaseProxy::CharacterFilter, btBroadphaseProxy::DefaultFilter | btBroadphaseProxy::StaticFilter | btBroadphaseProxy::CharacterFilter | btBroadphaseProxy::AllFilter);
        break;

    case PhysicsCollisionObject::GHOST_OBJECT:
        _world->addCollisionObject(object->getCollisionObject(), btBroadphaseProxy::DefaultFilter, btBroadphaseProxy::DefaultFilter | btBroadphaseProxy::StaticFilter | btBroadphaseProxy::CharacterFilter | btBroadphaseProxy::AllFilter);
        break;

    default:
        GP_ERROR("Unsupported collision object type (%d).", object->getType());
        break;
    }
}

void PhysicsController::removeCollisionObject(PhysicsCollisionObject* object)
{
    GP_ASSERT(object);
    GP_ASSERT(_world);

    // Remove the collision object from the world.
    if (object->getCollisionObject())
    {
        switch (object->getType())
        {
        case PhysicsCollisionObject::RIGID_BODY:
            _world->removeRigidBody(static_cast<btRigidBody*>(object->getCollisionObject()));
            break;

        case PhysicsCollisionObject::CHARACTER:
        case PhysicsCollisionObject::GHOST_OBJECT:
            _world->removeCollisionObject(object->getCollisionObject());
            break;

        default:
            GP_ERROR("Unsupported collision object type (%d).", object->getType());
            break;
        }
    }

    // Find all references to the object in the collision status cache and mark them for removal.
    std::map<PhysicsCollisionObject::CollisionPair, CollisionInfo>::iterator iter = _collisionStatus.begin();
    for (; iter != _collisionStatus.end(); iter++)
    {
        if (iter->first.objectA == object || iter->first.objectB == object)
            iter->second._status |= REMOVE;
    }
}

PhysicsCollisionObject* PhysicsController::getCollisionObject(const btCollisionObject* collisionObject) const
{
    // Gameplay collision objects are stored in the userPointer data of Bullet collision objects.
    GP_ASSERT(collisionObject);
    return reinterpret_cast<PhysicsCollisionObject*>(collisionObject->getUserPointer());
}

static void getBoundingBox(Node* node, BoundingBox* out, bool merge = false)
{
    GP_ASSERT(node);
    GP_ASSERT(out);

    if (node->getModel())
    {
        GP_ASSERT(node->getModel()->getMesh());

        if (merge)
            out->merge(node->getModel()->getMesh()->getBoundingBox());
        else
        {
            out->set(node->getModel()->getMesh()->getBoundingBox());
            merge = true;
        }
    }

    Node* child = node->getFirstChild();
    while (child)
    {
        getBoundingBox(child, out, merge);
        child = child->getNextSibling();
    }
}

static void getBoundingSphere(Node* node, BoundingSphere* out, bool merge = false)
{
    GP_ASSERT(node);
    GP_ASSERT(out);

    if (node->getModel())
    {
        GP_ASSERT(node->getModel()->getMesh());

        if (merge)
            out->merge(node->getModel()->getMesh()->getBoundingSphere());
        else
        {
            out->set(node->getModel()->getMesh()->getBoundingSphere());
            merge = true;
        }
    }

    Node* child = node->getFirstChild();
    while (child)
    {
        getBoundingSphere(child, out, merge);
        child = child->getNextSibling();
    }
}

static void computeCenterOfMass(const Vector3& center, const Vector3& scale, Vector3* centerOfMassOffset)
{
    GP_ASSERT(centerOfMassOffset);

    // Update center of mass offset.
    *centerOfMassOffset = center;
    centerOfMassOffset->x *= scale.x;
    centerOfMassOffset->y *= scale.y;
    centerOfMassOffset->z *= scale.z;
    centerOfMassOffset->negate();
}

PhysicsCollisionShape* PhysicsController::createShape(Node* node, const PhysicsCollisionShape::Definition& shape, Vector3* centerOfMassOffset)
{
    GP_ASSERT(node);

    PhysicsCollisionShape* collisionShape = NULL;

    // Get the node's world scale (we need to apply this during creation since rigid bodies don't scale dynamically).
    Vector3 scale;
    node->getWorldMatrix().getScale(&scale);

    switch (shape.type)
    {
    case PhysicsCollisionShape::SHAPE_BOX:
        {
            if (shape.isExplicit)
            {
                // Use the passed in box information.
                collisionShape = createBox(Vector3(shape.data.box.extents), Vector3::one());

                if (shape.centerAbsolute)
                {
                    computeCenterOfMass(Vector3(shape.data.box.center), Vector3::one(), centerOfMassOffset);
                }
                else
                {
                    BoundingBox box;
                    getBoundingBox(node, &box);
                    computeCenterOfMass(box.getCenter() + Vector3(shape.data.box.center), scale, centerOfMassOffset);
                }
            }
            else
            {
                // Automatically compute bounding box from mesh's bounding box.
                BoundingBox box;
                getBoundingBox(node, &box);
                collisionShape = createBox(Vector3(std::fabs(box.max.x - box.min.x), std::fabs(box.max.y - box.min.y), std::fabs(box.max.z - box.min.z)), scale);

                computeCenterOfMass(box.getCenter(), scale, centerOfMassOffset);
            }
        }
        break;

    case PhysicsCollisionShape::SHAPE_SPHERE:
        {
            if (shape.isExplicit)
            {
                // Use the passed in sphere information.
                collisionShape = createSphere(shape.data.sphere.radius, Vector3::one());

                if (shape.centerAbsolute)
                {
                    computeCenterOfMass(Vector3(shape.data.sphere.center), Vector3::one(), centerOfMassOffset);
                }
                else
                {
                    BoundingSphere sphere;
                    getBoundingSphere(node, &sphere);
                    computeCenterOfMass(sphere.center + Vector3(shape.data.sphere.center), scale, centerOfMassOffset);
                }
            }
            else
            {
                // Automatically compute bounding sphere from mesh's bounding sphere.
                BoundingSphere sphere;
                getBoundingSphere(node, &sphere);
                collisionShape = createSphere(sphere.radius, scale);

                computeCenterOfMass(sphere.center, scale, centerOfMassOffset);
            }
        }
        break;

    case PhysicsCollisionShape::SHAPE_CAPSULE:
        {
            if (shape.isExplicit)
            {
                // Use the passed in capsule information.
                collisionShape = createCapsule(shape.data.capsule.radius, shape.data.capsule.height, Vector3::one());

                if (shape.centerAbsolute)
                {
                    computeCenterOfMass(Vector3(shape.data.capsule.center), Vector3::one(), centerOfMassOffset);
                }
                else
                {
                    BoundingBox box;
                    getBoundingBox(node, &box);
                    computeCenterOfMass(box.getCenter() + Vector3(shape.data.capsule.center), scale, centerOfMassOffset);
                }
            }
            else
            {
                // Compute a capsule shape that roughly matches the bounding box of the mesh.
                BoundingBox box;
                getBoundingBox(node, &box);
                float radius = std::max((box.max.x - box.min.x) * 0.5f, (box.max.z - box.min.z) * 0.5f);
                float height = box.max.y - box.min.y;
                collisionShape = createCapsule(radius, height, scale);

                computeCenterOfMass(box.getCenter(), scale, centerOfMassOffset);
            }
        }
        break;

    case PhysicsCollisionShape::SHAPE_HEIGHTFIELD:
        {
            // Build heightfield rigid body from the passed in shape.
            collisionShape = createHeightfield(node, shape.data.heightfield, centerOfMassOffset);
        }
        break;

    case PhysicsCollisionShape::SHAPE_MESH:
        {
            // Build mesh from passed in shape.
            collisionShape = createMesh(shape.data.mesh, scale);
        }
        break;
    default:
        GP_ERROR("Unsupported collision shape type (%d).", shape.type);
        break;
    }

    return collisionShape;
}

PhysicsCollisionShape* PhysicsController::createBox(const Vector3& extents, const Vector3& scale)
{
    btVector3 halfExtents(scale.x * 0.5 * extents.x, scale.y * 0.5 * extents.y, scale.z * 0.5 * extents.z);

    PhysicsCollisionShape* shape;

    // Return the box shape from the cache if it already exists.
    for (unsigned int i = 0; i < _shapes.size(); ++i)
    {
        shape = _shapes[i];
        GP_ASSERT(shape);
        if (shape->getType() == PhysicsCollisionShape::SHAPE_BOX)
        {
            btBoxShape* box = static_cast<btBoxShape*>(shape->_shape);
            if (box && box->getHalfExtentsWithMargin() == halfExtents)
            {
                shape->addRef();
                return shape;
            }
        }
    }

    // Create the box shape and add it to the cache.
    shape = new PhysicsCollisionShape(PhysicsCollisionShape::SHAPE_BOX, bullet_new<btBoxShape>(halfExtents));
    _shapes.push_back(shape);

    return shape;
}

PhysicsCollisionShape* PhysicsController::createSphere(float radius, const Vector3& scale)
{
    // Since sphere shapes depend only on the radius, the best we can do is take
    // the largest dimension and apply that as the uniform scale to the rigid body.
    float uniformScale = scale.x;
    if (uniformScale < scale.y)
        uniformScale = scale.y;
    if (uniformScale < scale.z)
        uniformScale = scale.z;

    float scaledRadius = radius * uniformScale;

    PhysicsCollisionShape* shape;

    // Return the sphere shape from the cache if it already exists.
    for (unsigned int i = 0; i < _shapes.size(); ++i)
    {
        shape = _shapes[i];
        GP_ASSERT(shape);
        if (shape->getType() == PhysicsCollisionShape::SHAPE_SPHERE)
        {
            btSphereShape* sphere = static_cast<btSphereShape*>(shape->_shape);
            if (sphere && sphere->getRadius() == scaledRadius)
            {
                shape->addRef();
                return shape;
            }
        }
    }

    // Create the sphere shape and add it to the cache.
    shape = new PhysicsCollisionShape(PhysicsCollisionShape::SHAPE_SPHERE, bullet_new<btSphereShape>(scaledRadius));
    _shapes.push_back(shape);

    return shape;
}

PhysicsCollisionShape* PhysicsController::createCapsule(float radius, float height, const Vector3& scale)
{
    float girthScale = scale.x;
    if (girthScale < scale.z)
        girthScale = scale.z;
    float scaledRadius = radius * girthScale;
    float scaledHeight = height * scale.y - radius * 2;

    PhysicsCollisionShape* shape;

    // Return the capsule shape from the cache if it already exists.
    for (unsigned int i = 0; i < _shapes.size(); i++)
    {
        shape = _shapes[i];
        GP_ASSERT(shape);
        if (shape->getType() == PhysicsCollisionShape::SHAPE_CAPSULE)
        {
            btCapsuleShape* capsule = static_cast<btCapsuleShape*>(shape->_shape);
            if (capsule && capsule->getRadius() == scaledRadius && capsule->getHalfHeight() == 0.5f * scaledHeight)
            {
                shape->addRef();
                return shape;
            }
        }
    }

    // Create the capsule shape and add it to the cache.
    shape = new PhysicsCollisionShape(PhysicsCollisionShape::SHAPE_CAPSULE, bullet_new<btCapsuleShape>(scaledRadius, scaledHeight));
    _shapes.push_back(shape);

    return shape;
}

PhysicsCollisionShape* PhysicsController::createHeightfield(Node* node, Image* image, Vector3* centerOfMassOffset)
{
    GP_ASSERT(node);
    GP_ASSERT(image);
    GP_ASSERT(centerOfMassOffset);

    // Get the dimensions of the heightfield.
    // If the node has a mesh defined, use the dimensions of the bounding box for the mesh.
    // Otherwise simply use the image dimensions (with a max height of 255).
    float width, length, minHeight, maxHeight;
    if (node->getModel() && node->getModel()->getMesh())
    {
        const BoundingBox& box = node->getModel()->getMesh()->getBoundingBox();
        width = box.max.x - box.min.x;
        length = box.max.z - box.min.z;
        minHeight = box.min.y;
        maxHeight = box.max.y;
    }
    else
    {
        width = image->getWidth();
        length = image->getHeight();
        minHeight = 0.0f;
        maxHeight = 255.0f;
    }

    // Get the size in bytes of a pixel (we ensure that the image's
    // pixel format is actually supported before calling this constructor).
    unsigned int pixelSize = 0;
    switch (image->getFormat())
    {
        case Image::RGB:
            pixelSize = 3;
            break;
        case Image::RGBA:
            pixelSize = 4;
            break;
        default:
            GP_ERROR("Unsupported pixel format for heightmap image (%d).", image->getFormat());
            return NULL;
    }

    // Calculate the heights for each pixel.
    float* heights = new float[image->getWidth() * image->getHeight()];
    unsigned char* data = image->getData();
    for (unsigned int x = 0, w = image->getWidth(); x < w; ++x)
    {
        for (unsigned int y = 0, h = image->getHeight(); y < h; ++y)
        {
            heights[x + y * w] = ((((float)data[(x + y * h) * pixelSize + 0]) +
                ((float)data[(x + y * h) * pixelSize + 1]) +
                ((float)data[(x + y * h) * pixelSize + 2])) / 768.0f) * (maxHeight - minHeight) + minHeight;
        }
    }

    PhysicsCollisionShape::HeightfieldData* heightfieldData = new PhysicsCollisionShape::HeightfieldData();
    heightfieldData->heightData = NULL;
    heightfieldData->normalData = NULL;
    heightfieldData->inverseIsDirty = true;

    unsigned int sizeWidth = width;
    unsigned int sizeHeight = length;
    GP_ASSERT(sizeWidth);
    GP_ASSERT(sizeHeight);
    
    // Generate the heightmap data needed for physics (one height per world unit).
    heightfieldData->width = sizeWidth + 1;
    heightfieldData->height = sizeHeight + 1;
    heightfieldData->heightData = new float[heightfieldData->width * heightfieldData->height];
    heightfieldData->normalData = new Vector3[heightfieldData->width * heightfieldData->height];
    unsigned int heightIndex = 0, prevRowIndex = 0, prevColIndex = 0;
    float widthImageFactor = (float)(image->getWidth() - 1) / sizeWidth;
    float heightImageFactor = (float)(image->getHeight() - 1) / sizeHeight;
    float x = 0.0f;
    float z = 0.0f;
    const float horizStepsize = 1.0f;
    for (unsigned int row = 0, z = 0.0f; row <= sizeHeight; row++, z += horizStepsize)
    {
        for (unsigned int col = 0, x = 0.0f; col <= sizeWidth; col++, x += horizStepsize)
        {
            heightIndex = row * heightfieldData->width + col;
            prevRowIndex = heightIndex - heightfieldData->width; // ignored if row<1
            prevColIndex = heightIndex - 1; // ignored if col<1

            heightfieldData->heightData[heightIndex] = calculateHeight(heights, image->getWidth(), image->getHeight(), x * widthImageFactor, (sizeHeight - z) * heightImageFactor);

            //
            // Normal calculation based on height data using a backward difference.
            //
            if (row == 0 || col == 0)
            {
                // This is just a safe default value.
                heightfieldData->normalData[heightIndex].set(Vector3::unitY());
            }
            else
            {
                heightfieldData->normalData[heightIndex].set(
                    heightfieldData->heightData[prevColIndex] - heightfieldData->heightData[heightIndex],
                    horizStepsize,
                    heightfieldData->heightData[prevRowIndex] - heightfieldData->heightData[heightIndex]);
                heightfieldData->normalData[heightIndex].normalize();
            }

            // For the starting row, just copy from the second row (i.e., a forward difference).
            if (row == 1)
            {
                heightfieldData->normalData[prevRowIndex].set(heightfieldData->normalData[heightIndex]);
            }

            // For the starting column, just copy from the second column (i.e., a forward difference).
            // (We don't care which of the 2 valid sources heightfieldData->normalData[0] ultimately comes from).
            if (col == 1)
            {
                heightfieldData->normalData[prevColIndex].set(heightfieldData->normalData[heightIndex]);
            }
        }
    }
    SAFE_DELETE_ARRAY(heights);

    // Offset the heightmap's center of mass according to the way that Bullet calculates the origin 
    // of its heightfield collision shape; see documentation for the btHeightfieldTerrainShape for more info.
    Vector3 s;
    node->getWorldMatrix().getScale(&s);
    GP_ASSERT(s.y);
    centerOfMassOffset->set(0.0f, -(maxHeight - (0.5f * (maxHeight - minHeight))) / s.y, 0.0f);

    // Create the bullet terrain shape.
    btHeightfieldTerrainShape* terrainShape = bullet_new<btHeightfieldTerrainShape>(
        heightfieldData->width, heightfieldData->height, heightfieldData->heightData, 1.0f, minHeight, maxHeight, 1, PHY_FLOAT, false);

    // Create our collision shape object and store heightfieldData in it.
    PhysicsCollisionShape* shape = new PhysicsCollisionShape(PhysicsCollisionShape::SHAPE_HEIGHTFIELD, terrainShape);
    shape->_shapeData.heightfieldData = heightfieldData;

    _shapes.push_back(shape);

    return shape;
}

PhysicsCollisionShape* PhysicsController::createMesh(Mesh* mesh, const Vector3& scale)
{
    GP_ASSERT(mesh);

    // Only support meshes with triangle list primitive types.
    bool triMesh = true;
    if (mesh->getPartCount() > 0)
    {
        for (unsigned int i = 0; i < mesh->getPartCount(); ++i)
        {
            if (mesh->getPart(i)->getPrimitiveType() != Mesh::TRIANGLES)
            {
                triMesh = false;
                break;
            }
        }
    }
    else
    {
        triMesh = mesh->getPrimitiveType() == Mesh::TRIANGLES;
    }

    if (!triMesh)
    {
        GP_ERROR("Mesh rigid bodies are currently only supported on meshes with TRIANGLES primitive type.");
        return NULL;
    }

    // The mesh must have a valid URL (i.e. it must have been loaded from a Bundle)
    // in order to fetch mesh data for computing mesh rigid body.
    if (strlen(mesh->getUrl()) == 0)
    {
        GP_ERROR("Cannot create mesh rigid body for mesh without valid URL.");
        return NULL;
    }

    Bundle::MeshData* data = Bundle::readMeshData(mesh->getUrl());
    if (data == NULL)
    {
        GP_ERROR("Failed to load mesh data from url '%s'.", mesh->getUrl());
        return NULL;
    }

    // Create mesh data to be populated and store in returned collision shape.
    PhysicsCollisionShape::MeshData* shapeMeshData = new PhysicsCollisionShape::MeshData();
    shapeMeshData->vertexData = NULL;

    // Copy the scaled vertex position data to the rigid body's local buffer.
    Matrix m;
    Matrix::createScale(scale, &m);
    unsigned int vertexCount = data->vertexCount;
    shapeMeshData->vertexData = new float[vertexCount * 3];
    Vector3 v;
    int vertexStride = data->vertexFormat.getVertexSize();
    for (unsigned int i = 0; i < data->vertexCount; i++)
    {
        v.set(*((float*)&data->vertexData[i * vertexStride + 0 * sizeof(float)]),
              *((float*)&data->vertexData[i * vertexStride + 1 * sizeof(float)]),
              *((float*)&data->vertexData[i * vertexStride + 2 * sizeof(float)]));
        v *= m;
        memcpy(&(shapeMeshData->vertexData[i * 3]), &v, sizeof(float) * 3);
    }

    btTriangleIndexVertexArray* meshInterface = bullet_new<btTriangleIndexVertexArray>();

    unsigned int partCount = data->parts.size();
    if (partCount > 0)
    {
        PHY_ScalarType indexType = PHY_UCHAR;
        int indexStride = 0;
        Bundle::MeshPartData* meshPart = NULL;
        for (unsigned int i = 0; i < partCount; i++)
        {
            meshPart = data->parts[i];
            GP_ASSERT(meshPart);

            switch (meshPart->indexFormat)
            {
            case Mesh::INDEX8:
                indexType = PHY_UCHAR;
                indexStride = 1;
                break;
            case Mesh::INDEX16:
                indexType = PHY_SHORT;
                indexStride = 2;
                break;
            case Mesh::INDEX32:
                indexType = PHY_INTEGER;
                indexStride = 4;
                break;
            default:
                GP_ERROR("Unsupported index format (%d).", meshPart->indexFormat);
                SAFE_DELETE(meshInterface);
                SAFE_DELETE_ARRAY(shapeMeshData->vertexData);
                SAFE_DELETE(shapeMeshData);
                SAFE_DELETE(data);
                return NULL;
            }

            // Move the index data into the rigid body's local buffer.
            // Set it to NULL in the MeshPartData so it is not released when the data is freed.
            shapeMeshData->indexData.push_back(meshPart->indexData);
            meshPart->indexData = NULL;

            // Create a btIndexedMesh object for the current mesh part.
            btIndexedMesh indexedMesh;
            indexedMesh.m_indexType = indexType;
            indexedMesh.m_numTriangles = meshPart->indexCount / 3; // assume TRIANGLES primitive type
            indexedMesh.m_numVertices = meshPart->indexCount;
            indexedMesh.m_triangleIndexBase = (const unsigned char*)shapeMeshData->indexData[i];
            indexedMesh.m_triangleIndexStride = indexStride*3;
            indexedMesh.m_vertexBase = (const unsigned char*)shapeMeshData->vertexData;
            indexedMesh.m_vertexStride = sizeof(float)*3;
            indexedMesh.m_vertexType = PHY_FLOAT;

            // Add the indexed mesh data to the mesh interface.
            meshInterface->addIndexedMesh(indexedMesh, indexType);
        }
    }
    else
    {
        // Generate index data for the mesh locally in the rigid body.
        unsigned int* indexData = new unsigned int[data->vertexCount];
        for (unsigned int i = 0; i < data->vertexCount; i++)
        {
            indexData[i] = i;
        }
        shapeMeshData->indexData.push_back((unsigned char*)indexData);

        // Create a single btIndexedMesh object for the mesh interface.
        btIndexedMesh indexedMesh;
        indexedMesh.m_indexType = PHY_INTEGER;
        indexedMesh.m_numTriangles = data->vertexCount / 3; // assume TRIANGLES primitive type
        indexedMesh.m_numVertices = data->vertexCount;
        indexedMesh.m_triangleIndexBase = shapeMeshData->indexData[0];
        indexedMesh.m_triangleIndexStride = sizeof(unsigned int);
        indexedMesh.m_vertexBase = (const unsigned char*)shapeMeshData->vertexData;
        indexedMesh.m_vertexStride = sizeof(float)*3;
        indexedMesh.m_vertexType = PHY_FLOAT;

        // Set the data in the mesh interface.
        meshInterface->addIndexedMesh(indexedMesh, indexedMesh.m_indexType);
    }

    // Create our collision shape object and store shapeMeshData in it.
    PhysicsCollisionShape* shape = new PhysicsCollisionShape(PhysicsCollisionShape::SHAPE_MESH, bullet_new<btBvhTriangleMeshShape>(meshInterface, true));
    shape->_shapeData.meshData = shapeMeshData;

    _shapes.push_back(shape);

    // Free the temporary mesh data now that it's stored in physics system.
    SAFE_DELETE(data);

    return shape;
}

void PhysicsController::destroyShape(PhysicsCollisionShape* shape)
{
    if (shape)
    {
        if (shape->getRefCount() == 1)
        {
            // Remove shape from shape cache.
            std::vector<PhysicsCollisionShape*>::iterator shapeItr = std::find(_shapes.begin(), _shapes.end(), shape);
            if (shapeItr != _shapes.end())
                _shapes.erase(shapeItr);
        }

        // Release the shape.
        shape->release();
    }
}

float PhysicsController::calculateHeight(float* data, unsigned int width, unsigned int height, float x, float y,
    const Matrix* worldMatrix, Vector3* normalData, Vector3* normalResult)
{
    GP_ASSERT(data);

    unsigned int x1 = x;
    unsigned int y1 = y;
    unsigned int x2 = x1 + 1;
    unsigned int y2 = y1 + 1;
    float tmp;
    float xFactor = x2 >= width ? 0.0f : modf(x, &tmp);
    float yFactor = y2 >= height ? 0.0f : modf(y, &tmp);
    float xFactorI = 1.0f - xFactor;
    float yFactorI = 1.0f - yFactor;

    float a = xFactorI * yFactorI;
    float b = xFactorI * yFactor;
    float c = xFactor * yFactor;
    float d = xFactor * yFactorI;

    if (normalResult)
    {
        normalResult->set(normalData[x1 + y1 * width] * a + normalData[x1 + y2 * width] * b + 
            normalData[x2 + y2 * width] * c + normalData[x2 + y1 * width] * d);
        normalResult->normalize();
        worldMatrix->transformVector(normalResult);
    }
    return data[x1 + y1 * width] * a + data[x1 + y2 * width] * b + 
        data[x2 + y2 * width] * c + data[x2 + y1 * width] * d;
}

void PhysicsController::addConstraint(PhysicsRigidBody* a, PhysicsRigidBody* b, PhysicsConstraint* constraint)
{
    GP_ASSERT(a);
    GP_ASSERT(constraint);
    GP_ASSERT(_world);

    a->addConstraint(constraint);
    if (b)
    {
        b->addConstraint(constraint);
    }
    
    _world->addConstraint(constraint->_constraint);
}

bool PhysicsController::checkConstraintRigidBodies(PhysicsRigidBody* a, PhysicsRigidBody* b)
{
    GP_ASSERT(a);

    if (!a->supportsConstraints())
    {
        GP_ASSERT(a->_node);
        GP_ERROR("Rigid body '%s' does not support constraints; unexpected behavior may occur.", a->_node->getId());
        return false;
    }
    
    if (b && !b->supportsConstraints())
    {
        GP_ASSERT(b->_node);
        GP_ERROR("Rigid body '%s' does not support constraints; unexpected behavior may occur.", b->_node->getId());
        return false;
    }

    return true;
}

void PhysicsController::removeConstraint(PhysicsConstraint* constraint)
{
    GP_ASSERT(constraint);
    GP_ASSERT(_world);

    // Find the constraint and remove it from the physics world.
    for (int i = _world->getNumConstraints() - 1; i >= 0; i--)
    {
        btTypedConstraint* currentConstraint = _world->getConstraint(i);
        if (constraint->_constraint == currentConstraint)
        {
            _world->removeConstraint(currentConstraint);
            break;
        }
    }
}

PhysicsController::DebugDrawer::DebugDrawer()
    : _mode(btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawConstraintLimits | btIDebugDraw::DBG_DrawConstraints | 
       btIDebugDraw::DBG_DrawContactPoints | btIDebugDraw::DBG_DrawWireframe), _viewProjection(NULL), _meshBatch(NULL)
{
    // Vertex shader for drawing colored lines.
    const char* vs_str = 
    {
        "uniform mat4 u_viewProjectionMatrix;\n"
        "attribute vec4 a_position;\n"
        "attribute vec4 a_color;\n"
        "varying vec4 v_color;\n"
        "void main(void) {\n"
        "    v_color = a_color;\n"
        "    gl_Position = u_viewProjectionMatrix * a_position;\n"
        "}"
    };

    // Fragment shader for drawing colored lines.
    const char* fs_str = 
    {
    #ifdef OPENGL_ES
        "precision highp float;\n"
    #endif
        "varying vec4 v_color;\n"
        "void main(void) {\n"
        "   gl_FragColor = v_color;\n"
        "}"
    };

    Effect* effect = Effect::createFromSource(vs_str, fs_str);
    Material* material = Material::create(effect);
    GP_ASSERT(material && material->getStateBlock());
    material->getStateBlock()->setDepthTest(true);

    VertexFormat::Element elements[] =
    {
        VertexFormat::Element(VertexFormat::POSITION, 3),
        VertexFormat::Element(VertexFormat::COLOR, 4),
    };
    _meshBatch = MeshBatch::create(VertexFormat(elements, 2), Mesh::LINES, material, false);

    SAFE_RELEASE(material);
    SAFE_RELEASE(effect);
}

PhysicsController::DebugDrawer::~DebugDrawer()
{
    SAFE_DELETE(_meshBatch);
}

void PhysicsController::DebugDrawer::begin(const Matrix& viewProjection)
{
    GP_ASSERT(_meshBatch);
    _viewProjection = &viewProjection;
    _meshBatch->begin();
}

void PhysicsController::DebugDrawer::end()
{
    GP_ASSERT(_meshBatch && _meshBatch->getMaterial() && _meshBatch->getMaterial()->getParameter("u_viewProjectionMatrix"));
    _meshBatch->finish();
    _meshBatch->getMaterial()->getParameter("u_viewProjectionMatrix")->setValue(_viewProjection);
    _meshBatch->draw();
}

void PhysicsController::DebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
{
    GP_ASSERT(_meshBatch);

    static DebugDrawer::DebugVertex fromVertex, toVertex;

    fromVertex.x = from.getX();
    fromVertex.y = from.getY();
    fromVertex.z = from.getZ();
    fromVertex.r = fromColor.getX();
    fromVertex.g = fromColor.getY();
    fromVertex.b = fromColor.getZ();
    fromVertex.a = 1.0f;

    toVertex.x = to.getX();
    toVertex.y = to.getY();
    toVertex.z = to.getZ();
    toVertex.r = toColor.getX();
    toVertex.g = toColor.getY();
    toVertex.b = toColor.getZ();
    toVertex.a = 1.0f;

    _meshBatch->add(&fromVertex, 1);
    _meshBatch->add(&toVertex, 1);
}

void PhysicsController::DebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
    drawLine(from, to, color, color);
}

void PhysicsController::DebugDrawer::drawContactPoint(const btVector3& pointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color)
{
    drawLine(pointOnB, pointOnB + normalOnB, color);
}

void PhysicsController::DebugDrawer::reportErrorWarning(const char* warningString)
{
    GP_WARN(warningString);
}

void PhysicsController::DebugDrawer::draw3dText(const btVector3& location, const char* textString)
{
    GP_WARN("Physics debug drawing: 3D text is not supported.");
}

void PhysicsController::DebugDrawer::setDebugMode(int mode)
{
    _mode = mode;
}

int PhysicsController::DebugDrawer::getDebugMode() const
{
    return _mode;
}

PhysicsController::Listener::~Listener()
{
    GP_ASSERT(Game::getInstance()->getPhysicsController());
    Game::getInstance()->getPhysicsController()->removeStatusListener(this);
}

PhysicsController::HitFilter::HitFilter()
{
}

PhysicsController::HitFilter::~HitFilter()
{
}

bool PhysicsController::HitFilter::filter(PhysicsCollisionObject* object)
{
    return false;
}

bool PhysicsController::HitFilter::hit(const PhysicsController::HitResult& result)
{
    return true;
}

}