vrsode/src/PhysicsManager.cpp

Go to the documentation of this file.

#include "random_utils/LogManager.h"
#include "random_utils/ThreadSafeCanvas.h"

#include "PhysicsManager.h"
#include "PhysicsBody.h"

namespace vrsode
{

PhysicsManager* PhysicsManager::m_instance = 0;
bool PhysicsManager::m_keepRunning = true;
double PhysicsManager::m_stepSize = 0.02;
double PhysicsManager::m_stepTime = 0.004;
unsigned int PhysicsManager::m_updateResolution = 1;
bool PhysicsManager::m_runningMultiThreaded = false;
SDL_Thread* PhysicsManager::m_thread = 0;

const unsigned int g_initialCollisionCategory = 0xFFFF0000;
const unsigned int g_initialCollisionPartnerCategories = 0xFFFF0000;

static void
NearCallback(void* p_data, dGeomID p_geomOne, dGeomID p_geomTwo);

PhysicsManager::PhysicsManager()
{
    m_initialised           = false;
    m_performanceAutoAdjust = false;
    m_maxCollisionContacts  = 16;
    init();
}

PhysicsManager::~PhysicsManager()
{
    if(m_initialised)
        release();
}

void
PhysicsManager::registerCollisionBody(VRS::SO<CollisionBody> p_object)
{
    m_collisionBodies.push_back(p_object);

    p_object->setCollisionCategory(g_initialCollisionCategory);
    p_object->setCollisionPartnerCategories(
            g_initialCollisionPartnerCategories);
}

void
PhysicsManager::registerPhysicsBody(VRS::SO<PhysicsBody> p_object)
{
    m_physicsBodies.push_back(p_object);

    p_object->setCollisionCategory(g_initialCollisionCategory);
    p_object->setCollisionPartnerCategories(
            g_initialCollisionPartnerCategories);
}

void
PhysicsManager::unregisterCollisionBody(VRS::SO<CollisionBody> p_object)
{
    std::vector<VRS::SO<CollisionBody> >::iterator it;
    it = findCollisionBody(p_object);

    if(it != m_collisionBodies.end())
        m_collisionBodies.erase(it);

    if(p_object->geomId())
        dSpaceRemove(spaceId(), p_object->geomId());
}

void
PhysicsManager::unregisterPhysicsBody(VRS::SO<PhysicsBody> p_object)
{
    std::vector<VRS::SO<PhysicsBody> >::iterator it;
    it = findPhysicsBody(p_object);

    if(it != m_physicsBodies.end())
        m_physicsBodies.erase(it);

    if(p_object->geomId())
        dSpaceRemove(spaceId(), p_object->geomId());
}

const bool
PhysicsManager::isInitialised() const
{
    return m_initialised;
}

const dSpaceID
PhysicsManager::spaceId() const
{
    return m_spaceId;
}

const dWorldID
PhysicsManager::worldId() const
{
    return m_worldId;
}

const unsigned int
PhysicsManager::maxCollisionContacts() const
{
    return m_maxCollisionContacts;
}

void
PhysicsManager::setMaxCollisionContacts(unsigned int p_maxCollisionContacts)
{
    m_maxCollisionContacts = p_maxCollisionContacts;
}

const dJointGroupID
PhysicsManager::collisionContactGroupId() const
{
    return m_collisionContactGroupId;
}

const std::vector<VRS::SO<PhysicsBody> >&
PhysicsManager::physicsBodies() const
{
    return m_physicsBodies;
}

const std::vector<VRS::SO<CollisionBody> >&
PhysicsManager::collisionBodies() const
{
    return m_collisionBodies;
}

unsigned int
PhysicsManager::physicsBodyCount() const
{
    return m_physicsBodies.size();
}

unsigned int
PhysicsManager::collisionBodyCount() const
{
    return m_collisionBodies.size();
}

void
PhysicsManager::init()
{
    m_worldId = dWorldCreate();
    m_spaceId = dHashSpaceCreate(0);
//     m_spaceId = dSimpleSpaceCreate(0);
    m_collisionContactGroupId = dJointGroupCreate(0);

    // TODO: change those values via accessor function
//     dWorldSetERP(m_worldId, 0.2);
    dWorldSetERP(m_worldId, 0.9);
    dWorldSetCFM(m_worldId, 0.0001);
    dWorldSetGravity(m_worldId, 0.0, -9.81, 0.0);
    dWorldSetAutoDisableFlag(m_worldId, true);
    dWorldSetAutoDisableLinearThreshold(m_worldId, 0.01);
    dWorldSetAutoDisableAngularThreshold(m_worldId, 0.01);
    dWorldSetAutoDisableSteps(m_worldId, 10);
    dWorldSetAutoDisableTime(m_worldId, 0);
    dWorldSetContactMaxCorrectingVel(m_worldId, dInfinity);
    dWorldSetContactSurfaceLayer(m_worldId, 0.001);

    m_initialised   = true;
}

void
PhysicsManager::tick(double p_stepSize)
{
    if(!isInitialised())
        return;
    {
        std::vector<VRS::SO<CollisionBody> >::iterator it;
        for(it = m_collisionBodies.begin(); it != m_collisionBodies.end(); it++)
            (*it)->update();
    }

    {
        std::vector<VRS::SO<PhysicsBody> >::iterator it;
        for(it = m_physicsBodies.begin(); it != m_physicsBodies.end(); it++)
            (*it)->update();
    }

    dSpaceCollide(m_spaceId, 0, &NearCallback);
    dWorldStep(m_worldId, p_stepSize);
    dJointGroupEmpty(m_collisionContactGroupId);
}

void
PhysicsManager::update()
{
    lock();
    tick(stepSize() / (double)updateResolution());
    unlock();
}

void
PhysicsManager::update(double p_stepSize)
{
    for(unsigned int i = 0; i < updateResolution(); i++)
        tick(p_stepSize / (double)updateResolution());
    
    applyTransformations();
}

void
PhysicsManager::release()
{
    if(!isInitialised())
        return;

    dJointGroupEmpty(m_collisionContactGroupId);
    dJointGroupDestroy(m_collisionContactGroupId);
    dWorldDestroy(m_worldId);
    dSpaceDestroy(m_spaceId);
    dCloseODE();

    m_initialised = false;
}

void
PhysicsManager::applyTransformations()
{
    std::vector<VRS::SO<PhysicsBody> >::iterator it;
    for(it = m_physicsBodies.begin(); it != m_physicsBodies.end(); it++)
        (*it)->syncTransformationsToVRS();
}

std::vector<VRS::SO<PhysicsBody> >::iterator
PhysicsManager::findPhysicsBody(VRS::SO<PhysicsBody> p_physicsBody)
{
    std::vector<VRS::SO<PhysicsBody> >::iterator it;

    for(it = m_physicsBodies.begin(); it != m_physicsBodies.end(); it++)
        if(*it == p_physicsBody)
            return it;

    return m_physicsBodies.end();
}

std::vector<VRS::SO<CollisionBody> >::iterator
PhysicsManager::findCollisionBody(VRS::SO<CollisionBody> p_collisionBody)
{
    std::vector<VRS::SO<CollisionBody> >::iterator it;

    for(it = m_collisionBodies.begin(); it != m_collisionBodies.end(); it++)
        if(*it == p_collisionBody)
            return it;

    return m_collisionBodies.end();
}

void
PhysicsManager::setDebugNodesGlobal(bool p_debugNodes)
{
    std::vector<VRS::SO<PhysicsBody> >::iterator it1;
    std::vector<VRS::SO<CollisionBody> >::iterator it2;

    for(it1 = m_physicsBodies.begin(); it1 != m_physicsBodies.end(); it1++)
        (*it1)->setDebugNode(p_debugNodes);
    for(it2 = m_collisionBodies.begin(); it2 != m_collisionBodies.end(); it2++)
        (*it2)->setDebugNode(p_debugNodes);
}

static void
NearCallback(void* p_data, dGeomID p_geomOne, dGeomID p_geomTwo)
{
    unsigned int maxContacts = PhysicsManager::get()->maxCollisionContacts();

    if(dGeomGetBody(p_geomOne) && dGeomGetBody(p_geomTwo))
        if(dAreConnected(dGeomGetBody(p_geomOne), dGeomGetBody(p_geomTwo)))
            return;

    if (dGeomIsSpace(p_geomOne) || dGeomIsSpace(p_geomTwo))
    {
        dSpaceCollide2(p_geomOne, p_geomTwo, p_data, &NearCallback);

        if(dGeomIsSpace(p_geomOne))
            dSpaceCollide((dSpaceID)p_geomOne, p_data, &NearCallback);

        if(dGeomIsSpace(p_geomTwo))
            dSpaceCollide((dSpaceID)p_geomTwo, p_data, &NearCallback);

        return;
    }

    VRS::SO<CollisionBody> bodyOne = (CollisionBody*)dGeomGetData(p_geomOne);
    VRS::SO<CollisionBody> bodyTwo = (CollisionBody*)dGeomGetData(p_geomTwo);
    VRS::SO<Surface> surfaceOne = bodyOne ? bodyOne->surface() : 0;
    VRS::SO<Surface> surfaceTwo = bodyTwo ? bodyTwo->surface() : 0;

    // check collision categories
    unsigned long bodyOneCategory = bodyOne->collisionCategory();
    unsigned long bodyOnePartners = bodyOne->collisionPartnerCategories();
    unsigned long bodyTwoCategory = bodyTwo->collisionCategory();
    unsigned long bodyTwoPartners = bodyTwo->collisionPartnerCategories();

    // collide if both want to collide
    if((bodyOneCategory&bodyTwoPartners) && (bodyTwoCategory&bodyOnePartners))
    {
        dContact     contacts[maxContacts];
        unsigned int numberOfContacts =
                dCollide(p_geomOne, p_geomTwo, maxContacts,
                &(contacts[0].geom), sizeof(dContact));

        for (unsigned int i = 0; i < numberOfContacts; i++)
        {
            contacts[i].surface.mode =
                    dContactSoftERP | dContactSoftCFM | dContactBounce |
                    dContactApprox1 | dContactSlip1   | dContactSlip2;

            // if we do not have surface information, use default values
            if(!surfaceOne || !surfaceTwo)
            {
                rErr("no surface, using hardcoded default values...");
                contacts[i].surface.slip1       = 0.1;
                contacts[i].surface.slip2       = 0.1;
                contacts[i].surface.mu          = dInfinity;
                contacts[i].surface.bounce      = 0.0;
                contacts[i].surface.bounce_vel  = dInfinity;
                contacts[i].surface.soft_erp    = 0.99;
                contacts[i].surface.soft_cfm    = 0.02;
            }
            // if we have surfaces and no ghost, set values
            else if(!surfaceOne->ghostMode() && !surfaceTwo->ghostMode())
            {
                // the bigger slip value is the one we are using
                contacts[i].surface.slip1 =
                        fmax(surfaceOne->slip1(), surfaceTwo->slip1());

                // the bigger slip value is the one we are using
                contacts[i].surface.slip2 =
                        fmax(surfaceOne->slip2(), surfaceTwo->slip2());

                // the bigger slip value is the one we are using
                contacts[i].surface.mu =
                        fmax(surfaceOne->mu(), surfaceTwo->mu());

                // the bigger bounce value will be used
                contacts[i].surface.bounce =
                        fmax(surfaceOne->bounce(), surfaceTwo->bounce());

                // the smaller bounce velocity will be used
                contacts[i].surface.bounce =
                        fmin(surfaceOne->bounceVelocity(),
                            surfaceTwo->bounceVelocity());

                // use average erp value
                contacts[i].surface.soft_erp =
                        (surfaceOne->softErp() + surfaceTwo->softErp()) / 2.0;

                // use average cfm value
                contacts[i].surface.soft_cfm =
                        surfaceOne->softCfm() + surfaceTwo->softCfm() / 2.0;
            }

            // if we have no ghosts, create contact joint
            if(!surfaceOne->ghostMode() && !surfaceTwo->ghostMode())
            {
                dJointID contact = dJointCreateContact(
                        PhysicsManager::get()->worldId(),
                        PhysicsManager::get()->collisionContactGroupId(),
                        &contacts[i]);

                dJointAttach(contact, dGeomGetBody(contacts[i].geom.g1),
                        dGeomGetBody(contacts[i].geom.g2));
            }
        }

        // inform collision partners
        if(bodyOne && bodyTwo)
        {
            bodyOne->handleCollision(bodyTwo);
            bodyTwo->handleCollision(bodyOne);
        }
    }
}

void
PhysicsManager::setStepSize(double p_value)
{
    m_stepSize = p_value;
}

double
PhysicsManager::stepSize()
{
    return m_stepSize;
}

void
PhysicsManager::setStepTime(double p_value)
{
    m_stepTime = p_value;
}

double
PhysicsManager::stepTime()
{
    return m_stepTime;
}

bool
PhysicsManager::keepRunning()
{
    return m_keepRunning;
}

void
PhysicsManager::stop()
{
    m_keepRunning = false;
}

int
PhysicsManager::updateWorker(void* p_data)
{
    // never waste a void* :P
    PhysicsManager* physicsManager = (PhysicsManager*)p_data;

    const int performanceAdjustmentThreshold = 25;
    int performanceMeter = 0;
    double timeTemp;
    double updateTime;

    while(physicsManager->keepRunning())
    {
        timeTemp = physicsManager->currentTime();
        for(unsigned int i = 0; i < m_updateResolution; i++)
            physicsManager->update();

        if(m_runningMultiThreaded)
            random_utils::ThreadSafeCanvas::get()->sleep();

        physicsManager->applyTransformations();

        if(m_runningMultiThreaded)
            random_utils::ThreadSafeCanvas::get()->wakeUp();

        updateTime = physicsManager->currentTime() - timeTemp;

        if(physicsManager->performanceAutoAdjust())
        {
            // if we needed too much time for the update
            if(updateTime > physicsManager->stepTime())
                performanceMeter--;

            // if we needed less than one fifth of the time for the update
            if((updateTime * 5.0) < physicsManager->stepTime())
                performanceMeter++;

            // if the performance was good several times
            if(performanceMeter > performanceAdjustmentThreshold)
            {
                // reset the performance meter
                performanceMeter = 0;

                // update the update resolution
                physicsManager->setUpdateResolution(
                        (physicsManager->updateResolution()) + 1);

                std::cout << "+++ " << "increasing update resolution, now: "
                        << physicsManager->updateResolution() << std::endl;
            }

            // if the performance was bad several times
            if(performanceMeter < -performanceAdjustmentThreshold)
            {
                // reset the performance meter
                performanceMeter = 0;

                // update the update resolution
                if(physicsManager->updateResolution() > 1)
                {
                    physicsManager->setUpdateResolution(
                            (physicsManager->updateResolution()) - 1);
                    std::cout << "--- " << "decreasing update resolution, now: "
                            << physicsManager->updateResolution() << std::endl;
                }
                else
                    std::cout << "!!! BAD PERFORMANCE !!!" << std::endl
                            << "no more real time performance tweaking possible"
                            << std::endl;

            }
        }
        else if(physicsManager->stepTime() - updateTime > 0)
            SDL_Delay((unsigned long)((physicsManager->stepTime() -
                    updateTime) * 1000.0));
    }

    return 0;
}

double
PhysicsManager::currentTime()
{
    return SDL_GetTicks() / 1000.0;
}

void
PhysicsManager::setPerformanceAutoAdjust(bool p_value)
{
    m_performanceAutoAdjust = p_value;
}

void
PhysicsManager::setUpdateSpeed(double p_updateSpeed)
{
    double currentSpeed = updateSpeed();

    m_stepSize *= (p_updateSpeed / currentSpeed);
}

double
PhysicsManager::updateSpeed()
{
    return m_stepSize / m_stepTime;
}

void
PhysicsManager::setUpdateResolution(unsigned int p_updateResolution)
{
    m_updateResolution = p_updateResolution;
}

unsigned int
PhysicsManager::updateResolution()
{
    return m_updateResolution;
}

bool
PhysicsManager::performanceAutoAdjust()
{
    return m_performanceAutoAdjust;
}

bool
PhysicsManager::runningMultiThreaded()
{
    return m_runningMultiThreaded;
}

std::string
PhysicsManager::odeVersionString()
{
    return std::string(PACKAGE_STRING);
}

}

---