World.cpp

Go to the documentation of this file.

//-----------------------------------------------------------------------------------
//
// Pro-Vocation Light Engine (PVLE)
// Copyright (C) 2007-2009  Sukender, KinoX & Buzib
// For more information, contact us : sukender@free.fr
//
// 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.
//
// For any use that is not compatible with the terms of the GNU 
// General Public License, please contact the authors for alternative
// licensing options.
//
// 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, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//-----------------------------------------------------------------------------------

#include <PVLE/Physics/World.h>
#include <PVLE/Physics/Space.h>
#include <PVLE/Physics/Geom.h>
#include <PVLE/Physics/Joint.h>
#include <PVLE/Physics/Body.h>
#include <PVLE/Physics/Contact.h>
#include <boost/cast.hpp>
#include <vector>
#include <osg/Notify>
#include <PVLE/Util/Util.h>

#include <PVLE/Entity/GeomCollisionContainer.h>

#define DISABLE_NULL_NORMAL_CONTACT_CHECK           
//#define DISABLE_CLOSE_CONTACT_CHECK               


namespace Physics {

void NearCallback::toPVLENearCallback(void * data, dGeomID o1, dGeomID o2) {
    World *This = static_cast<World *>(data);
    ASSERT(This);

    NearCallback defaultCB;
    NearCallback * pBB = This->getNearCallback();
    if (!pBB) pBB = &defaultCB;
        
    pBB->operator()(This, static_cast<AbstractGeom *>(dGeomGetData(o1)), static_cast<AbstractGeom *>(dGeomGetData(o2)));
}

void NearCallback::operator()(World * pWorld, AbstractGeom * pG1, AbstractGeom * pG2) {
    DBG_TRY_BLOCK_START

    if (pG1->isSpace() || pG2->isSpace()) {
        // Colliding a space with something
        //pWorld->spaceCollide2(boost::polymorphic_downcast<Space*>(pG1), boost::polymorphic_downcast<Space*>(pG2));
        pWorld->spaceCollide2(pG1, pG2);
        // Collide all geoms internal to the space(s)
        if (pG1->isSpace()) pWorld->spaceCollide(boost::polymorphic_downcast<Space*>(pG1));
        if (pG2->isSpace()) pWorld->spaceCollide(boost::polymorphic_downcast<Space*>(pG2));
    } else {
        // Colliding two non-space geoms, so generate contact points between o1 and o2
        Geom * pCastG1 = boost::polymorphic_downcast<Geom*>(pG1);
        Geom * pCastG2 = boost::polymorphic_downcast<Geom*>(pG2);
        Body * pB1 = pCastG1->getBody();
        Body * pB2 = pCastG2->getBody();

        // Exit without doing anything if there are 2 bodies and if the two bodies are connected by a joint that is not a contact (and if an option does not disable this "cutout").
        if (pB1 && pB2 &&
            !(pB1->getAllowCollideWithJointBodies() || pB1->getAllowCollideWithJointBodies()) &&
            pB1->isConnectedToExcludingContact(pB2)) return;

        // Collide and set contacts
        const UINT nbMaxContacts = 10;
        static Contact pContacts[nbMaxContacts];
        UINT nbContacts = pCastG1->collide(pCastG2, nbMaxContacts, pContacts);
        UINT numContact;        // Loop variable

        if (nbContacts) {
#ifdef _DEBUG
            static bool writtenOnce = false;
            if (!writtenOnce && nbContacts == nbMaxContacts) { osg::notify(osg::DEBUG_INFO) << "Max contacts reached (Message displayed only once)" << std::endl; writtenOnce = true; }
#endif

            // Try to improve the simulation by removing some contacts
            std::vector<osg::Vec3>::const_iterator it, itEnd;
            osg::Vec3 tempPos;
            bool deleted = false;
            for(numContact=0; numContact<nbContacts; ) {
                deleted = false;

#ifndef DISABLE_NULL_NORMAL_CONTACT_CHECK
                // Testing code : removing those with a null normal (seems useless since ODE 0.7)
                if (!osg::equivalent(pContacts[numContact].getNormal().length2(), 1)) {
                    pContacts[numContact] = pContacts[nbContacts-1];        // Does nothing special if [numContact] == [nbContacts-1]
                    --nbContacts;
                    deleted = true;
                    continue;       // Avoids an "else" statement and a #ifndef for the closing parenthesis :)
                }
#endif
#ifndef DISABLE_CLOSE_CONTACT_CHECK
                // Filter contacts points that are too close (for each body)
                tempPos = pContacts[numContact].getPos();
                // 1. Filter close points in pContacts (= duplicates generated by collision system)
                for(UINT i=numContact+1; i<nbContacts; ++i) {
                    // Note : deletes [i] and not [numContact] - can delete multiple contacts at once (no break statement because it deletes [i]).
                    if ((pContacts[i].getPos() - tempPos).length() < 1e-5f) { pContacts[i] = pContacts[nbContacts-1]; --nbContacts; }
                }

                // 2. Filter other close points (for composites, etc.)
                if (pB1) {
                    BOOST_FOREACH(osg::Vec3 & pos, pB1->contactPos) {
                        if ((pos - tempPos).length() < 1e-5f) {
                            if (pContacts[numContact].geom.g2 == NULL) { pContacts[numContact] = pContacts[nbContacts-1]; --nbContacts; deleted = true; break; }
                            else pContacts[numContact].geom.g1 = NULL;
                        }
                    }
                    if (deleted) continue;
                }
                if (pB2) {
                    BOOST_FOREACH(osg::Vec3 & pos, pB2->contactPos) {
                        if ((pos - tempPos).length() < 1e-5f) {
                            if (pContacts[numContact].geom.g1 == NULL) { pContacts[numContact] = pContacts[nbContacts-1]; --nbContacts; deleted = true; break; }
                            else pContacts[numContact].geom.g2 = NULL;
                        }
                    }
                    //if (deleted) continue;
                }
#endif
                if (!deleted) ++numContact;
            }
        }
        if (nbContacts) {

            Contact* itContact = pContacts, * itEndContact = pContacts+nbContacts;

            // Data to find the most energized contact (genarally used to remove life points)
            float maxEnergy = 0;
            Contact * pMaxEnergyContact = pContacts;

            for(numContact=0; itContact!=itEndContact; ++itContact, ++numContact) {
                // Compute relative velocity / energy (if a geom has no body, then its velocity and its mass are considered to be 0)
                // Ec = 1/2 * m*v²
                osg::Vec3 relVel;       // Velocity compared to geom/body 2
                itContact->arithmeticCineticEnergy = 0;
                if (pB1) {
                    relVel = dBodyGetPointVelV(*pB1, itContact->getPos());      // Positive component of relative velocity
                    itContact->arithmeticCineticEnergy = pB1->getMassData().getMass();
                }
                if (pB2) {
                    relVel -= dBodyGetPointVelV(*pB2, itContact->getPos());     // Negative component
                    itContact->arithmeticCineticEnergy += pB2->getMassData().getMass();
                }
                double relNormalVelocity = relVel * itContact->getNormal();
                itContact->arithmeticCineticEnergy *= 0.5 * (relNormalVelocity*relNormalVelocity);
                if (relNormalVelocity>0) itContact->arithmeticCineticEnergy *= -1;      // (relVel * normal) is <0 if bodies move towards each other!

                if (itContact->arithmeticCineticEnergy > maxEnergy) { pMaxEnergyContact = itContact; maxEnergy = pMaxEnergyContact->arithmeticCineticEnergy; }
            }

            // Calls hitBefore() methods if necessary, and check if contacts have to be created (false is prioritary)
            bool createContact = pB1 || pB2;
            Body * pHitB1 = pB1;
            Body * pHitB2 = pB2;
            if (pCastG1->getCollisionContainer()) createContact &= pCastG1->getCollisionContainer()->hitBefore(pContacts, numContact, pMaxEnergyContact, *pCastG1, *pCastG2, pHitB1, pHitB2);
            if (pCastG2->getCollisionContainer()) createContact &= pCastG2->getCollisionContainer()->hitBefore(pContacts, numContact, pMaxEnergyContact, *pCastG2, *pCastG1, pHitB2, pHitB1);

            // Note : Rays do not need contacts
            if (createContact && pCastG1->getHandler()->type() != Physics::GeometryHandler::RAY) {
                for(itContact=pContacts; itContact!=itEndContact; ++itContact) {

                    // Setup contact by merging two surface parameters sets
                    itContact->getSurfaceParams().mergeInto(pCastG1->surface, pCastG2->surface);

                    /*
                    // Set friction dir (Friction dir must be normalized and perpendicular to the contact normal)
                    WHAT IF RelVel IS NEAR ZERO-LENGTH ?;
                    relVel.normalize();
                    toPhyVec3(relVel, itContact->fdir1);
                    */

                    // Add to contact group
                    Joint * pJ = pWorld->getContactGroup()->createAndAdd(pWorld, itContact);    // Add the contact point to the simulation
                    // Effectively attach bodies : do not call "pJ->attach(pB1, pB2);" because hitBefore() could have set a body to NULL in order to do a "one way" collision.
                    pJ->attach(pHitB1, pHitB2);

                    // ----- TEMPORARY CODE FOR DEBUG -----
                    if (pHitB1) pHitB1->contactPos.push_back(itContact->getPos());
                    if (pHitB2) pHitB2->contactPos.push_back(itContact->getPos());
                }
            }

            // Inform collision containers of the contact creation (or not)
            // WARNING : hitAfter() MUST be called after hitBefore(), whether the contact is created or not (see hitAfter() code doc).
            if (pCastG1->getCollisionContainer()) pCastG1->getCollisionContainer()->hitAfter(pContacts, numContact, pMaxEnergyContact, *pCastG1, *pCastG2, createContact);
            if (pCastG2->getCollisionContainer()) pCastG2->getCollisionContainer()->hitAfter(pContacts, numContact, pMaxEnergyContact, *pCastG2, *pCastG1, createContact);
        }
    }

    DBG_TRY_BLOCK_END
}


// --------------------------------------------------

World::World() : id(0), bUseQuick(true), pNcb(NULL) {
    pSpace = new Space();
    init();
}

World::World(const osg::Vec3 & worldBox) : id(0), bUseQuick(true), pNcb(NULL) {
    pSpace = new Space(worldBox);
    init();
}

World::World(Space * space) : id(0), bUseQuick(true), pNcb(NULL) {
    pSpace = space ? space : new Space();
    init();
}

void World::init() {
#if defined(_DEBUG) && defined(PVLE_UNIT_TESTS)
    Util::UnitTestSingleton::testPart2();
#endif
    pContactGroup = new JointGroup();

    id = dWorldCreate();

    // Default parameters
    dWorldSetContactSurfaceLayer(id, 0.001f);
    dWorldSetAutoDisableFlag(id, 1);        // Auto-disable "on" for newly created bodies
    //dWorldSetAutoDisableSteps(id, 15);
    dWorldSetGravity(id, osg::Vec3(0,0,-9.81f));

    ODEHolder::instance();          // Used to force the presence of ODE holder singleton (will close ODE on program exit)
}

World::~World() {
    if (pContactGroup.valid()) pContactGroup->clear();
    pContactGroup = NULL;
    pSpace = NULL;
    if (id) dWorldDestroy(id);
}

void World::step(dReal stepSize) {
    DBG_TRY_BLOCK_START

    // 0. Remove all joints in the contact joint group
    pContactGroup->clear();

    // 1. Apply forces to the bodies as necessary.

    // 2. Adjust the joint parameters as necessary.

    // 3. Call collision detection. Creates a contact joint for every collision point, and puts it in the contact joint group.
    resetSpaceCollideFlag(pSpace.get());
    spaceCollide(pSpace.get());

    // 4. Take a simulation step.
    if (bUseQuick) dWorldQuickStep(id, stepSize);
    else dWorldStep(id, stepSize);

    DBG_TRY_BLOCK_END
}

void World::spaceCollide2(AbstractGeom * pAGeom1, AbstractGeom * pAGeom2) {
    DBG_TRY_BLOCK_START
    dSpaceCollide2(pAGeom1->id, pAGeom2->id, this, &NearCallback::toPVLENearCallback);
    DBG_TRY_BLOCK_END
}

void World::spaceCollide(Space * pCurSpace) {
    DBG_TRY_BLOCK_START

    // Process space
    if (pCurSpace->collided) return;
    dSpaceCollide(pCurSpace->asSpaceId(), this, &NearCallback::toPVLENearCallback);
    pCurSpace->collided = true;

    // Process sub-spaces
    Space * pCurSpaceSub;
    BOOST_FOREACH(AbstractGeom * pAGeom, pCurSpace->vGeoms) {
        pCurSpaceSub = pAGeom->asSpace();
        if (pCurSpaceSub && !pCurSpaceSub->collided) spaceCollide(pCurSpaceSub);
    }

    DBG_TRY_BLOCK_END
}

void World::resetSpaceCollideFlag(Space * pCurSpace) {
    DBG_TRY_BLOCK_START

    // Process space
    pCurSpace->collided = false;

    // Process sub-spaces
    Space * pCurSpaceSub;
    BOOST_FOREACH(AbstractGeom * pAGeom, pCurSpace->vGeoms) {
        pCurSpaceSub = pAGeom->asSpace();
        if (pCurSpaceSub) resetSpaceCollideFlag(pCurSpaceSub);
    }

    DBG_TRY_BLOCK_END
}

}   // namespace Physics

---