SO3Engine/SO3PhysicGraph/SO3Body.cpp

Go to the documentation of this file.

#include "SO3Body.h"
#include "..\SO3SceneGraph\SO3NodeScol.h"
#include "..\SO3SceneGraph\SO3Scene.h"

namespace SO3
{

SBody::SBody()
{
  // Forbiden, private.
}

SBody::SBody(SNode* node, SShape* shape)
{
  mScene = node->GetParentScene();
  mNode = node;
  mShape = shape;
  mUpJoint = 0;

  mMass = 0.0;

  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;

  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(mShape->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);
}

SBody::~SBody()
{
  RemoveUpJoint();

  if(mScene->GetPhysicsWorld()->GetPhysicWorld())
  {
    // Checking in the body pair list if this body is referenced
    const BodyPairMap bodyMapList = mScene->GetPhysicsWorld()->bodyPairMap;
    BodyPairMap::const_iterator iBodyPairSearched = bodyMapList.begin();
    while (iBodyPairSearched != bodyMapList.end())
    {
      SBody** bpair = iBodyPairSearched->second;
      if ((bpair[0] == this) || (bpair[1] == this))
      {
        free(bpair);
        mScene->GetPhysicsWorld()->bodyPairMap.erase(iBodyPairSearched->first);
      }

      iBodyPairSearched++;
    }

    // Delete newton object if not already done by newton
    SAFE_DELETE(O3Body);
  }
  
        mNode = 0;
        mScene = 0;
}

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(mNode!=NULL)
  {
    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->addLocalForce(constantForce, Ogre::Vector3::ZERO);

  body->addGlobalForce(globalForce, globalLocationForce);
  globalForce = Ogre::Vector3::ZERO;
  globalLocationForce = Ogre::Vector3::ZERO;
}

OgreNewt::Body* SBody::getOgreNewtBodyPointer() 
{
        return O3Body;
}

Ogre::Real SBody::GetMass() 
{
        return mMass;
}

bool SBody::GetFreezeState() 
{
  return O3Body->isFreezed();
}

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(Ogre::Real mass) 
{
        mMass = mass;
  O3Body->setMassMatrix(mMass, mMass * mShape->CalculateInertialMatrix());
}

void SBody::SetMassMatrix(Ogre::Real mass, Ogre::Vector3 inertial) 
{
        mMass = mass;
  O3Body->setMassMatrix(mMass, mMass * inertial);
}


void SBody::SetMaterialID(OgreNewt::MaterialID* mMatID) 
{
  O3Body->setMaterialGroupID(mMatID);
}

const OgreNewt::MaterialID* SBody::GetMaterialID() 
{
  return O3Body->getMaterialGroupID();
}

void SBody::SetFreezeState(bool state) 
{
  if (state)
  {
    O3Body->freeze();
  }
  else
  {
    O3Body->unFreeze();
  }
}

void SBody::SetForce(Ogre::Vector3 mForce) 
{
        force = mForce;
}

void SBody::SetTorque(Ogre::Vector3 mTorque) 
{
        torque = mTorque;
}

void SBody::SetOmega(Ogre::Vector3 omega)
{
  O3Body->setOmega(omega);
}

Ogre::Vector3 SBody::GetOmega()
{
  return O3Body->getOmega();
}

void SBody::SetConstantForce(Ogre::Vector3 mForce)
{
  constantForce = mForce;
}

void SBody::SetConstantTorque(Ogre::Vector3 mTorque)
{
  constantTorque = mTorque;
}

void SBody::AddConstantForce(Ogre::Vector3 mForce)
{
  constantForce = constantForce + mForce;
}

void SBody::AddConstantTorque(Ogre::Vector3 mTorque)
{
  constantTorque = constantTorque + mTorque;
}

Ogre::Vector3 SBody::GetConstantForce()
{
  return constantForce;
}

Ogre::Vector3 SBody::GetConstantTorque()
{
  return constantTorque;
}

void SBody::SetLocalForce(Ogre::Vector3 mLocalForce) 
{
        localForce = mLocalForce;
}

void SBody::SetGlobalForce(Ogre::Vector3 mGlobalForce) 
{
        globalForce = mGlobalForce;
}

void SBody::SetGlobalLocationForce(Ogre::Vector3 mGlobalLocationForce) 
{
        globalLocationForce = mGlobalLocationForce;
}

void SBody::SetLocalLocationForce(Ogre::Vector3 mLocalLocationForce) 
{
        localLocationForce = mLocalLocationForce;
}

void SBody::SetPositionOrientation(Ogre::Vector3 mDerivedPos, Ogre::Quaternion mDerivedOrientation)
{
  O3Body->setPositionOrientation(mDerivedPos, mDerivedOrientation);
}

void SBody::UpdatePositionOrientation()
{
  O3Body->setPositionOrientationFromNode();
}

void SBody::SetMatrix(Ogre::Vector3 mDerivedPos, Ogre::Quaternion mDerivedOrientation)
{
        float matrix[16];
  OgreNewt::Converters::QuatPosToMatrix(mDerivedOrientation, mDerivedPos, &matrix[0]);
        NewtonBodySetMatrix(O3Body->getNewtonBody(), &matrix[0]);
}

void SBody::SetVelocity(Ogre::Vector3 velocity)
{
  O3Body->setVelocity(velocity);
}

Ogre::Vector3 SBody::GetVelocity()
{
  return O3Body->getVelocity();
}

void SBody::UpdateMatrix()
{
  SetMatrix(mNode->GetGlobalPosition(), mNode->GetGlobalOrientation());
}

void SBody::AddImpulse(Ogre::Vector3 delta, Ogre::Vector3 pos)
{
  O3Body->addImpulse(delta, pos);
}

SNode* SBody::GetParentNode()
{
  return mNode;
}

void SBody::SetUpJoint(Ogre::Vector3 vec)
{
  SAFE_DELETE(mUpJoint);
  mUpJoint = new OgreNewt::UpVector(O3Body, vec);
}

void SBody::RemoveUpJoint()
{
  SAFE_DELETE(mUpJoint);
}

void SBody::SetAngularDamping(Ogre::Vector3 vec)
{
  O3Body->setAngularDamping(vec);
}

Ogre::Vector3 SBody::GetAngularDamping()
{
  return O3Body->getAngularDamping();
}

void SBody::SetCenterOfMass(Ogre::Vector3 vec)
{
  O3Body->setCenterOfMass(vec);
}

Ogre::Vector3 SBody::GetCenterOfMass()
{
  return O3Body->getCenterOfMass();
}

void SBody::SetLinearDamping(Ogre::Real ang)
{
  O3Body->setLinearDamping(ang);
}

Ogre::Real SBody::GetLinearDamping()
{
  return O3Body->getLinearDamping();
}

void SBody::SetAutoSleep(bool enable)
{
  O3Body->setAutoSleep(enable ? 1 : 0);
}

bool SBody::GetAutoSleep()
{
  return O3Body->getAutoSleep() ? true : false;
}

void SBody::SetContinuousCollisionMode(bool enable)
{
  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);
}

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)
{
  O3Body->getPositionOrientation(pos, orient);
}

void SBody::SetType(int value)
{
  O3Body->setType(value);
}

int SBody::GetType()
{
  return O3Body->getType();
}

}

---