3DPhy.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/Entity/3DPhy.h>

#include <osg/MatrixTransform>

#include <PVLE/Game/PVLEPlayer.h>
#include <PVLE/Game/PVLETeam.h>
#include <PVLE/Physics/Contact.h>
#include <PVLE/Physics/Joint.h>
#include <PVLE/Physics/Body.h>
#include <PVLE/Physics/Space.h>
#include <PVLE/Physics/Visitor.h>
#include <PVLE/Physics/CombinedTraversal.h>
#include <PVLE/3D/Commons.h>
#include <PVLE/3D/CollectNodesVisitor.h>
#include <PVLE/3D/Utility3D.h>


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

#ifndef REMOVE_DEV_FEATURES

    class DirtyDebugGeodeCallback : public Physics::GHChangeCallback {
    public:
        DirtyDebugGeodeCallback(bool init_dirty = false) : dirty(init_dirty) {}
        virtual void operator()(Physics::GeometryHandler * pGH) { dirty = true; traverse(pGH); }
        bool dirty;
    };

    #include <osg/CullFace>
    #include <osg/BlendFunc>
    #include <osg/Geode>
    #include <osg/Depth>

    class PhysicsDebugUpdateCB : public PhysicsUpdateCB {
    public:
        PhysicsDebugUpdateCB(Physics::Geom * pGeom) : PhysicsUpdateCB(pGeom) {}
        virtual void operator() (osg::Node * pNode, osg::NodeVisitor * pNV) {
            boost::polymorphic_downcast<osg::MatrixTransform *>(pNode)->setMatrix(pGeom->getMatrix());
            osg::StateSet * pStateSet = boost::polymorphic_downcast<osg::MatrixTransform *>(pNode)->getOrCreateStateSet();

            // Set color for debug, alpha mode, depth mode, cull face, and render bin
            static const float alpha = .4f;
            static const osg::Vec4 BE_GE(.9f, .2f, .2f, alpha);     // Red:   Body Enabled,  Geom Enabled  (Full object)
            static const osg::Vec4 BD_GE(.2f, .2f, .9f, alpha);     // Blue:  Body Disabled, Geom Enabled  (Collision only)
            static const osg::Vec4 BE_GD(.3f, .9f, .2f, alpha);     // Green: Body Enabled,  Geom Disabled (Forces only)
            static const osg::Vec4 BD_GD(.4f, .4f, .4f, alpha);     // Grey:  Body Disabled, Geom Disabled (Disabled)
            bool ge = dGeomIsEnabled(*pGeom)!=0;
            bool be = pGeom->getBody() && dBodyIsEnabled(*pGeom->getBody());
            setColor(pStateSet, be ? (ge ? BE_GE : BE_GD) : (ge ? BD_GE : BD_GD));
            pStateSet->setAttributeAndModes(new osg::CullFace(osg::CullFace::FRONT_AND_BACK), osg::StateAttribute::OFF);            // No cull-face
            pStateSet->setAttributeAndModes(new osg::BlendFunc(osg::BlendFunc::ONE, osg::BlendFunc::ONE_MINUS_SRC_ALPHA));          // Alpha
            pStateSet->setAttributeAndModes(new osg::Depth(osg::Depth::LEQUAL, 0, 1, false));                                       // Draw "Less or equal" (in case 3D and geoms are the same)
            pStateSet->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);                                                            // Draw back to front

            // Update size (or other parameters) in case geom has changed
            // This part of the function can be commented if geoms parameters do not change after debug 3D objects are created. But as we're in debug mode, a small performance loss is not important.
            // If this code is commented out, you'll nedd to call "createPhysicsDebugView(false); createPhysicsDebugView(true);" if you wish to refresh (= recreate) 3D debug objects.
            Physics::GeometryHandler * pGH = pGeom->getHandler();
            DirtyDebugGeodeCallback * dirtyCallback = boost::polymorphic_downcast<DirtyDebugGeodeCallback *>(pGH->pChangeCallback.get());
            if (dirtyCallback->dirty) {
                // Geom has changed - 3D representation must change
                dirtyCallback->dirty = false;

                osg::Geode * pGeode = boost::polymorphic_downcast<osg::Geode *>(boost::polymorphic_downcast<osg::MatrixTransform *>(pNode)->getChild(0));
                osg::Shape * pShape = NULL;
                if (pGH->type() != Physics::GeometryHandler::MESH) pShape = pGeode->getDrawable(0)->getShape();

                std::pair<dReal, dReal> params;
                switch (pGH->type()) {
                    case Physics::GeometryHandler::BOX:
                        boost::polymorphic_downcast<osg::Box *>(pShape)->setHalfLengths(boost::polymorphic_downcast<Physics::BoxHandler *>(pGH)->getSize()/2.f);
                        break;
                    case Physics::GeometryHandler::SPHERE:
                        boost::polymorphic_downcast<osg::Sphere *>(pShape)->setRadius(boost::polymorphic_downcast<Physics::SphereHandler *>(pGH)->getRadius());
                        break;
                    case Physics::GeometryHandler::CAPSULE:
                        params = boost::polymorphic_downcast<Physics::CapsuleHandler *>(pGH)->getParams();
                        boost::polymorphic_downcast<osg::Capsule *>(pShape)->set(osg::Vec3(0,0,0), params.first, params.second);
                        break;
                    case Physics::GeometryHandler::CYLINDER:
                    case Physics::GeometryHandler::RAY:
                        params = boost::polymorphic_downcast<Physics::CylinderHandler *>(pGH)->getParams();
                        boost::polymorphic_downcast<osg::Cylinder *>(pShape)->set(osg::Vec3(0,0,0), params.first, params.second);
                        break;
                    //case Physics::GeometryHandler::RAY :
                    //  break;
                    case Physics::GeometryHandler::MESH:
                        // Mesh geom is not supposed to change (morph not supported... yet)
                        pGeode->setDrawable(0, new osg::Geometry(*(boost::polymorphic_downcast<Physics::MeshHandler *>(pGH)->getGeometry()), osg::CopyOp::DEEP_COPY_ALL));
                        break;
                    default:
                        THROW_TRACED_EXCEPTION("Not implemented - Error on creation");
                }
                ASSERT(pGeode->getNumDrawables() == 1);     // Or else do a for() loop below
                pGeode->getDrawable(0)->setUseDisplayList(false);
            }

            traverse(pNode, pNV);
        }
    };

    #include <PVLE/3D/RemoveStatesetsVisitor.h>

#endif


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


#ifdef PVLE_NETWORKING
#include <tnl/tnlGhostConnection.h>
#include <PVLE/Network/NetConverters.h>

//TNL_IMPLEMENT_NETOBJECT(C3DPhy);

void C3DPhy::packBody(const Physics::Body * pBody, TNL::BitStream * pStream) {
    Net::write(pStream, dBodyGetPositionV(*pBody));
    Net::write(pStream, dBodyGetQuaternionV(*pBody));
    Net::write(pStream, dBodyGetLinearVelV(*pBody));
    Net::write(pStream, dBodyGetAngularVelV(*pBody));
}

void C3DPhy::unpackBody(Physics::Body * pBody, TNL::BitStream * pStream) {
    osg::Vec3 pos, vel, avel;
    osg::Quat quat;

    Net::read(pStream, pos);
    Net::read(pStream, quat);
    Net::read(pStream, vel);
    Net::read(pStream, avel);

    dBodyEnable(*pBody);
    dBodySetPosition(*pBody, pos);
    dBodySetQuaternion(*pBody, quat);
    dBodySetLinearVel(*pBody, vel);
    dBodySetAngularVel(*pBody, avel);
}

TNL::U32 C3DPhy::packUpdate(TNL::GhostConnection *, TNL::U32 mask, TNL::BitStream * pStream) {
    // MOVED - Send position & velocity of all bodies.
    if (pStream->writeFlag((mask & MOVED) != 0)) {

#ifdef _DEBUG
        // Send debug info : the number of geoms, the list of geoms that have a body, and the number of additionnal bodies
        {
            pStream->write(static_cast<UINT>(vGeoms.size()));
            BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) pStream->writeFlag(pGeom->getBody() != NULL);
            pStream->write(static_cast<UINT>(vAdditionalBodies.size()));
        }
#endif

        Physics::Body * pBody = NULL;
        BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) {
            pBody = pGeom->getBody();
            if (pBody) packBody(pBody, pStream);
        }
        BOOST_FOREACH(pBody, vAdditionalBodies) packBody(pBody, pStream);

        mask &= ~MOVED;
    }

    return mask;        // As we don't know if the update is fully done, we return a non-zero mask (final class, such as Tank, Adav, etc. should return 0).
}

void C3DPhy::unpackUpdate(TNL::GhostConnection *, TNL::BitStream * pStream) {
    // MOVED
    if (pStream->readFlag()) {
#ifdef _DEBUG
        // Read debug info : the number of geoms, the list of geoms that have a body, and the number of additionnal bodies
        {
            UINT nb;
            pStream->read(&nb);
            ASSERT(nb == vGeoms.size());                // Server's number of geoms is not the same
            BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) ASSERT(pStream->readFlag() == (pGeom->getBody() != NULL));     // Server's geoms that have bodies are not the same number or are not in the same order
            pStream->read(&nb);
            ASSERT(nb == vAdditionalBodies.size());     // Server's number of additional bodies is not the same
        }
#endif

        Physics::Body * pBody = NULL;
        BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) {
            pBody = pGeom->getBody();
            if (pBody) unpackBody(pBody, pStream);
        }
        BOOST_FOREACH(pBody, vAdditionalBodies) unpackBody(pBody, pStream);
    }
}

void C3DPhy::performScopeQuery(TNL::GhostConnection * pConnection) {
    ASSERT(false && "Implementation error - C3DPhy is not supposed to be a reference for scope query. Use Player instead.");
}

void C3DPhy::onGhostRemove() {
    // The object is about to be deleted by the networking system.
    // The 3DPhy must be removed from its owner so that its ref count is 0 (but not deleted of course), because an osg::Referenced object is not supposed to be deleted if its ref count is not 0.
    if (p3DPhyOwner) p3DPhyOwner->doNetworkClientRemove3DPhy(this);
    ASSERT(_refCount == 0);         // If not, then the 3DPhy is referenced from somewhere else (= not in its owner since we just dereferenced). You must then unreference the object before the network deletes it.
}
#endif


C3DPhy::C3DPhy(const C3DPhy * pInheritTeamAndPlayer) :
    pModel(NULL), p3DPhyOwner(NULL), ttl(-1), pPlayer(NULL), pTeam(NULL)
#ifndef REMOVE_DEV_FEATURES
    , pModel_debug(NULL)
#endif
{
    //mNetFlags.set(Ghostable);

    if (pInheritTeamAndPlayer) setTeamAndPlayer(*pInheritTeamAndPlayer);
}

C3DPhy::C3DPhy(osg::MatrixTransform * pModel, Physics::Geom * pGeom, EInitOptions options, const C3DPhy * pInheritTeamAndPlayer) :
    pModel(pModel), p3DPhyOwner(NULL), ttl(-1), pPlayer(NULL), pTeam(NULL)
#ifndef REMOVE_DEV_FEATURES
    , pModel_debug(NULL)
#endif
{
    //mNetFlags.set(Ghostable);

    if (pGeom) addGeom(pGeom);
    if (options == INIT_BIND_ONCE) set3DtoPhy();
    else if (options == INIT_AUTO_BIND) bind();
    if (pInheritTeamAndPlayer) setTeamAndPlayer(*pInheritTeamAndPlayer);
}


C3DPhy::C3DPhy(const C3DPhy & v, UINT copyOpts) :
    pModel(NULL), p3DPhyOwner(NULL), ttl(-1), pPlayer(NULL), pTeam(NULL)
#ifndef REMOVE_DEV_FEATURES
    , pModel_debug(NULL)
#endif
{
    //mNetFlags.set(Ghostable);

    const UINT modelCopyOpts = osg::CopyOp::DEEP_COPY_NODES | osg::CopyOp::DEEP_COPY_OBJECTS | osg::CopyOp::DEEP_COPY_STATEATTRIBUTES | osg::CopyOp::DEEP_COPY_STATESETS;
    const UINT ageomCopyOpts = Physics::AbstractGeom::COPY_ALL;

    // TTL
    if (copyOpts & COPY_TTL) ttl = v.ttl;

    // Team and player
    setTeamAndPlayer(v);

    // -- 3D --
    if (copyOpts & COPY_3D) {
        if (v.pModel) pModel = new osg::MatrixTransform(*(v.pModel), modelCopyOpts);
#ifndef REMOVE_DEV_FEATURES
        if (v.pModel_debug) pModel_debug = new osg::MatrixTransform(*(v.pModel_debug), modelCopyOpts);
#endif
    }

    // -- Physics --
    if (copyOpts & COPY_PHYSICS) {
        Physics::TDuplicatedBodiesMap duplicatedBodies;                             // Map for composite objects handling
        Physics::TDuplicatedBodiesMap * pDuplicatedBodies = &duplicatedBodies;      // Avoids doing it each time... :)

        // Copy spaces (including children geoms/spaces)
        ASSERT(vSpaces.size() == 0);
        BOOST_FOREACH(Physics::Space * pSpace, v.vSpaces) {
            vSpaces.push_back(new Physics::Space(*pSpace, ageomCopyOpts, this, pDuplicatedBodies));
        }
        // Copy geoms (copy constructor does not insert geoms in any space)
        BOOST_FOREACH(const Physics::Geom * pGeom, v.vGeoms) {
            vGeoms.push_back(new Physics::Geom(*pGeom, ageomCopyOpts, this, pDuplicatedBodies));
        }
        // Copy additionnal bodies (copy constructor does not copy joints nor auto-join bodies)
        Physics::TDuplicatedBodiesMap::iterator dupIt;
        BOOST_FOREACH(const Physics::Body * pBody, v.vAdditionalBodies) {
            dupIt = duplicatedBodies.find(pBody);
            if (dupIt != duplicatedBodies.end()) {
                vAdditionalBodies.push_back(new Physics::Body(*pBody));
                //duplicatedBodies.insert(Physics::TDuplicatedBody(pBody, TODO));       // Not needed here
            }
        }
        // Copy joints (copy constructor does not attach the joint to anything)
        BOOST_FOREACH(Physics::Joint * pJoint, v.vJoints) {
            vJoints.push_back(new Physics::Joint(*pJoint));
        }

        // -- Post-copy opertations --

        // ----- OBSOLETE -----
        //for(SpaceList::const_iterator it = v.vSpaces.begin(), itEnd = v.vSpaces.end(), itCopy = vSpaces.begin(); it != itEnd; ++it, ++itCopy) {
        //  insertAbstractGeomInSpaces(v.vSpaces, *it, *itCopy);
        //}
        //for(GeomList::const_iterator it = v.vGeoms.begin(), itEnd = v.vGeoms.end(), itCopy = vGeoms.begin(); it != itEnd; ++it, ++itCopy) {
        //  insertAbstractGeomInSpaces(v.vSpaces, *it, *itCopy);
        //}
        // ----- OBSOLETE (end) -----

        // Link copied bodies (and copied ADDITIONAL bodies) with copied joints
        for(JointList::const_iterator it = v.vJoints.begin(), itEnd = v.vJoints.end(), itCopy = vJoints.begin(); it != itEnd; ++it, ++itCopy) {
            attachBodies(v.vGeoms, v.vSpaces, v.vAdditionalBodies, *it, *itCopy);
        }

        // Copy bindings
        if (copyOpts & COPY_3D) {
            // For each node in the ORIGINAL model
            //   If it has a PhysicsUpdateCB to an ORIGINAL geom, and if the node has effectively been duplicated
            //     Add a PhysicsUpdateCB to the COPIED geom into the COPIED node
            //     NOTE - Actually, callbacks POINTERS are copied with the model

            // 1. Create a list of nodes for both original and copied models
            CollectNodesVisitor collectOriginal;
            CollectNodesVisitor collectCopy;
            v.pModel->accept(collectOriginal);
            pModel->accept(collectCopy);
            ASSERT(collectOriginal.nodes.size() == collectCopy.nodes.size());
            // 2. For each ORIGINAL node...
            const osg::NodeCallback * pOriginalCB;
            const PhysicsUpdateCB  * pOriginalPhysicsCB;
            //osg::NodeCallback * pCopyCB;
            //PhysicsUpdateCB  * pCopyPhysicsCB;
            //bool bRemoveCallback;

            Physics::Geom * pGeomCopy;
            std::vector<osg::Node *>::iterator itCopy = collectCopy.nodes.begin();
            for(std::vector<osg::Node *>::const_iterator it = collectOriginal.nodes.begin(), itEnd = collectOriginal.nodes.end(); it != itEnd; ++it, ++itCopy) {
                if (*it == *itCopy) continue;           // Node hasn't been duplicated - We do nothing
                (*itCopy)->setUpdateCallback(NULL);     // Copied node points to the same updataCallback, so we reset it

                //for(pCopyCB = (*itCopy)->getUpdateCallback(); pCopyCB; pCopyCB = pCopyCB->getNestedCallback()) {
                //  bRemoveCallback = true;
                //  pCopyPhysicsCB = dynamic_cast<PhysicsUpdateCB *>(pCopyCB);
                //  if (pCopyPhysicsCB) {
                //      // PhysicsUpdateCB exists - Update the binding if the geom exists in the 3DPhy
                //      pGeomCopy = findGeomCopy(v.vGeoms, pCopyPhysicsCB->getGeom());
                //      if (pGeomCopy) {
                //          pCopyPhysicsCB->setGeom(pGeomCopy);
                //          bRemoveCallback = false;
                //      }
                //  }

                //  // Delete all copied callbacks except modified bindings
                //  if (bRemoveCallback) removeUpdateCallback(**itCopy, pCopyPhysicsCB);
                //}

                // For every callback the original node has
                for(pOriginalCB = (*it)->getUpdateCallback(); pOriginalCB; pOriginalCB = pOriginalCB->getNestedCallback()) {
                    pOriginalPhysicsCB = dynamic_cast<const PhysicsUpdateCB *>(pOriginalCB);
                    if (pOriginalPhysicsCB) {
                        // PhysicsUpdateCB exists - Copy the binding if the geom exists in the 3DPhy
                        pGeomCopy = findGeomCopy(v.vGeoms, v.vSpaces, pOriginalPhysicsCB->getGeom());
                        if (pGeomCopy) {
                            // Add a new update callback
                            (*itCopy)->setUpdateCallback(new PhysicsUpdateCB(pGeomCopy));
                            //addUpdateCallback(**itCopy, new PhysicsUpdateCB(pGeomCopy));
                            break;      // At most one PhysicsUpdateCB
                        }
                    }
                }
            }
        }
    }

    // 3DPhyOwner
    if ((copyOpts & COPY_OWNER) && v.p3DPhyOwner) v.p3DPhyOwner->addInScene(this);

    //if ((copyOpts & COPY_DELETE_CALLBACK) && v.pDeleteCB) pDeleteCB = ???;
}

//void C3DPhy::insertAbstractGeomInSpaces(const SpaceList & vOriginalSpaces, Physics::AbstractGeom * pOriginalAbstractGeom, Physics::AbstractGeom * pCopyAbstractGeom) {
//  // For each ORIGINAL space...
//  for(SpaceList::const_iterator itSpace = vOriginalSpaces.begin(), itSpaceEnd = vOriginalSpaces.end(), itSpaceCopy = vSpaces.begin(); itSpace != itSpaceEnd; ++itSpace, ++itSpaceCopy) {
//      // ... if the ORIGINAL space contains the ORIGINAL geom...
//      if ((*itSpace)->isInSpace(pOriginalAbstractGeom)) {
//          // ... then add the COPIED geom in the COPIED space.
//          ASSERT(*itSpaceCopy != pCopyAbstractGeom);
//          (*itSpaceCopy)->add(pCopyAbstractGeom);
//      }
//  }
//}

class FindBodiesCopy {
public:
    FindBodiesCopy(const Physics::Body * pOriginalBody1, const Physics::Body * pOriginalBody2)
        : pOriginalBody1(pOriginalBody1), pOriginalBody2(pOriginalBody2), pCopyBody1(NULL), pCopyBody2(NULL), body1Found(false), body2Found(false)
    {
        body1Found = (pOriginalBody1 == NULL);      // true if the search is complete for this body
        body2Found = (pOriginalBody2 == NULL);
    }

    inline bool operator()(Physics::Geom * pA, Physics::Geom * pB) {
        return operator()(pA->getBody(), pB->getBody());
    }

    bool operator()(Physics::Body * pA, Physics::Body * pB) {
        // ... if the ORIGINAL body is attached to the ORIGINAL joint...
        if (pA) {
            // ... then save the COPIED body.
            if (!body1Found && pA == pOriginalBody1) {
                pCopyBody1 = pB;
                body1Found = true;
                if (body2Found) return false;       // We've got both !
            }
            if (!body2Found && pA == pOriginalBody2) {
                pCopyBody2 = pB;
                body2Found = true;
                if (body1Found) return false;       // We've got both !
            }
        }
        return true;    // Continue loop
    }


    Physics::Body * pCopyBody1, * pCopyBody2;
    bool body1Found, body2Found;

    const Physics::Body * const pOriginalBody1;
    const Physics::Body * const pOriginalBody2;
};



void C3DPhy::attachBodies(const GeomList & vOriginalGeoms, const SpaceList & vOriginalSpaces, const BodyList & vOriginalAdditionnalBodies, Physics::Joint * pOriginalJoint, Physics::Joint * pCopyJoint) {
    // Here, we must find BOTH bodies attached to the joint in order to call pCopyJoint->attach().
    FindBodiesCopy finder(pOriginalJoint->getBody1(), pOriginalJoint->getBody2());      // Bodies we search

    // First check ORIGINAL additional bodies
    for(BodyList::const_iterator itBody = vOriginalAdditionnalBodies.begin(), itBodyEnd = vOriginalAdditionnalBodies.end(), itBodyCopy = vAdditionalBodies.begin(); itBody != itBodyEnd; ++itBody, ++itBodyCopy) {
        if (!finder(*itBody, *itBodyCopy)) break;
    }

    if (!(finder.body1Found && finder.body2Found)) {
        // Search ORIGINAL geoms
        for(GeomList::const_iterator itGeom = vOriginalGeoms.begin(), itGeomEnd = vOriginalGeoms.end(), itGeomCopy = vGeoms.begin(); itGeom != itGeomEnd; ++itGeom, ++itGeomCopy) {
            if (!finder(*itGeom, *itGeomCopy)) break;
        }

        // Search ORIGINAL spaces (recursively)
        if (!(finder.body1Found && finder.body2Found)) {
            for(SpaceList::const_iterator itSpace = vOriginalSpaces.begin(), itSpaceEnd = vOriginalSpaces.end(), itSpaceCopy = vSpaces.begin(); itSpace != itSpaceEnd; ++itSpace, ++itSpaceCopy) {
                combinedTraversal(**itSpace, **itSpaceCopy, finder);
                if (finder.body1Found && finder.body2Found) break;
            }
        }
    }

    // Do not attach bodies if only one has been found (else we would attach a body to the environment !)
    if (finder.body1Found && finder.body2Found) {
        pCopyJoint->attach(finder.pCopyBody1, finder.pCopyBody2);
        pCopyJoint->copyParameters(*pOriginalJoint);
    }
    else
        LOG_INFO << "3DPhy copy - A joint was not attached because bodies where not found inside the same 3DPhy" << std::endl;
}


class FindGeomCopy {
public:
    FindGeomCopy(const Physics::Geom * pOriginalGeom) : pOriginalGeom(pOriginalGeom), pCopyGeom(NULL) {}
    bool operator()(Physics::Geom * pA, Physics::Geom * pB) {
        if (pA == pOriginalGeom) {
            pCopyGeom = pB;
            return false;
        }
        return true;    // Continue loop
    }

    Physics::Geom * pCopyGeom;

protected:
    const Physics::Geom * pOriginalGeom;
};


Physics::Geom * C3DPhy::findGeomCopy(const GeomList & vOriginalGeoms, const SpaceList & vOriginalSpaces, const Physics::Geom * pOriginalGeom) {
    // Search in geoms
    for(GeomList::const_iterator itGeom = vOriginalGeoms.begin(), itGeomEnd = vOriginalGeoms.end(), itGeomCopy = vGeoms.begin(); itGeom != itGeomEnd; ++itGeom, ++itGeomCopy) {
        if (*itGeom == pOriginalGeom) return *itGeomCopy;
    }
    // Search in spaces (recursively)
    FindGeomCopy finder(pOriginalGeom);
    for(SpaceList::const_iterator itSpace = vOriginalSpaces.begin(), itSpaceEnd = vOriginalSpaces.end(), itSpaceCopy = vSpaces.begin(); itSpace != itSpaceEnd; ++itSpace, ++itSpaceCopy) {
        combinedTraversal(**itSpace, **itSpaceCopy, finder);
        if (finder.pCopyGeom) return finder.pCopyGeom;
    }
    return NULL;
}



void C3DPhy::addGeom(Physics::Geom * pGeom) {
    ASSERT(pGeom);
    ASSERT(std::find(vGeoms.begin(), vGeoms.end(), pGeom) == vGeoms.end());     // Do not add twice!
    vGeoms.push_back(pGeom);
#ifdef _DEBUG
    IGeomCollisionContainer * pCC = pGeom->getCollisionContainer();
    if (pCC && pCC != this) LOG_WARN << "3D-Phy is having a geom with already another collision container." << std::endl;
#endif
    if (!pGeom->getCollisionContainer()) pGeom->setCollisionContainer(this);
}

void C3DPhy::addSpace(Physics::Space * pSpace) {
    ASSERT(pSpace);
    ASSERT(std::find(vSpaces.begin(), vSpaces.end(), pSpace) == vSpaces.end());     // Do not add twice!
    if (pSpace->getParentSpace()) {
        LOG_WARN << "[Performance warn] Cannot add a space that already has a parent space to a 3DPhy." << std::endl;
        return;
    }
    vSpaces.push_back(pSpace);
}

void C3DPhy::addBody(Physics::Body * pBody) { ASSERT(pBody); vAdditionalBodies.push_back(pBody); }

void C3DPhy::bind() {
#ifdef _DEBUG
    if (vGeoms.size() >1) LOG_NOTICE << "bind() called on a 3D-Phy that has multiple geoms : Using only the first one." << std::endl;
#endif
    bind(0);
}

void C3DPhy::bind(UINT numGeom) {
    ASSERT(vGeoms.size() >0);
    pModel->setUpdateCallback(new PhysicsUpdateCB(vGeoms[numGeom]));
}

void C3DPhy::set3DtoPhy() {
#ifdef _DEBUG
    if (vGeoms.size() >1) LOG_NOTICE << "set3DtoPhy() called on a 3D-Phy that has multiple geoms : Using only the first one." << std::endl;
#endif
    set3DtoPhy(0);
}

void C3DPhy::set3DtoPhy(UINT numGeom) {
    ASSERT(vGeoms.size() >0);
    pModel->setMatrix(vGeoms[numGeom]->getMatrix());
}


void C3DPhy::hitAfter(Physics::Contact * pContacts, unsigned int nbContacts, Physics::Contact * pMaxEnergyContact, Physics::Geom & thisGeom, Physics::Geom & otherGeom, bool contactJointCreated) {
    // -----
    // Dev note: This way of removing life points on collision is stupid from a physical point of view: it "duplicates" energy from collision to life points removal.
    // Actually, 3D-Phy that can take damage should slow down their velocity and use a part of the cinetic energy of the collition to deform itself (=loose life points). Similarily to perforant collision, we should compute collision energy, split into 2 according to an "elasticity" parameter => cinetic / deformation, slow down geoms speeds (how to do it ?), split deformation into 2 : damage (loose life points) / heat (does nothing).
    // But it takes time to compute, so we can approximate this result by using armour factors.
    // -----

    // Typical case: Multiple contacts (in 1 frame) - Do not remove multiple times life points! Use the most energetic contact for instance.
    if (contactJointCreated) isHitByCinetic(pMaxEnergyContact->arithmeticCineticEnergy, pMaxEnergyContact->getPos());
}


void C3DPhy::onTTLZero() {
    if (!pDeleteCB.valid() || (*pDeleteCB)(this)) {
        if (p3DPhyOwner) p3DPhyOwner->markAsRemoved(this);
    }
}

void C3DPhy::setPlayer(PVLEPlayer * pPlayer) {
    this->pPlayer = pPlayer;
    this->pTeam = pPlayer ? pPlayer->getTeam() : NULL;
}

void C3DPhy::setTeam(PVLETeam * pTeam) {
    if (this->pTeam == pTeam) return;
    this->pTeam = pTeam;
    pPlayer = NULL;
}

void C3DPhy::setTeamAndPlayer(const C3DPhy & v) {
    ASSERT(!v.pPlayer || v.pPlayer->getTeam() == v.getTeam());
    pTeam = v.pTeam;
    pPlayer = v.pPlayer;
}

void C3DPhy::createPhysicsDebugView(bool enable) {
#ifndef REMOVE_DEV_FEATURES
    if (enable) {
        if (!pModel_debug) {
            // Create all objects needed
            pModel_debug = new osg::MatrixTransform();
            ASSERT(p3DPhyOwner);        // This method can only be called after the 3DPhyOwner has been set !
            p3DPhyOwner->getParentGroup()->addChild(pModel_debug);
            // Temp variables :
            osg::MatrixTransform * pNodeTemp;
            Physics::GeometryHandler * pGH;
            std::pair<dReal, dReal> params;
            // For all geoms :
            //for(std::vector<Physics::Geom*>::const_iterator it = vGeoms.begin(), itEnd = vGeoms.end(); it!=itEnd; ++it) {
            BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) {
                pGH = pGeom->getHandler();
                switch (pGH->type()) {
                    case Physics::GeometryHandler::BOX:
                        pNodeTemp = createShapeGeodeBox(boost::polymorphic_downcast<Physics::BoxHandler *>(pGH)->getSize());
                        break;
                    case Physics::GeometryHandler::SPHERE:
                        pNodeTemp = createShapeGeodeSphere(boost::polymorphic_downcast<Physics::SphereHandler *>(pGH)->getRadius());
                        break;
                    case Physics::GeometryHandler::CAPSULE:
                        params = boost::polymorphic_downcast<Physics::CapsuleHandler *>(pGH)->getParams();
                        pNodeTemp = createShapeGeodeCapsule(params.first, params.second);
                        break;
                    case Physics::GeometryHandler::CYLINDER:
                        params = boost::polymorphic_downcast<Physics::CylinderHandler *>(pGH)->getParams();
                        pNodeTemp = createShapeGeodeCylinder(params.first, params.second);
                        break;
                    case Physics::GeometryHandler::RAY:
                        {
                            // Temporary implementation : use a cylinder instead of a segment

                            //Physics::RayHandler * pRay = boost::polymorphic_downcast<Physics::RayHandler *>(pGH);
                            //osg::Vec3 pos = pRay->getStartPoint();
                            //pNodeTemp = new osg::MatrixTransform();
                            //pNodeTemp->addChild(createSegment(pos, pos + pRay->getDir() * pRay->getLength()));

                            pNodeTemp = createShapeGeodeCylinder(.1, boost::polymorphic_downcast<Physics::RayHandler *>(pGH)->getLength());
                        }
                        break;
                    case Physics::GeometryHandler::MESH:
                        {
                            osg::Geode * pGeode = new osg::Geode();
                            pGeode->addDrawable(new osg::Geometry(*(boost::polymorphic_downcast<Physics::MeshHandler *>(pGH)->getGeometry()), osg::CopyOp::DEEP_COPY_ALL));
                            osg::ref_ptr<osg::NodeVisitor> nv = new RemoveStatesetsVisitor();
                            pGeode->accept(*nv.get());      // Removes Statesets in order to add a special one.

                            pNodeTemp = new osg::MatrixTransform();
                            pNodeTemp->addChild(pGeode);
                        }
                        break;
                    default:
                        pNodeTemp = NULL;       // Not implemented !
                }

                if (pNodeTemp) {
                    // Bind with half-transparent shapes
                    pNodeTemp->setUpdateCallback(new PhysicsDebugUpdateCB(pGeom));
                    pGH->addOrSetChangeCallback(new DirtyDebugGeodeCallback());
                    pModel_debug->addChild(pNodeTemp);
                }
            }
        }
    } else {
        if (pModel_debug) {
            // Delete all objects
            p3DPhyOwner->getParentGroup()->removeChild(pModel_debug);
            pModel_debug = NULL;
        }
    }
#endif
}


void C3DPhy::clearBodyContactPos() {
    Physics::Body * pBody;
    BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) {
        pBody = pGeom->getBody();
        if (pBody) pBody->contactPos.clear();
    }
}


void C3DPhy::accept(Physics::Visitor & v) {
    // Geoms
    BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) pGeom->accept(v);
    // Spaces
    BOOST_FOREACH(Physics::Space * pSpace, vSpaces) pSpace->accept(v);
    // Additional bodies
    BOOST_FOREACH(Physics::Body * pBody, vAdditionalBodies) pBody->accept(v);
    // Joints
    BOOST_FOREACH(Physics::Joint * pJoint, vJoints) pJoint->accept(v);
}


class GeomTranslateVisitor : public Physics::Visitor {
public:
    GeomTranslateVisitor(const osg::Vec3 & vec) : vec(vec) {}
    virtual void apply(Physics::Geom & geom) {
        geom.translate(vec);
        traverse(geom);
    }
protected:
    const osg::Vec3 & vec;
};


void C3DPhy::translate(const osg::Vec3 & vec) {
    // Geoms
    BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) pGeom->translate(vec);
    // Spaces
    GeomTranslateVisitor tv(vec);
    BOOST_FOREACH(Physics::Space * pSpace, vSpaces) pSpace->accept(tv);
    // Additional bodies
    BOOST_FOREACH(Physics::Body * pBody, vAdditionalBodies) pBody->translate(vec);
    // Joints (if attached to the environment)
    BOOST_FOREACH(Physics::Joint * pJoint, vJoints) pJoint->translate(vec);

    // TODO Move unbound models ?
}


class GeomRotateVisitor : public Physics::Visitor {
public:
    GeomRotateVisitor(const osg::Quat & quat, const osg::Vec3 & center) : quat(quat), center(center) {}
    virtual void apply(Physics::Geom & geom) {
        geom.rotate(quat, center);
        traverse(geom);
    }
protected:
    const osg::Quat & quat;
    const osg::Vec3 & center;
};

void C3DPhy::rotate(const osg::Quat & quat) {
    const osg::Vec3 center = getPos();

    // Geoms
    BOOST_FOREACH(Physics::Geom * pGeom, vGeoms) pGeom->rotate(quat, center);
    // Spaces
    GeomRotateVisitor rv(quat, center);
    BOOST_FOREACH(Physics::Space * pSpace, vSpaces) pSpace->accept(rv);
    // Additional bodies
    BOOST_FOREACH(Physics::Body * pBody, vAdditionalBodies) pBody->rotate(quat, center);
    // Joints attached to the environment
    //BOOST_FOREACH(Physics::Joint * pJoint, vJoints) pJoint->rotate(quat, center);

    // TODO Move unbound models ?
}


class FindVisitor : public Physics::Visitor {
public:
    FindVisitor() : result(NULL) {}
    virtual void apply(Physics::Geom & geom) {
        if (!result) result = &geom;
        // DO NOT traverse() ! There's no need to, since we've got our result !
    }

    const Physics::Geom * result;
};

std::pair<const void *, C3DPhy::EReferenceType> C3DPhy::getReferenceFrameObject() const {
    std::pair<const void *, C3DPhy::EReferenceType> ref( getReferenceFrameObjectNoThrow() );
    if (ref.second == NO_REF) {
        // Nothing found: that's an error
        THROW_TRACED_EXCEPTION("No reference frame exists in the 3D-Phy (needs a geom or an additional body). Use hasReferenceFrameObject() to check this.");
    }
    return ref;
}

std::pair<const void *, C3DPhy::EReferenceType> C3DPhy::getReferenceFrameObjectNoThrow() const {
    // Is there a geom ?
    if (!vGeoms.empty()) return std::pair<const void *, EReferenceType>(vGeoms[0], REF_GEOM);

    // Search in spaces.
    FindVisitor fg;
    BOOST_FOREACH(Physics::Space * pSpace, vSpaces) {
        pSpace->accept(fg);
        if (fg.result) return std::pair<const void *, EReferenceType>(fg.result, REF_GEOM_IN_SPACE);
    }

    // Is there an additional body ?
    if (!vAdditionalBodies.empty()) return std::pair<const void *, EReferenceType>(vAdditionalBodies[0], REF_ADDITIONAL_BODY);

    return std::pair<const void *, EReferenceType>(NULL, NO_REF);
}


void C3DPhy::markAsRemoved() { ASSERT(p3DPhyOwner); p3DPhyOwner->markAsRemoved(this); }
void C3DPhy::markAsRemoved(Physics::Geom * pGeom) { ASSERT(p3DPhyOwner); p3DPhyOwner->markAsRemoved(this, pGeom); }
void C3DPhy::markAsRemoved(Physics::Space * pSpace) { ASSERT(p3DPhyOwner); p3DPhyOwner->markAsRemoved(this, pSpace); }
void C3DPhy::markAsRemoved(Physics::Joint * pJoint) { ASSERT(p3DPhyOwner); p3DPhyOwner->markAsRemoved(this, pJoint); }
void C3DPhy::markAsRemoved(Physics::Body * pAdditionalBody) { ASSERT(p3DPhyOwner); p3DPhyOwner->markAsRemoved(this, pAdditionalBody); }


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

#ifdef _DEBUG
    #define C3DPHY_LOCK ++lockCount
#else
    #define C3DPHY_LOCK ((void)0)
#endif

#ifdef _DEBUG
    #define C3DPHY_UNLOCK --lockCount
#else
    #define C3DPHY_UNLOCK ((void)0)
#endif

#ifdef _DEBUG
    #define C3DPHY_LOCK_CHECK ASSERT(lockCount == 0)
#else
    #define C3DPHY_LOCK_CHECK ((void)0)
#endif


#include <osg/Group>
#include <PVLE/Physics/Space.h>

void C3DPhyOwner::addInScene(C3DPhy * p3DPhy) {
    ASSERT(p3DPhy);
    if (p3DPhy->get3DPhyOwner()) THROW_TRACED_EXCEPTION("Cannot own (and add to scene) a 3D-Phy that already have a 3D-Phy owner");
    p3DPhy->set3DPhyOwner(this);

    C3DPHY_LOCK_CHECK;
#ifdef _DEBUG
    ASSERT(v3DPhys.insert(p3DPhy).second);
#else
    v3DPhys.insert(p3DPhy);
#endif
    p3DPhy->ref();      // Manually increase ref count

    pParentGroup->addChild(*p3DPhy);
    pParentSpace->add(*p3DPhy);
}


void C3DPhyOwner::markAsRemoved(C3DPhy * p3DPhy) {
    // Do not delete twice a 3D-Phy (here it's a set, so no need to test).
    ASSERT(p3DPhy);
    C3DPHY_LOCK_CHECK;
    v3DPhyToDelete.insert(p3DPhy);
}


void C3DPhyOwner::markAsRemoved(C3DPhy * p3DPhy, Physics::Geom * pGeom) {
    C3DPHY_LOCK_CHECK;
    ASSERT(pGeom);
    C3DPhy::GeomList::iterator it = std::find(p3DPhy->vGeoms.begin(), p3DPhy->vGeoms.end(), pGeom);
    ASSERT(it != p3DPhy->vGeoms.end());     // The object must belong to the 3DPhy
    p3DPhy->vGeoms.erase(it);
    ASSERT(std::find(vGeomsToDelete.begin(), vGeomsToDelete.end(), pGeom) == vGeomsToDelete.end());     // The object must not already be enqueued for deletion
    vGeomsToDelete.push_back(pGeom);
}


void C3DPhyOwner::markAsRemoved(C3DPhy * p3DPhy, Physics::Space * pSpace) {
    C3DPHY_LOCK_CHECK;
    ASSERT(pSpace);
    C3DPhy::SpaceList::iterator it = std::find(p3DPhy->vSpaces.begin(), p3DPhy->vSpaces.end(), pSpace);
    ASSERT(it != p3DPhy->vSpaces.end());        // The object must belong to the 3DPhy
    p3DPhy->vSpaces.erase(it);
    ASSERT(std::find(vGeomsToDelete.begin(), vGeomsToDelete.end(), pSpace) == vGeomsToDelete.end());        // The object must not already be enqueued for deletion
    vGeomsToDelete.push_back(pSpace);
}


void C3DPhyOwner::markAsRemoved(C3DPhy * p3DPhy, Physics::Joint * pJoint) {
    C3DPHY_LOCK_CHECK;
    ASSERT(pJoint);
    C3DPhy::JointList::iterator it = std::find(p3DPhy->vJoints.begin(), p3DPhy->vJoints.end(), pJoint);
    ASSERT(it != p3DPhy->vJoints.end());        // The object must belong to the 3DPhy
    p3DPhy->vJoints.erase(it);
    ASSERT(std::find(vJointsToDelete.begin(), vJointsToDelete.end(), pJoint) == vJointsToDelete.end());     // The object must not already be enqueued for deletion
    vJointsToDelete.push_back(pJoint);
}


void C3DPhyOwner::markAsRemoved(C3DPhy * p3DPhy, Physics::Body * pAdditionalBody) {
    C3DPHY_LOCK_CHECK;
    ASSERT(pAdditionalBody);
    C3DPhy::BodyList::iterator it = std::find(p3DPhy->vAdditionalBodies.begin(), p3DPhy->vAdditionalBodies.end(), pAdditionalBody);
    ASSERT(it != p3DPhy->vAdditionalBodies.end());      // The object must belong to the 3DPhy
    p3DPhy->vAdditionalBodies.erase(it);
    ASSERT(std::find(vBodiesToDelete.begin(), vBodiesToDelete.end(), pAdditionalBody) == vBodiesToDelete.end());        // The object must not already be enqueued for deletion
    vBodiesToDelete.push_back(pAdditionalBody);
}


void C3DPhyOwner::detatch(C3DPhy * p3DPhy) {
    p3DPhy->set3DPhyOwner(NULL);
    p3DPhy->unref();    // Manually decrease ref count
#ifdef _DEBUG
    ASSERT(v3DPhys.erase(p3DPhy) >0);
#else
    v3DPhys.erase(p3DPhy);
#endif
}

void C3DPhyOwner::detatch_nodelete(C3DPhy * p3DPhy) {
    p3DPhy->set3DPhyOwner(NULL);
    p3DPhy->unref_nodelete();   // Manually decrease ref count
#ifdef _DEBUG
    ASSERT(v3DPhys.erase(p3DPhy) >0);
#else
    v3DPhys.erase(p3DPhy);
#endif
}


void C3DPhyOwner::doDelayed3DPhy() {
    bIsDeleting = true;

    for(T3DPhyToDeleteSet::iterator it=v3DPhyToDelete.begin(), itEnd=v3DPhyToDelete.end(); it!=itEnd; ++it) {
        //osg::ref_ptr<C3DPhy> & p3DPhy = *it;      // Can't do a non-const reference because set::iterator is immutable (the risk is to change the key, as in a map)
        C3DPhy * p3DPhy = it->get();
        // Add model, geoms, spaces and joints to the delete list if the reference count is about to be 0 (= referenced by v3DPhys AND vNodesToDelete).
        ASSERT(p3DPhy->referenceCount() >= 2);      // Here the 3DPhy is ref-pointed by v3DPhys and v3DPhyToDelete
        if (p3DPhy->referenceCount() <= 2) {
            C3DPHY_LOCK_CHECK;
            if (p3DPhy->getModel()) {
#ifdef PVLE_3DPHY_USES_REF_PTR
                ASSERT(p3DPhy->getModel()->referenceCount() >= 2);          // Referenced by the 3DPhy *and* the 3DPhyOwner's group
#endif
                vNodesToDelete.push_back(p3DPhy->getModel());
            }
#ifndef REMOVE_DEV_FEATURES
            if (p3DPhy->pModel_debug) {
#   ifdef PVLE_3DPHY_USES_REF_PTR
                ASSERT(p3DPhy->pModel_debug->referenceCount() >= 2);            // Referenced by the 3DPhy *and* the 3DPhyOwner's group
#   endif
                vNodesToDelete.push_back(p3DPhy->pModel_debug);
            }
#endif
            vGeomsToDelete.insert (vGeomsToDelete.end(),  p3DPhy->vGeoms.begin(), p3DPhy->vGeoms.end());
            vGeomsToDelete.insert (vGeomsToDelete.end(),  p3DPhy->vSpaces.begin(), p3DPhy->vSpaces.end());
            vJointsToDelete.insert(vJointsToDelete.end(), p3DPhy->vJoints.begin(), p3DPhy->vJoints.end());
            vBodiesToDelete.insert(vBodiesToDelete.end(), p3DPhy->vAdditionalBodies.begin(), p3DPhy->vAdditionalBodies.end());

#ifdef _DEBUG       // Only in debug because it can take time to cleanup the 3DPhy (See C3DPhy destructor).
            p3DPhy->vJoints.clear();
            p3DPhy->vAdditionalBodies.clear();
#endif
        }

        // Remove the 3DPhy from the list
        detatch(p3DPhy);        // Manually decrease ref count
    }
    C3DPHY_LOCK;
    v3DPhyToDelete.clear();
    C3DPHY_UNLOCK;

    // Delayed insertions
    BOOST_FOREACH(osg::ref_ptr<C3DPhy> & p3DPhy, v3DPhyToAdd) {
        addInScene(p3DPhy.get());
    }
    C3DPHY_LOCK;
    v3DPhyToAdd.clear();
    C3DPHY_UNLOCK;

    bIsDeleting = false;
}

void C3DPhyOwner::doDeleteGfx() {
    bIsDeleting = true;

    BOOST_FOREACH(osg::MatrixTransform * pNode, vNodesToDelete) {
        if (pNode) {
#ifdef _DEBUG
            ASSERT(pParentGroup->removeChild(pNode));
#else
            pParentGroup->removeChild(pNode);
#endif
        }
    }

    C3DPHY_LOCK;
    vNodesToDelete.clear();
    C3DPHY_UNLOCK;

    bIsDeleting = false;
}

void C3DPhyOwner::doDeletePhy() {
    bIsDeleting = true;

    // Geoms and spaces
    //Physics::Space * pCurSpace;
    BOOST_FOREACH(Physics::AbstractGeom * pAGeom, vGeomsToDelete) {
        if (pAGeom) {
            ASSERT((pAGeom)->getParentSpace() == pParentSpace);
            //pCurSpace = pAGeom->asSpace();
            //if (pCurSpace) pCurSpace->removeAll();        // STUPID - Need to check if still referenced...
            pParentSpace->remove(pAGeom);
        }
    }
    C3DPHY_LOCK;
    vGeomsToDelete.clear();
    C3DPHY_UNLOCK;

    // Bodies
    BOOST_FOREACH(Physics::Body * pBody, vBodiesToDelete) {
        ASSERT(pBody);
        delete pBody;
    }
    C3DPHY_LOCK;
    vBodiesToDelete.clear();
    C3DPHY_UNLOCK;

    // Joints
    BOOST_FOREACH(Physics::Joint * pJoint, vJointsToDelete) {
        ASSERT(pJoint);
        delete pJoint;
    }
    C3DPHY_LOCK;
    vJointsToDelete.clear();
    C3DPHY_UNLOCK;

    bIsDeleting = false;
}


void C3DPhyOwner::doNetworkClientRemove3DPhy(C3DPhy * p3DPhy) {
    C3DPHY_LOCK_CHECK;
    bIsDeleting = true;

    // Do the same as in doDelayed3DPhy()
    vNodesToDelete.push_back(p3DPhy->getModel());
#ifndef REMOVE_DEV_FEATURES
    vNodesToDelete.push_back(p3DPhy->pModel_debug);
#endif
    vGeomsToDelete.insert (vGeomsToDelete.end(),  p3DPhy->vGeoms.begin(), p3DPhy->vGeoms.end());
    vGeomsToDelete.insert (vGeomsToDelete.end(),  p3DPhy->vSpaces.begin(), p3DPhy->vSpaces.end());
    vJointsToDelete.insert(vJointsToDelete.end(), p3DPhy->vJoints.begin(), p3DPhy->vJoints.end());
    vBodiesToDelete.insert(vBodiesToDelete.end(), p3DPhy->vAdditionalBodies.begin(), p3DPhy->vAdditionalBodies.end());

    // And detach the 3DPhy without deleting it.
    detatch_nodelete(p3DPhy);
    bIsDeleting = false;
}

void C3DPhyOwner::createPhysicsDebugView(bool enable) {
#ifndef REMOVE_DEV_FEATURES
    BOOST_FOREACH(C3DPhy * p3DPhy, v3DPhys) {
        p3DPhy->createPhysicsDebugView(enable);
    }
#endif
}

---