Physics collision callbacks

/// <summary>
/// Collision record structure
///
/// Holds single collision information. It should be stored in associative container, where key
/// is collision object B. Together one key-value pair forms single manifold.
/// </summary>
struct CollisionRecord
{
    /// <summary>Collision object A that is collided with</summary>
    btCollisionObject* mObjectA;

    /// <summary>Contact point on A</summary>
    btVector3 mPointA;

    /// <summary>Contact point on B</summary>
    btVector3 mPointB;

    /// <summary>Collision normal on B (Note: on A it is flipped, i.e. -mNormalOnB)</summary>
    btVector3 mNormalOnB;

    /// <summary>
    /// Default constructor
    /// </summary>
    CollisionRecord()
    {
        mObjectA = nullptr;
    }

    /// <summary>
    /// Parametric constructor
    /// </summary>
    /// <param name="objectA">Collision object with which this contact point collides</param>
    /// <param name="pointA">Contact point on A</param>
    /// <param name="pointB">Contact point on B</param>
    /// <param name="normalOnB">Collision normal on B</param>
    CollisionRecord(btCollisionObject* objectA, const btVector3& pointA, const btVector3& pointB, const btVector3& normalOnB)
    {
        mObjectA = objectA;
        mPointA = pointA;
        mPointB = pointB;
        mNormalOnB = normalOnB;
    }
};

/// <summary>
/// List of contact points from previous simulation step
/// </summary>
std::map<btCollisionObject*, CollisionRecord> mContacts;

/// <summary>
/// Check collision and call callbacks on game obejcts
///
/// Update contacts - fire events for on collision enter/on collision exit
/// </summary>
void CheckCollisions()
{
    ////////////////////////////////////////////////////////////////////////////////////////////////
    // The first step is to obtain all contact points in this step

    // Associative container holding new contacts from current manifolds
    std::map<btCollisionObject*, CollisionRecord> newContacts;

    // Loop through ALL collision pairs
    int numManifolds = mWorld->getDispatcher()->getNumManifolds();
    for (int i = 0; i < numManifolds; i++)
    {
        btPersistentManifold* contactManifold = mWorld->getDispatcher()->getManifoldByIndexInternal(i);
        btCollisionObject* colliderA = (btCollisionObject*)contactManifold->getBody0();
        btCollisionObject* colliderB = (btCollisionObject*)contactManifold->getBody1();

        // Loop through ALL contact points in collision pairs
        int numContacts = contactManifold->getNumContacts();
        for (int j = 0; j < numContacts; j++)
        {
            // Check if contact distance is < 0 (i.e. objects penetrate), if so and there isn't a contact
            // in the current list of contacts - add it.
            btManifoldPoint& pt = contactManifold->getContactPoint(j);
            if (pt.getDistance() < 0.0f)
            {
                const btVector3 ptA = pt.getPositionWorldOnA();
                const btVector3 ptB = pt.getPositionWorldOnB();
                const btVector3 normalOnB = pt.m_normalWorldOnB;

                if (newContacts.find(colliderB) == newContacts.end())
                {
                    newContacts[colliderB] = CollisionRecord(colliderA, ptA, ptB, normalOnB);
                }
            }
        }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////
    // The second step is to check for newly added contacts

    if (!newContacts.empty())
    {
        // Loop through all new contact points and compare them to previous step contact points, the
        // ones that are not present in previous step but are now, are new collisions
        for (std::map<btCollisionObject*, CollisionRecord>::iterator it = newContacts.begin(); it != newContacts.end(); it++)
        {
            // Not found
            if (mContacts.find(it->first) == mContacts.end())
            {
                // Loop through game object components and call CollisionEnter on each of those
                Entity* entityB = (Entity*)(it->first->getCollisionShape()->getUserPointer());

                for (auto componentIt = entityB->GameObject().ComponentsItBegin();
                    componentIt != entityB->GameObject().ComponentsItEnd();
                    componentIt++)
                {
                    Entity* entityA = (Entity*)(it->second.mObjectA->getCollisionShape()->getUserPointer());

                    componentIt->second->CollisionEnter(&(entityA->GameObject()));
                    componentIt->second->CollisionEnter(&(entityB->GameObject()));
                }
            }
            // Found - remove to filter inactive contacts
            else
            {
                mContacts.erase(it->first);
            }
        }
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////
    // The last step is to check for removed contacts

    if (!mContacts.empty())
    {
        // Loop through all existing contacts
        for (std::map<btCollisionObject*, CollisionRecord>::iterator it = newContacts.begin(); it != newContacts.end(); it++)
        {
            // For each collision that ends, fire CollisionExit
            // Loop through game object components and call CollisionEnter on each of those
            Entity* entityB = (Entity*)(it->first->getCollisionShape()->getUserPointer());

            for (auto componentIt = entityB->GameObject().ComponentsItBegin();
                componentIt != entityB->GameObject().ComponentsItEnd();
                componentIt++)
            {
                Entity* entityA = (Entity*)(it->second.mObjectA->getCollisionShape()->getUserPointer());

                componentIt->second->CollisionExit(&(entityA->GameObject()));
                componentIt->second->CollisionExit(&(entityB->GameObject()));
            }
        }
    }

    // Update previous contacts to the ones in current step
    mContacts = newContacts;
}