SO3Body.cpp

Go to the documentation of this file.

 
#include "SO3PhysicGraph/SO3Body.h"
#include "SO3PhysicGraph/SO3MaterialID.h"
#include "SO3SceneGraph/SO3NodeScol.h"
#include "SO3SceneGraph/SO3Bone.h"
#include "SO3SceneGraph/SO3Scene.h"
#include "SO3SceneGraph/SO3Skeleton.h"
 
namespace SO3
{
 
  SBody::SBody()
  {
    // Forbiden, private.
  }
 
  SBody::SBody(SNode* node, SShape* shape)
  {
    mScene = node->GetParentScene();
    mNode = node;
    mShape = shape;
    mUpJoint = 0;
    mDebugBody = NULL;
    mIgnoreCollision = false;
    mApplyGravity = true;
    HasBuoyancy = false;
    mEnable = true;
    mMass = 0.0;
    haveToMove = false;
    haveToRotate = false;
    moveStiffness = 0.3f;
    rotateStiffness = 0.3f;
    force = Ogre::Vector3::ZERO;
    torque = Ogre::Vector3::ZERO;
    localForce = Ogre::Vector3::ZERO;
    globalForce = Ogre::Vector3::ZERO;
    globalLocationForce = Ogre::Vector3::ZERO;
    localLocationForce = Ogre::Vector3::ZERO;
    constantForce = Ogre::Vector3::ZERO;
    constantTorque = Ogre::Vector3::ZERO;
    mFluidWaterToVolume = 0.9f;
    mFluidViscosity = 0.99f;
    isFluid = false;
    materialId = 0;
 
    //protect async updates
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    O3Body = new OgreNewt::Body(mScene->GetPhysicsWorld()->GetPhysicWorld(), mShape->GetOgreNewtCollisionPtr());
    O3Body->freeze();
    O3Body->setAutoSleep(1);
 
    //Why not directly this ?
    O3Body->setUserData((Ogre::Any) node);
 
    //$BB attach node call NodeUpdate so we have to keep the previous position
    Ogre::Quaternion prevOrientation = node->GetGlobalOrientation();
    Ogre::Vector3 prevPosition = node->GetGlobalPosition();
 
    //move body tu current position
    SetPositionOrientation(prevPosition, prevOrientation);
 
    O3Body->setCenterOfMass(O3Body->calculateOffset());
 
    // attach node to body
    O3Body->attachNode(node->GetOgreSceneNodePointer());
 
    // set default physic material
    O3Body->setMaterialGroupID(mScene->GetPhysicsWorld()->GetPhysicWorld()->getDefaultMaterialID());
 
    //$BB add the new initial position to node
    //mNode->StoreInitialPRS();
 
    O3Body->setCustomForceAndTorqueCallback<SBody>(&SBody::BodyAddForceCallback, this);
 
    O3Body->setNodeUpdateNotify<SBody>(&SBody::BodyUpdateCallback, this);
  }
 
  SBody::~SBody()
  {
    SAFE_DELETE(mDebugBody);
 
    //protect async updates
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    RemoveUpJoint();
 
    //remove associated physic joints and contacts
    if (mScene->GetPhysicsWorld())
    {
      mScene->GetPhysicsWorld()->RemoveBodyPair(this);
      mScene->GetPhysicsWorld()->RemoveBodyContraint(this);
    }
 
    // Delete newton object if not already done by newton
    SO3_SAFE_DELETE(O3Body);
    SO3_SAFE_DELETE(mShape);
    mNode = 0;
    mScene = 0;
  }
 
  SShape* SBody::GetShape()
  {
    return mShape;
  }
 
  Ogre::Vector3 SBody::CalculateBodyOffset()
  {
    return O3Body->calculateOffset();
  }
 
  void SBody::Reset()
  {
    haveToRotate = false;
    haveToMove = false;
    if (mNode)
    {
      if (mNode->GetNodeType() == SNode::ENTITY_TYPE_ID)
      {
        SBone* bone = static_cast<SBone*>(mNode);
        bone->ResetToInitialPRS();
      }
      else
      {
        mNode->ResetToInitialPRS();
      }
      
      SetVelocity(Ogre::Vector3::ZERO);
      SetOmega(Ogre::Vector3::ZERO);
      SetTorque(Ogre::Vector3::ZERO);
      SetForce(Ogre::Vector3::ZERO);
      SetConstantForce(Ogre::Vector3::ZERO);
      SetConstantTorque(Ogre::Vector3::ZERO);
      SetGlobalForce(Ogre::Vector3::ZERO);
 
      if (mNode->GetNodeType() == SNode::ENTITY_TYPE_ID)
      {
        SEntity* curEntity = static_cast<SEntity*> (mNode);
        SSkeleton* skeleton = curEntity->GetSkeleton();
        if (skeleton)
        {
          SBoneMap rootBoneList = skeleton->GetRootBones();
          SBoneMap::iterator iRootBones = rootBoneList.begin();
          while (iRootBones != rootBoneList.end())
          {
            iRootBones->second->ResetNode();
            iRootBones++;
          }
        }
      }
 
      SNodeMap childNodeList = mNode->GetChildrenNodes();
      SNodeMap::iterator iChildNodeList = childNodeList.begin();
      while (iChildNodeList != childNodeList.end())
      {
        iChildNodeList->second->ResetNode();
        iChildNodeList++;
      }
    }
  }
 
  void SBody::BodyUpdateCallback(OgreNewt::Body* body)
  {
    if (mNode)
    {
      //if (GetMass() <= 0.0f)
        //return;
 
      Ogre::Vector3 pos;
      Ogre::Quaternion orient;
      body->getVisualPositionOrientation(pos, orient);
 
      mNode->SetGlobalPosition(pos, false);
      mNode->SetGlobalOrientation(orient, false);
      
      if (mNode->GetNodeType() == SNode::ENTITY_TYPE_ID)
      {
        SEntity* curEntity = static_cast<SEntity*> (mNode);
        SSkeleton* skeleton = curEntity->GetSkeleton();
        if (skeleton)
        {
          SBoneMap rootBoneList = skeleton->GetRootBones();
          SBoneMap::iterator iRootBones = rootBoneList.begin();
          while (iRootBones != rootBoneList.end())
          {
            iRootBones->second->UpdateNodeFromBody();
            iRootBones++;
          }
        }
      }
 
      SNodeMap childNodeList = mNode->GetChildrenNodes();
      SNodeMap::iterator iChildNodeList = childNodeList.begin();
      while (iChildNodeList != childNodeList.end())
      {
        iChildNodeList->second->UpdateNodeFromBody();
        iChildNodeList++;
      }
    }
    haveToRotate = false;
    haveToMove = false;
  }
 
  void SBody::BodyAddForceCallback(OgreNewt::Body* body, float timeStep, int threadIndex)
  {
    //apply a simple gravity force.
    Ogre::Real mass;
    Ogre::Vector3 inertia;
 
    body->getMassMatrix(mass, inertia);
    if (!HasBuoyancy && (mApplyGravity && mNode != 0) && (mNode->GetParentScene() != 0) && (mass != 0.0))
    {
      Ogre::Vector3 gravityForce = mNode->GetParentScene()->GetPhysicsWorld()->GetPhysicGravity() * mass;
      body->addForce(gravityForce);
    }
 
    //OgreNewt accumulated forces
    /*
    int nth = 0;
    for (std::vector<std::pair<Ogre::Vector3, Ogre::Vector3> >::const_iterator it = body->m_accumulatedGlobalForces.begin(); it != body->m_accumulatedGlobalForces.end(); it++)
    {
      //Ogre::LogManager::getSingleton().getDefaultLog()->logMessage("# [" + Ogre::StringConverter::toString(++nth) + "] force " + Ogre::StringConverter::toString(it->first) + " at " + Ogre::StringConverter::toString(it->second) + " to " + body->getOgreNode()->getName());
 
      body->addGlobalForce(it->first, it->second);
    }
 
    body->m_accumulatedGlobalForces.clear();
    */
 
    body->addForce(force);
    force = Ogre::Vector3::ZERO;
 
    body->addTorque(constantTorque);
 
    body->addTorque(torque);
    torque = Ogre::Vector3::ZERO;
 
    body->addLocalForce(localForce, localLocationForce);
    localForce = Ogre::Vector3::ZERO;
    localLocationForce = Ogre::Vector3::ZERO;
 
    body->addForce(constantForce);
 
    body->addGlobalForce(globalForce, globalLocationForce);
    globalForce = Ogre::Vector3::ZERO;
    globalLocationForce = Ogre::Vector3::ZERO;
 
    //hack to wakeup frozen bodies
    if (body->isFreezed() && (haveToMove || haveToRotate || ((O3Body->getVelocity().length() > 0.0f) && (O3Body->getOmega().length() > 0.0f) && (O3Body->getForce().length() > 0.0f) && (O3Body->getForceAcceleration().length() > 0.0f))))
      body->unFreeze();
 
    if (haveToMove)
    {
      O3Body->MoveToPosition(destPosition, moveStiffness, timeStep);
    }
 
    if (haveToRotate)
    {
      O3Body->RotateToOrientation(destOrientation, rotateStiffness, timeStep);
    }
  }
 
  void SBody::BuoyancyForce(OgreNewt::Body* visitor)
  {
    dFloat Ixx;
    dFloat Iyy;
    dFloat Izz;
    dFloat mass;
 
    NewtonBodyGetMassMatrix(visitor->getNewtonBody(), &mass, &Ixx, &Iyy, &Izz);
    if (mass > 0.0f)
    {
      dMatrix matrix;
      dMatrix offmatrix;
      dVector cog;
      dVector accelPerUnitMass;
      dVector torquePerUnitMass;
 
      //gravity
      Ogre::Vector3 gravity(Ogre::Vector3::ZERO);
      if (mApplyGravity && mNode != 0 && mNode->GetParentScene() != 0)
        gravity = mNode->GetParentScene()->GetPhysicsWorld()->GetPhysicGravity();
 
      //get plane from the top of trigger body
      dVector plane(0.0f, 1.0f, 0.0f, 1.5f);
      NewtonCollision* const pcollision = NewtonBodyGetCollision(O3Body->getNewtonBody());
      NewtonBodyGetMatrix(O3Body->getNewtonBody(), &offmatrix[0][0]);
 
      dVector minABB;
      dVector maxABB;
      NewtonCollisionCalculateAABB(pcollision, &offmatrix[0][0], &minABB[0], &maxABB[0]);
 
      Ogre::Quaternion pquat = mNode->GetOgreSceneNodePointer()->_getDerivedOrientation();
      Ogre::Vector3 dir = pquat * Ogre::Vector3::UNIT_Y;
 
      //scale visitor values
      NewtonCollision* const collision = NewtonBodyGetCollision(visitor->getNewtonBody());
      NewtonBodyGetMatrix(visitor->getNewtonBody(), &matrix[0][0]);
 
      NewtonBodyGetCentreOfMass(visitor->getNewtonBody(), &cog[0]);
      cog = matrix.TransformVector(cog);
 
      //plane;
      plane.m_x = dir.x;
      plane.m_y = dir.y;
      plane.m_z = dir.z;
      plane.m_w = dir.dotProduct(-(Ogre::Vector3(maxABB.m_x, maxABB.m_y, maxABB.m_z) * dir));
 
      dFloat shapeVolume = NewtonConvexCollisionCalculateVolume(collision);
      dFloat fluidDentity = std::max((mass / shapeVolume) * mFluidWaterToVolume, std::min((mass / shapeVolume) / (1.0f - mFluidWaterToVolume), mass / (mFluidWaterToVolume * shapeVolume * ((mass * 0.01f) / shapeVolume))));
      //dFloat fluidDentity = mass / (mFluidWaterToVolume * shapeVolume * ((mass * 0.01f) / shapeVolume));
      //dFloat fluidDentity = (mass / shapeVolume) * mFluidWaterToVolume;
      //dFloat fluidDentity = (mass / shapeVolume) / (1.0f - mFluidWaterToVolume);
 
      NewtonConvexCollisionCalculateBuoyancyAcceleration(collision, &matrix[0][0], &cog[0], &gravity.x, &plane[0], fluidDentity, mFluidViscosity, &accelPerUnitMass[0], &torquePerUnitMass[0]);
      if (accelPerUnitMass[0] != 0.0 || accelPerUnitMass[1] != 0.0 || accelPerUnitMass[2] != 0.0 || torquePerUnitMass[0] != 0.0 || torquePerUnitMass[1] != 0.0 || torquePerUnitMass[2] != 0.0)
      {
        dVector omega;
        NewtonBodyGetOmega(visitor->getNewtonBody(), &omega[0]);
        omega = omega.Scale(mFluidViscosity);
        NewtonBodySetOmega(visitor->getNewtonBody(), &omega[0]);
        dVector vel;
        NewtonBodyGetVelocity(visitor->getNewtonBody(), &vel[0]);
        vel = vel.Scale(mFluidViscosity);
        NewtonBodySetVelocity(visitor->getNewtonBody(), &vel[0]);
 
        NewtonBodyAddForce(visitor->getNewtonBody(), &accelPerUnitMass[0]);
        NewtonBodyAddTorque(visitor->getNewtonBody(), &torquePerUnitMass[0]);
        HasBuoyancy = true;
      }
      else
        HasBuoyancy = false;
    }
  }
 
  OgreNewt::Body* SBody::getOgreNewtBodyPointer()
  {
    return O3Body;
  }
 
  void SBody::SetFluidState(bool state)
  {
    if (isFluid == state)
      return;
 
    isFluid = state;
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    RemoveUpJoint();
 
    int autosleep = O3Body->getAutoSleep();
    Ogre::Vector3 centerofmass = O3Body->getCenterOfMass();
 
    // Delete newton object
    SO3_SAFE_DELETE(O3Body);
 
    O3Body = new OgreNewt::Body(mScene->GetPhysicsWorld()->GetPhysicWorld(), mShape->GetOgreNewtCollisionPtr(), 0, state);
    //$BB attach node call NodeUpdate so we have to keep the previous position
    Ogre::Quaternion prevOrientation = mNode->GetGlobalOrientation();
    Ogre::Vector3 prevPosition = mNode->GetGlobalPosition();
 
    //Why not directly this ?
    O3Body->setUserData((Ogre::Any) mNode);
 
    //move body tu current position
    SetPositionOrientation(prevPosition, prevOrientation);
 
    O3Body->setCenterOfMass(O3Body->calculateOffset());
 
    // attach node to body
    O3Body->attachNode(mNode->GetOgreSceneNodePointer());
 
    // set default physic material
    if (materialId != 0)
      O3Body->setMaterialGroupID(materialId->getOgreNewtMaterialID());
    else
      O3Body->setMaterialGroupID(mScene->GetPhysicsWorld()->GetPhysicWorld()->getDefaultMaterialID());
 
    O3Body->setCustomForceAndTorqueCallback<SBody>(&SBody::BodyAddForceCallback, this);
 
    O3Body->setMass(mMass);
    O3Body->setAutoSleep(autosleep);
    O3Body->setCenterOfMass(centerofmass);
 
    SetScale(mNode->GetGlobalScale());
 
    O3Body->setNodeUpdateNotify<SBody>(&SBody::BodyUpdateCallback, this);
 
    if (state)
    {
      O3Body->setTriggerInsideCallback<SBody>(&SBody::BuoyancyForce, this);
    }
  }
 
  bool SBody::GetFluidState()
  {
    return isFluid;
  }
 
  void SBody::SetFluidVolumeRatio(float waterToVolume)
  {
    mFluidWaterToVolume = waterToVolume;
  }
 
  void SBody::SetFluidViscosity(float viscosity)
  {
    mFluidViscosity = viscosity;
  }
 
  float SBody::GetFluidWaterToVolumeRatio()
  {
    return mFluidWaterToVolume;
  }
 
  float SBody::GetFluidViscosity()
  {
    return mFluidViscosity;
  }
 
  Ogre::Real SBody::GetMass()
  {
    return mMass;
  }
 
  bool SBody::GetFreezeState()
  {
    return O3Body->isFreezed();
  }
 
  void SBody::SetGravityEnable(bool state)
  {
    mApplyGravity = state;
  }
 
  bool SBody::GetGravityEnable()
  {
    return mApplyGravity;
  }
 
  Ogre::Vector3 SBody::GetForce()
  {
    return force;
  }
 
  Ogre::Vector3 SBody::GetTorque()
  {
    return torque;
  }
 
  Ogre::Vector3 SBody::GetLocalForce()
  {
    return localForce;
  }
 
  Ogre::Vector3 SBody::GetGlobalForce()
  {
    return globalForce;
  }
 
  Ogre::Vector3 SBody::GetGlobalLocationForce()
  {
    return globalLocationForce;
  }
 
  Ogre::Vector3 SBody::GetLocalLocationForce()
  {
    return localLocationForce;
  }
 
  void SBody::SetMass(const Ogre::Real& mass)
  {
    mMass = mass;
    O3Body->setMass(mMass);
  }
 
  void SBody::SetMassMatrix(const Ogre::Real& mass, const Ogre::Vector3& inertial)
  {
    mMass = mass;
    O3Body->setMassMatrix(mMass, mMass * inertial);
  }
 
  void SBody::SetMaterialID(SMaterialID* matID)
  {
    materialId = matID;
    if (matID != 0)
      O3Body->setMaterialGroupID(matID->getOgreNewtMaterialID());
  }
 
  const std::string SBody::GetMaterialName()
  {
    if (materialId == 0)
      return "default";
    else
      return materialId->GetName();
  }
 
  const OgreNewt::MaterialID* SBody::GetMaterialID()
  {
    return O3Body->getMaterialGroupID();
  }
 
  const SMaterialID* SBody::GetMaterial()
  {
    return materialId;
  }
 
  void SBody::SetFreezeState(const bool& state)
  {
    if (state)
    {
      O3Body->freeze();
    }
    else
    {
      O3Body->unFreeze();
    }
  }
 
  void SBody::SetForce(const Ogre::Vector3& mForce)
  {
    force = mForce;
  }
 
  void SBody::SetTorque(const Ogre::Vector3& mTorque)
  {
    torque = mTorque;
  }
 
  void SBody::SetOmega(const Ogre::Vector3& omega)
  {
    O3Body->setOmega(omega);
  }
 
  Ogre::Vector3 SBody::GetOmega()
  {
    return O3Body->getOmega();
  }
 
  void SBody::SetConstantForce(const Ogre::Vector3& mForce)
  {
    constantForce = mForce;
  }
 
  void SBody::SetConstantTorque(const Ogre::Vector3& mTorque)
  {
    constantTorque = mTorque;
  }
 
  void SBody::AddConstantForce(const Ogre::Vector3& mForce)
  {
    constantForce = constantForce + mForce;
  }
 
  void SBody::AddConstantTorque(const Ogre::Vector3& mTorque)
  {
    constantTorque = constantTorque + mTorque;
  }
 
  Ogre::Vector3 SBody::GetConstantForce()
  {
    return constantForce;
  }
 
  Ogre::Vector3 SBody::GetConstantTorque()
  {
    return constantTorque;
  }
 
  void SBody::SetLocalForce(const Ogre::Vector3& mLocalForce)
  {
    localForce = mLocalForce;
  }
 
  void SBody::SetGlobalForce(const Ogre::Vector3& mGlobalForce)
  {
    globalForce = mGlobalForce;
  }
 
  void SBody::SetGlobalLocationForce(const Ogre::Vector3& mGlobalLocationForce)
  {
    globalLocationForce = mGlobalLocationForce;
  }
 
  void SBody::SetLocalLocationForce(const Ogre::Vector3& mLocalLocationForce)
  {
    localLocationForce = mLocalLocationForce;
  }
 
  void SBody::SetPositionOrientation(const Ogre::Vector3& mDerivedPos, const Ogre::Quaternion& mDerivedOrientation)
  {
    haveToMove = false;
    O3Body->setPositionOrientation(mDerivedPos, mDerivedOrientation);
  }
 
  void SBody::UpdatePositionOrientation()
  {
    haveToMove = false;
    O3Body->setPositionOrientationFromNode();
  }
 
  void SBody::UpdatePositionOrientation(const Ogre::Vector3& mDerivedPos, const Ogre::Quaternion& mDerivedOrientation)
  {
    haveToMove = false;
    O3Body->updatePositionOrientation(mDerivedPos, mDerivedOrientation);
  }
 
  void SBody::SetScale(const Ogre::Vector3& mDerivedScale)
  {
#ifndef USE_COLLISION_NOSCALE
    if (mShape)
    {
      Ogre::Vector3 scale = mDerivedScale * mShape->GetBaseScale();
      Ogre::Vector3 offset = mShape->GetComputedOffset() * mDerivedScale;
      O3Body->setScale(mShape->GetBaseRotation() * scale, mShape->GetBaseRotation(), offset);
    }
#endif
  }
 
  void SBody::SetMatrix(const Ogre::Vector3& mDerivedPos, const Ogre::Quaternion& mDerivedOrientation)
  {
    float matrix[16];
    OgreNewt::Converters::QuatPosToMatrix(mDerivedOrientation, mDerivedPos, &matrix[0]);
    NewtonBodySetMatrix(O3Body->getNewtonBody(), &matrix[0]);
  }
 
  void SBody::SetVelocity(const Ogre::Vector3& velocity)
  {
    O3Body->setVelocity(velocity);
  }
 
  Ogre::Vector3 SBody::GetVelocity()
  {
    return O3Body->getVelocity();
  }
 
  void SBody::UpdateMatrix()
  {
    SetMatrix(mNode->GetGlobalPosition(), mNode->GetGlobalOrientation());
  }
 
  void SBody::AddImpulse(const Ogre::Vector3& delta, const Ogre::Vector3& pos)
  {
    O3Body->addImpulse(delta, pos);
  }
 
  SNode* SBody::GetParentNode()
  {
    return mNode;
  }
 
  void SBody::SetUpJoint(const Ogre::Vector3& vec)
  {
    SO3_SAFE_DELETE(mUpJoint);
    mUpJoint = new OgreNewt::UpVector(O3Body, vec);
  }
 
  void SBody::RemoveUpJoint()
  {
    SO3_SAFE_DELETE(mUpJoint);
  }
 
  void SBody::SetAngularDamping(const Ogre::Vector3& vec)
  {
    O3Body->setAngularDamping(vec);
  }
 
  Ogre::Vector3 SBody::GetAngularDamping()
  {
    return O3Body->getAngularDamping();
  }
 
  void SBody::SetCenterOfMass(const Ogre::Vector3& vec)
  {
    O3Body->setCenterOfMass(vec);
  }
 
  Ogre::Vector3 SBody::GetCenterOfMass()
  {
    return O3Body->getCenterOfMass();
  }
 
  void SBody::SetLinearDamping(const Ogre::Real& ang)
  {
    O3Body->setLinearDamping(ang);
  }
 
  Ogre::Real SBody::GetLinearDamping()
  {
    return O3Body->getLinearDamping();
  }
 
  void SBody::SetAutoSleep(const bool& enable)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    O3Body->setAutoSleep(enable ? 1 : 0);
  }
 
  bool SBody::GetAutoSleep()
  {
    return O3Body->getAutoSleep() ? true : false;
  }
 
  void SBody::EnableSimulation(const bool& enable)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    mEnable = enable;
    O3Body->enableSimulation(mEnable);
  }
 
  bool SBody::GetSimulationState()
  {
    return mEnable;
  }
 
  void SBody::SetContinuousCollisionMode(const bool& enable)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    O3Body->setContinuousCollisionMode(enable ? 1 : 0);
  }
 
  bool SBody::GetContinuousCollisionMode()
  {
    return O3Body->getContinuousCollisionMode() ? true : false;
  }
 
  void SBody::GetInvMass(Ogre::Real& mass, Ogre::Vector3& inertia)
  {
    O3Body->getInvMass(mass, inertia);
  }
 
  void SBody::GetMassMatrix(Ogre::Real& mass, Ogre::Vector3& inertia)
  {
    O3Body->getMassMatrix(mass, inertia);
  }
 
  void SBody::MoveToPosition(const Ogre::Vector3 &destPos, const Ogre::Real &stiffness)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    moveStiffness = stiffness;
    destPosition = destPos;
    haveToMove = true;
  }
 
  void SBody::RotateToOrientation(const Ogre::Quaternion &destQuat, const Ogre::Real &stiffness)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    rotateStiffness = stiffness;
    destOrientation = destQuat;
    haveToRotate = true;
  }
 
  bool SBody::GetSleepState()
  {
    //$BB pass from newton since the function is not yet available in OgreNewt
    return NewtonBodyGetSleepState(O3Body->getNewtonBody()) ? true : false;
  }
 
  void SBody::GetPositionOrientation(Ogre::Vector3& pos, Ogre::Quaternion& orient)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    O3Body->getPositionOrientation(pos, orient);
  }
 
  void SBody::SetType(const int& value)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    O3Body->setType(value);
  }
 
  int SBody::GetType()
  {
    return O3Body->getType();
  }
 
  void SBody::SetIgnoreCollision(const bool& state)
  {
    mScene->GetPhysicsWorld()->GetPhysicWorld()->waitForUpdateToFinish();
    mIgnoreCollision = state;
  }
 
  bool SBody::GetIgnoreCollision()
  {
    return mIgnoreCollision;
  }
 
  void SBody::newtonPerPoly(void* userData, int vertexCount, const float* faceVertec, int id)
  {
    Ogre::ManualObject* lines = (Ogre::ManualObject*)userData;
    Ogre::Vector3 p0, p1;
 
    Ogre::Vector3 invscale = Ogre::Vector3::UNIT_SCALE / lines->getParentNode()->_getDerivedScale();
    if (vertexCount < 2)
      return;
 
    int i = vertexCount - 1;
    p0 = Ogre::Vector3(faceVertec[(i * 3) + 0], faceVertec[(i * 3) + 1], faceVertec[(i * 3) + 2]) * invscale;
 
    for (i = 0; i < vertexCount; i++)
    {
      p1 = Ogre::Vector3(faceVertec[(i * 3) + 0], faceVertec[(i * 3) + 1], faceVertec[(i * 3) + 2]) * invscale;
 
      lines->position(p0);
      lines->position(p1);
 
      p0 = p1;
    }
  }
 
  void SBody::ShowDebug(bool state)
  {
 
    if (!state && mDebugBody)
    {
      if (mNode->GetNodeType() == SNode::BONE_TYPE_ID)
      {
        SBone* boneFather = static_cast <SBone*> (mNode);
        boneFather->DetachFromBone(mDebugBody);
      }
      else
      {
        mNode->GetOgreSceneNodePointer()->detachObject(mDebugBody);
      }
 
      SAFE_DELETE(mDebugBody);
    }
    else if (state && !mDebugBody)
    {
      mDebugBody = new Ogre::ManualObject("SO3BODY_" + mNode->GetName());
 
      if (mNode->GetNodeType() == SNode::BONE_TYPE_ID)
      {
        SBone* boneFather = static_cast <SBone*> (mNode);
        boneFather->AttachToBone(mDebugBody);
      }
      else
      {
        mNode->GetOgreSceneNodePointer()->attachObject(mDebugBody);
      }
 
      mDebugBody->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
      mDebugBody->colour(Ogre::ColourValue::White);
 
      float matrix[16];
      OgreNewt::Converters::QuatPosToMatrix(Ogre::Quaternion::IDENTITY, Ogre::Vector3::ZERO, &matrix[0]);
 
      NewtonCollisionForEachPolygonDo(NewtonBodyGetCollision(O3Body->getNewtonBody()), &matrix[0], newtonPerPoly, mDebugBody);
      mDebugBody->end();
      mDebugBody->setCastShadows(false);
    }
  }
 
}