SO3Physics.cpp

Go to the documentation of this file.

 
#include "SCOLPack/SO3SCOL.h"
#include "SO3SceneGraph/SO3Physics.h"
#include "SO3SceneGraph/SO3Scene.h"
#include "SO3SceneGraph/SO3Skeleton.h"
#include "SO3SceneGraph/SO3Bone.h"
#include "SO3PhysicGraph/SO3Body.h"
#include "SO3PhysicGraph/SO3PhysicContraint.h"
#include "SO3PhysicGraph/SO3MaterialPair.h"
#include "SO3PhysicGraph/SO3MaterialID.h"
#include "SO3Renderer/SO3Root.h"
 
extern int SCENE_PRE_RENDER_PHYSIC_EVENT;
//extern int SO3_BODY_LEAVE_WORLD_MSG;
extern int getMaterialOverlapStartedCallback(mmachine m, SCOL_PTR_TYPE id, SCOL_PTR_TYPE param);
extern int getMaterialContactCallback(mmachine m, SCOL_PTR_TYPE id, SCOL_PTR_TYPE param);
extern int getMaterialOverlapEndedCallback(mmachine m, SCOL_PTR_TYPE id, SCOL_PTR_TYPE param);
extern SMaterialPair* getMaterialPairByWorld(SPhysicWorld* world, int ID1, int ID2);
 
 
namespace SO3
{
 
  //forbidden
  SPhysicWorld::SPhysicWorld()
  {
  }
 
  SPhysicWorld::SPhysicWorld(SScene* scene): mscene(scene)
  {
    maxBodyPair = 100;
    initNewtonWorld();
  }
 
  SPhysicWorld::~SPhysicWorld()
  {
    if(worldDebugger)
          {
      if(worldDebuggerIsInit)
      {
              worldDebugger->hideDebugInformation();
        worldDebugger->stopRaycastRecording();
        worldDebugger->deInit();
        worldDebuggerIsInit = false;
      }
      worldDebugger = 0;
          }
 
    ClearContactOverlapStart();
    ClearContactActivated();
    ClearBodyPairs();
    ClearPhysicContraint();
          listOfMaterialID.clear();
          listOfPhysicsMaterialPair.clear();
 
    SO3_SAFE_DELETE(worldDebugger);
    SO3_SAFE_DELETE(world);
  }
 
  void SPhysicWorld::initNewtonWorld()
  {
    world = NULL;
          showPhysicsLine = false;
    mResetPhysicWorld = false;
    mGravity = Ogre::Vector3(0.0f, -9.81f, 0.0f);
          worldIsEnabled = false;
          worldDebugger = 0;
          solverModel = 0;
          frictionModel = 0;
 
          world = new OgreNewt::World();
    world->setThreadCount(2);
 
    SetPhysicFPS(120);
 
    worldDebugger = &world->getDebugger();
    worldDebuggerIsInit = false;
 
    //scol callback with body who leaves
    //deprecated
    //world->setLeaveWorldCallback(std::bind(&SPhysicWorld::LeaveWorldCallBack, this, std::placeholders::_1, std::placeholders::_2));
 
    world->setPreListener(mscene->GetName(), this, PreUpdate);
    world->setpostListener(mscene->GetName(), this, PreUpdate);
  }
 
  OgreNewt::World* SPhysicWorld::GetPhysicWorld()
  {
    return world;
  }
 
  const SScene* SPhysicWorld::GetScene()
  {
    return mscene;
  }
 
  void SPhysicWorld::PreUpdate(const NewtonWorld* const world, void* const listenerUserData, dFloat timestep)
  {
    /*SPhysicWorld* const me = static_cast<SPhysicWorld*>(listenerUserData);
    if(me)
          {
      //OBJpostEvent(SCENE_PRE_RENDER_PHYSIC_EVENT, (int)me->GetScene(), SCOL_PTR (timestep * 1000.0));
    };*/
  }
 
  void SPhysicWorld::PostUpdate(const NewtonWorld* const world, void* const listenerUserData, dFloat timestep)
  {
    /*SPhysicWorld* const me = static_cast<SPhysicWorld*>(listenerUserData);
    if(me)
          {
      //OBJpostEvent(SCENE_POST_RENDER_PHYSIC_EVENT, (int)me->GetScene(), SCOL_PTR (timestep * 1000.0));
    };*/
  }
 
 
  bool SPhysicWorld::GetPhysicWorldEnable()
  {
    return worldIsEnabled;
  }
 
  void SPhysicWorld::SetPhysicWorldEnable(const bool& enable)
  {
    worldIsEnabled = enable;
 
    if (world && worldDebugger)
    {
      if (!worldIsEnabled && showPhysicsLine && worldDebuggerIsInit)
      {
        worldDebugger->hideDebugInformation();
        worldDebugger->stopRaycastRecording();
        // remove debug bodies
        worldDebugger->deInit();
        worldDebuggerIsInit = false;
      }
      else if (worldIsEnabled && showPhysicsLine)
      {
        SetPhysicDebugEnable(true);
      }
    }
  }
 
  bool SPhysicWorld::GetPhysicDebugEnable()
  {
    return showPhysicsLine;
  }
 
  void SPhysicWorld::SetPhysicDebugEnable(const bool& enable)
  {
    //OgreNewt debugger is crappy :/
    //we deinit debugger to remove unused bodies frame listener
    //and to be sure that the bodies list is up to date
    if (worldDebugger)
    {
      if (worldDebuggerIsInit)
      {
        worldDebugger->hideDebugInformation();
        worldDebugger->stopRaycastRecording();
        // remove debug bodies
        worldDebugger->deInit();
        worldDebuggerIsInit = false;
      }
 
      showPhysicsLine = enable;
 
      // create debug bodies
      if (showPhysicsLine)
      {
        worldDebugger->init(const_cast<SScene*>(mscene)->GetOgreScenePointer());
        worldDebugger->startRaycastRecording(false);
        worldDebuggerIsInit = true;
      }
    }
  }
 
  OgreNewt::Debugger* SPhysicWorld::GetPhysicDebugger()
  {
    return worldDebugger;
  }
  
  void SPhysicWorld::UpdatePhysic(const float& frameTime)
  {
    //$BB apply physics after render callback
    if (GetPhysicWorld() != 0)
    {
      //invalidateCache must not be done in world update
      if (GetResetPhysicWorld())
      {
        GetPhysicWorld()->invalidateCache();
        SetResetPhysicWorld(false);
      }
 
      std::vector<MCOLL*> lOverlapStartCp;
      std::vector<MCOLL*> lOverlapStopCp;
      std::vector<MCOLL*> lContactCp;
 
      if (GetPhysicWorldEnable())
      {
        try
        {
          GetPhysicWorld()->update(frameTime);
        }
        catch (std::exception &)
        {
          // update failed
          return;
        }
 
        if (GetPhysicDebugEnable())
          GetPhysicDebugger()->showDebugInformation();
 
        //mutex
        std::unique_lock<std::mutex> l(mutexUpdate);
 
        //$BB send callbacks here to be sure that no scol or other code will be executed while the physic update loop
        //send overlap start callbacks
        const PhysicContactMap lOverlapStart = physicContactOverlapStartMap;
        PhysicContactMap::const_iterator iOverlapStartMap = lOverlapStart.begin();
        while (iOverlapStartMap != lOverlapStart.end())
        {
          MCOLL* mCollResult = iOverlapStartMap->second;
          lOverlapStartCp.push_back(mCollResult);
          iOverlapStartMap++;
        }
        physicContactOverlapStartMap.clear();
 
        //send contact callbacks
        const PhysicContactMap lContacts = physicContactMap;
        PhysicContactMap::const_iterator iContactMap = lContacts.begin();
        while (iContactMap != lContacts.end())
        {
          MCOLL* mCollResult = iContactMap->second;
          lContactCp.push_back(mCollResult);
          iContactMap++;
        }
        physicContactMap.clear();
 
        const BodyPairMap bodyPairList = bodyPairMap;
        // Parsing all bodies that were detected in contact
        BodyPairMap::const_iterator iBodyPairMap = bodyPairList.begin();
        while (iBodyPairMap != bodyPairList.end())
        {
          // Checking if bodies are still in contact. Note that NewtonBodyGetFirstContactJoint() is not binded in OgreNewt for the moment.
          bool bodiesInContact = false;
          SBody** bpair = iBodyPairMap->second;
          SBody* fbody0 = bpair[0];
          SBody* fbody1 = bpair[1];
          if ((fbody0 != NULL) && (fbody1 != NULL))
          {
            OgreNewt::Body* nbody0 = fbody0->getOgreNewtBodyPointer();
            OgreNewt::Body* nbody1 = fbody1->getOgreNewtBodyPointer();
 
            if ((nbody0 != NULL) && (nbody1 != NULL))
            {
              NewtonBody* b0 = nbody0->getNewtonBody();
              NewtonBody* b1 = nbody1->getNewtonBody();
              NewtonJoint* contactJoint = NewtonBodyGetFirstContactJoint(b0);
 
              while((contactJoint) && (!bodiesInContact))
              {
                // Get infos about the bodies of contact from Newton
                NewtonBody* b0Bis = NewtonJointGetBody0(contactJoint);
                NewtonBody* b1Bis = NewtonJointGetBody1(contactJoint);
                int colstate = NewtonJointIsActive(contactJoint);
                //void* contact = NewtonContactJointGetFirstContact(contactJoint);
 
                // Is the body still in contact with the second body of the pair value?
                if(colstate && ((b0Bis == b0)&&(b1Bis == b1)) || ((b0Bis == b1)&&(b1Bis == b0)))
                  bodiesInContact = true;
                else
                  contactJoint = NewtonBodyGetNextContactJoint(b0, contactJoint);
              }
 
              // If not, remove the bodies pair from the map.
              if(!bodiesInContact)
              {
                // Getting the material pair. TODO: cleaner way to parse MaterialPair list
                SMaterialPair* matPair = 0;
                const SMaterialPairMap materialPairList = listOfPhysicsMaterialPair;
                SMaterialPairMap::const_iterator iMaterialPairList = materialPairList.begin();
                while((iMaterialPairList != materialPairList.end()) && (!matPair))
                {
                  if(((*iMaterialPairList)->getID1() == fbody0->GetMaterialID()->getID() && (*iMaterialPairList)->getID2() == fbody1->GetMaterialID()->getID())
                    ||((*iMaterialPairList)->getID2() == fbody0->GetMaterialID()->getID() && (*iMaterialPairList)->getID1() == fbody1->GetMaterialID()->getID()))
                          matPair = *iMaterialPairList;
 
                  iMaterialPairList++;
                }
 
                if(matPair != 0)
                {
                  // Construct CollResult
                  MCOLL* mCollResult = new MCOLL();
                  mCollResult->node0 = fbody0->GetParentNode()->GetName();
                  mCollResult->node1 = fbody1->GetParentNode()->GetName();
                  mCollResult->curScene = mscene;
                  mCollResult->matPair = matPair;
 
                  lOverlapStopCp.push_back(mCollResult);
                }
 
                // erase the entry from the map.
                free(bpair);
                bodyPairMap.erase(iBodyPairMap->first);
              }
            }
          }
          // Next couple.
          iBodyPairMap++;
        }
      }
 
      // send callbacks without mutex
      if (GetPhysicWorldEnable())
      {
        unsigned int i = 0;
        for (i = 0; i < lOverlapStartCp.size(); i++)
        {
          MCOLL* mCollResult = lOverlapStartCp[i];
          if (mCollResult->matPair->GetOverlapStartedCallbackActivated())
            getMaterialOverlapStartedCallback(mm, SCOL_PTR mCollResult->matPair, SCOL_PTR mCollResult);
 
          // Release memory.
          delete(mCollResult);
        }
 
        for (i = 0; i < lContactCp.size(); i++)
        {
          MCOLL* mCollResult = lContactCp[i];
          if (mCollResult->matPair->GetContactCallbackActivated())
            getMaterialContactCallback(mm, SCOL_PTR mCollResult->matPair, SCOL_PTR mCollResult);
 
          // Release memory.
          delete(mCollResult);
        }
 
        for (i = 0; i < lOverlapStopCp.size(); i++)
        {
          MCOLL* mCollResult = lOverlapStopCp[i];
          // Send contact end callback to scol virtual machine.
          if (mCollResult->matPair->GetOverlapEndedCallbackActivated())
            getMaterialOverlapEndedCallback(mm, SCOL_PTR mCollResult->matPair, SCOL_PTR mCollResult);
 
          // Release memory.
          delete(mCollResult);
        }
      }
    }
  }
 
  void SPhysicWorld::ResetPhysicWorld()
  {
    bool bLastState = GetPhysicWorldEnable();
    if (bLastState)
    {
      SetPhysicWorldEnable(false);
      GetPhysicWorld()->waitForUpdateToFinish();
    }
 
    //clear physic material pair callback
    ClearBodyPairs();
 
    //not realy needed since these maps are cleared each world update
    ClearContactOverlapStart();
    ClearContactActivated();
 
          // reset world params before next update
          SetResetPhysicWorld(true);
    
    if (bLastState)
    {
      SetPhysicWorldEnable(true);
      UpdatePhysic(0.0f);
    }
 
    SNodeMap::const_iterator iNodeList = mscene->GetNodeList().begin();
    while (iNodeList != mscene->GetNodeList().end())
    {
      SNode* curNode = iNodeList->second;
      SBody* curBody = curNode->GetSceneNodeBody();
 
      if (curBody != 0)
        curBody->Reset();
 
      iNodeList++;
    }
  }
 
  void SPhysicWorld::SetPhysicBodiesInitialState()
  {
    SNodeMap::const_iterator iNodeList = mscene->GetNodeList().begin();
    while(iNodeList != mscene->GetNodeList().end())
          {
                  SNode* curNode = iNodeList->second;
                  SBody* curBody = curNode->GetSceneNodeBody();
 
                  if(curBody!=NULL)
                  {
        curNode->StoreInitialPRS();
                  }
      iNodeList++;
          }
  }
 
  void SPhysicWorld::SetPhysicSolverModel(const int& mod)
  {
    world->setSolverModel(mod);
    solverModel = mod;
  }
 
  int SPhysicWorld::GetPhysicSolverModel()
  {
    return solverModel;
  }
 
  void SPhysicWorld::SetPhysicPlatformArchitecture(const int& mod)
  {
    //world->setPlatformArchitecture(mod);
  }
 
  int SPhysicWorld::GetPhysicPlatformArchitecture()
  {
    Ogre::String aname;
    return world->getPlatformArchitecture(aname);
  }
 
  void SPhysicWorld::SetPhysicGravity(const Ogre::Vector3& gravity)
  {
    mGravity = gravity;
  }
 
  Ogre::Vector3 SPhysicWorld::GetPhysicGravity()
  {
    return mGravity;
  }
 
  void SPhysicWorld::SetPhysicFPS(const int& fps)
  {
    if (fps < 120)
      worldFPS = 120;
    else
      worldFPS = fps;
 
    // high frame skipping make the leap or other synch physic with prerender to be very slow
    if (world != 0)
      world->setUpdateFPS(Ogre::Real(worldFPS), 6);
  }
 
  int SPhysicWorld::GetPhysicFPS()
  {
    return worldFPS;
  }
 
  bool SPhysicWorld::GetResetPhysicWorld()
  {
    return mResetPhysicWorld;
  }
 
  void SPhysicWorld::SetResetPhysicWorld(const bool& state)
  {
    mResetPhysicWorld = state;
  }
 
  SBody* SPhysicWorld::PhysicsRayCast(const Ogre::Vector3& src, const Ogre::Vector3& dir, const Ogre::Real& maxdist, Ogre::Real& hitdistance, Ogre::Vector3& hitnormal)
  {
    OgreNewt::BasicRaycast* ray = new OgreNewt::BasicRaycast(world, src, (src + (dir.normalisedCopy() * maxdist)), true);
    OgreNewt::BasicRaycast::BasicRaycastInfo info = ray->getFirstHit();
    OgreNewt::Body* body = info.getBody();
 
    SBody* sbody = 0;
    if (info.getBody())
    {
      SNode* node = Ogre::any_cast<SNode*>(body->getUserData());
      sbody = node->GetSceneNodeBody();
 
      hitdistance = maxdist * info.getDistance();
      hitnormal = info.getNormal();
    }
    delete ray;
 
    return sbody;
  }
 
  std::vector<MRAYINFO> SPhysicWorld::PhysicsRayCast(const Ogre::Vector3& src, const Ogre::Vector3& dir, const Ogre::Real& maxdist)
  {
    OgreNewt::BasicRaycast* ray = new OgreNewt::BasicRaycast(world, src, (src + (dir.normalisedCopy() * maxdist)), true);
    std::vector<MRAYINFO> lray;
    int nbHits = ray->getHitCount();
 
    for (int i = 0; i < nbHits; i++)
    {
      OgreNewt::BasicRaycast::BasicRaycastInfo info = ray->getInfoAt(i);
      OgreNewt::Body* body = info.getBody();
 
      if (info.getBody())
      {
        MRAYINFO mray;
        SNode* node = Ogre::any_cast<SNode*>(body->getUserData());
        mray.body = node->GetSceneNodeBody();
 
        mray.distance = maxdist * info.getDistance();
        mray.normal = info.getNormal();
        lray.push_back(mray);
      }
    }
    delete ray;
 
    return lray;
  }
 
  //deprecated
  void SPhysicWorld::SetPhysicWorldSize(const Ogre::Vector3& minsize, const Ogre::Vector3& maxsize)
  {
    //world->setWorldSize(minsize, maxsize);
  }
 
  //deprecated
  void SPhysicWorld::GetPhysicWorldSize(Ogre::Vector3& minsize, Ogre::Vector3& maxsize)
  {
    //Ogre::AxisAlignedBox box = world->getWorldSize();
    //minsize = box.getMinimum();
    //maxsize = box.getMaximum();
  }
 
  SMaterialID* SPhysicWorld::CreatePhysicMaterialID(const std::string& materialIdName)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    SMaterialID* matid = new SMaterialID(const_cast<SScene*>(mscene), materialIdName);
    listOfMaterialID.insert(SMaterialIDMap::value_type(materialIdName, matid));
 
    return matid;
  }
 
  void SPhysicWorld::DeletePhysicMaterialID(SMaterialID* matID)
  {
          // Destroy Materials Pair Associated with IT
    SMaterialIDMap::iterator iMaterialIDList = listOfMaterialID.begin();
    while (iMaterialIDList != listOfMaterialID.end())
          {
                  // Getting Material Pair Associated in the Scene
                  SMaterialPair* matPair;
                  matPair = NULL;
                  matPair = getMaterialPairByWorld(this, matID->getID(), iMaterialIDList->second->getID());
      OBJdelTH(mm, SO3PHYSICSMATERIALPAIR, SCOL_PTR matPair);
 
      iMaterialIDList++;
          }
 
          // Replace all Material ID of bodies using it by default
          OgreNewt::Body* curBody = world->getFirstBody();
          while(curBody!=NULL)
          {
                  if(curBody->getMaterialGroupID()->getID() == matID->getID())
                          curBody->setMaterialGroupID(world->getDefaultMaterialID());
 
                  curBody = curBody->getNext();
          }
 
    // Destruction of pointer C
    std::unique_lock<std::mutex> l(mutexUpdate);
 
    SMaterialIDMap::iterator iMaterialIDSearched = listOfMaterialID.find(matID->GetName());
    if(iMaterialIDSearched != listOfMaterialID.end())
      listOfMaterialID.erase(iMaterialIDSearched);
 
    delete matID;
  }
 
  SMaterialPair* SPhysicWorld::CreatePhysicMaterialPair(SMaterialID* mat1, SMaterialID* mat2)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    SMaterialPair* matpair = new SMaterialPair(const_cast<SScene*>(mscene), mat1, mat2);
    listOfPhysicsMaterialPair.insert(matpair);
 
    return matpair;
  }
 
  void SPhysicWorld::DeletePhysicMaterialPair(SMaterialPair* matpair)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    listOfPhysicsMaterialPair.erase(matpair);
    delete matpair;
  }
 
  void SPhysicWorld::LeaveWorldCallBack(OgreNewt::Body* body, int threadIndex)
  {
    /*if(body)
    {
      std::unique_lock<std::mutex> l(mutexUpdate);
      SNode* nodeScol = Ogre::any_cast<SNode*>(body->getUserData());
 
      if(nodeScol != NULL)
            {
                    SBody* b0 = nodeScol->GetSceneNodeBody();
        if (b0 != 0)
          OBJpostEvent(SO3_BODY_LEAVE_WORLD_MSG, SCOL_PTR b0, 0);
      }
    }*/
  }
 
  unsigned int SPhysicWorld::GetMaxBodyPair()
  {
    return maxBodyPair;
  }
 
  bool SPhysicWorld::GetBodyPairExist(SBody* b0, SBody* b1)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    BodyPairMap::iterator iBodyPairSearched = bodyPairMap.find(b0->GetParentNode()->GetName() + "|" + b1->GetParentNode()->GetName());
    if(iBodyPairSearched == bodyPairMap.end())
    {
      iBodyPairSearched = bodyPairMap.find(b1->GetParentNode()->GetName() + "|" + b0->GetParentNode()->GetName());
      if(iBodyPairSearched == bodyPairMap.end())
        return false;
    }
    return true;
  }
 
  void SPhysicWorld::AddBodyPair(std::string name, SBody** pair)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    bodyPairMap.insert(BodyPairMap::value_type(name, pair));
  }
 
  unsigned int SPhysicWorld::GetBodyPairSize()
  {
    return bodyPairMap.size();
  }
 
  void SPhysicWorld::RemoveBodyPair(SBody* body)
  {
    if(world)
    {
      std::unique_lock<std::mutex> l(mutexUpdate);
      // Checking in the body pair list if this body is referenced
      const BodyPairMap bodyMapList = bodyPairMap;
      BodyPairMap::const_iterator iBodyPairSearched = bodyMapList.begin();
      while (iBodyPairSearched != bodyMapList.end())
      {
        SBody** bpair = iBodyPairSearched->second;
        if ((bpair[0] == body) || (bpair[1] == body))
        {
          free(bpair);
          bodyPairMap.erase(iBodyPairSearched->first);
        }
 
        iBodyPairSearched++;
      }
    }
  }
 
  void SPhysicWorld::ClearBodyPairs()
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    const BodyPairMap bodyListCopy = bodyPairMap;
    BodyPairMap::const_iterator iBodyMap = bodyListCopy.begin();
          while (iBodyMap != bodyListCopy.end())
          {
      free(iBodyMap->second);
      bodyPairMap.erase(iBodyMap->first);
      iBodyMap++;
    };
  }
 
  void SPhysicWorld::AddPhysicContraint(SPhysicContraint* contraint)
  {
    physicContraintList.insert(contraint);
  }
 
  void SPhysicWorld::RemovePhysicContraint(SPhysicContraint* contraint)
  {
    physicContraintList.erase(contraint);
    delete(contraint);
  }
 
  PhysicContraintList SPhysicWorld::GetPhysicContraintList()
  {
    return physicContraintList;
  }
 
  void SPhysicWorld::ClearPhysicContraint()
  {
    const PhysicContraintList list = physicContraintList;
    PhysicContraintList::const_iterator it;
    for (it = list.begin(); it != list.end(); ++it)
    {
      delete((*it));
      physicContraintList.erase(it);
    }
  }
 
  void SPhysicWorld::RemoveBodyContraint(SBody* body)
  {
    const PhysicContraintList list = physicContraintList;
    PhysicContraintList::const_iterator it;
    for (it = list.begin(); it != list.end(); ++it)
    {
      if (((*it)->GetParentBody() == body) || ((*it)->GetSonBody() == body))
      {
        if ((*it)->GetParentBody() == body)
        {
          (*it)->SetParentBody(0);
        }
        else
        {
          (*it)->SetSonBody(0);
        }
      }
    }
  }
 
  void SPhysicWorld::ClearContactOverlapStart()
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    const PhysicContactMap lOverlapStart = physicContactOverlapStartMap;
    PhysicContactMap::const_iterator iOverlapStartMap = lOverlapStart.begin();
    while (iOverlapStartMap != lOverlapStart.end())
    {
      MCOLL* mCollResult = iOverlapStartMap->second;
      // Release memory.
      delete(mCollResult);
 
      iOverlapStartMap++;
    }
    physicContactOverlapStartMap.clear();
  }
 
  void SPhysicWorld::AddContactOverlapStart(std::string name, MCOLL* coll)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    physicContactOverlapStartMap.insert(PhysicContactMap::value_type(name, coll));
  }
 
  void SPhysicWorld::ClearContactActivated()
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    const PhysicContactMap lContact = physicContactMap;
    PhysicContactMap::const_iterator iContact = lContact.begin();
    while (iContact != lContact.end())
    {
      MCOLL* mCollResult = iContact->second;
      // Release memory.
      delete(mCollResult);
 
      iContact++;
    }
    physicContactMap.clear();
  }
 
  void SPhysicWorld::SetContactActivated(SBody* b0, SBody* b1, MCOLL* coll)
  {
    std::unique_lock<std::mutex> l(mutexUpdate);
    PhysicContactMap::iterator iBodyContactSearched = physicContactMap.find(b0->GetParentNode()->GetName() + "|" + b1->GetParentNode()->GetName());
    if(iBodyContactSearched == physicContactMap.end())
      iBodyContactSearched = physicContactMap.find(b1->GetParentNode()->GetName() + "|" + b0->GetParentNode()->GetName());
 
    if (iBodyContactSearched != physicContactMap.end())
    {
      delete (iBodyContactSearched->second);
      iBodyContactSearched->second = coll;
    }
    else
    {
      physicContactMap.insert(PhysicContactMap::value_type(b0->GetParentNode()->GetName() + "|" + b1->GetParentNode()->GetName(), coll));
    }
  }
 
  int SPhysicWorld::ImpulsePointCallback(const NewtonBody* const body, void* const userData)
  {
    const OgreNewt::Body* obody = (const OgreNewt::Body*) NewtonBodyGetUserData(body);
    const NewtonWorld* newtonWorld = NewtonBodyGetWorld(body);
    const OgreNewt::World* world = (const OgreNewt::World*) NewtonWorldGetUserData(newtonWorld);
    const NewtonCollision* collision = NewtonBodyGetCollision(body);
 
    MIMPULSEDATA* data = (MIMPULSEDATA*)userData;
    dFloat Ixx;
    dFloat Iyy;
    dFloat Izz;
    dFloat mass;
    NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
    mass = std::max(0.9f, mass);
 
    dMatrix matrix;
    NewtonBodyGetMatrix(body, &matrix[0][0]);
    Ogre::Vector3 contact;
    Ogre::Vector3 normal;
 
          if (NewtonCollisionPointDistance(newtonWorld, &data->position.x, collision, &matrix[0][0], &contact.x, &normal.x, 0))
    {
                  float distance = (contact - data->position).squaredLength();
                  if (distance <= data->radius)
      {
                          Ogre::Vector3 impulse = (normal * (-data->strenght * (1.0f - distance / data->radius))) / mass;
                          NewtonBodyAddImpulse(body, &impulse[0], &contact[0]);
                  }
          }
    return 1;
  }
 
  void SPhysicWorld::ApplyImpulsePoint(Ogre::Vector3 pos, Ogre::Real radius, Ogre::Real strength)
  {
    // an axis-aligned bounding box of the sphere around position with radius
    Ogre::Vector3 vmin = pos - radius;
    Ogre::Vector3 vmax = pos + radius;
 
    MIMPULSEDATA data;
    data.radius = radius * radius;
    data.strenght = strength;
    data.position = pos;
 
    NewtonWorldForEachBodyInAABBDo(world->getNewtonWorld(), &vmin.x, &vmax.x, SPhysicWorld::ImpulsePointCallback, &data);
  }
}