Logo Search packages:      
Sourcecode: blender version File versions

CcdPhysicsController.cpp

/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/

#include "CcdPhysicsController.h"

#include "Dynamics/RigidBody.h"
#include "PHY_IMotionState.h"
#include "BroadphaseCollision/BroadphaseProxy.h"
#include "BroadphaseCollision/BroadphaseInterface.h"
#include "CollisionShapes/ConvexShape.h"
#include "CcdPhysicsEnvironment.h"
#include "SimdTransformUtil.h"

#include "CollisionShapes/SphereShape.h"
#include "CollisionShapes/ConeShape.h"

class BP_Proxy;

///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class

//'temporarily' global variables
float gDeactivationTime = 2.f;
bool  gDisableDeactivation = false;

float gLinearSleepingTreshold = 0.4f;
float gAngularSleepingTreshold = 1.0f;

#include "Dynamics/MassProps.h"

SimdVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
:m_cci(ci)
{
      m_collisionDelay = 0;
      m_newClientInfo = 0;
      
      m_MotionState = ci.m_MotionState;

      
      
      CreateRigidbody();
      

      
      #ifdef WIN32
      if (m_body->getInvMass())
            m_body->setLinearVelocity(startVel);
      #endif

}

SimdTransform     CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
{
      SimdTransform trans;
      float tmp[3];
      motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
      trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));

      SimdQuaternion orn;
      motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
      trans.setRotation(orn);
      return trans;

}

void CcdPhysicsController::CreateRigidbody()
{

      SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);

      MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);

      m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
      m_body->m_collisionShape = m_cci.m_collisionShape;
      

      //
      // init the rigidbody properly
      //
      
      m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
      //setMassProps this also sets collisionFlags
      m_body->m_collisionFlags = m_cci.m_collisionFlags;
      
      m_body->setGravity( m_cci.m_gravity);
      m_body->setDamping(m_cci.m_linearDamping, m_cci.m_angularDamping);
      m_body->setCenterOfMassTransform( trans );


}

CcdPhysicsController::~CcdPhysicsController()
{
      //will be reference counted, due to sharing
      m_cci.m_physicsEnv->removeCcdPhysicsController(this);
      delete m_MotionState;
      delete m_body;
}

            /**
                  SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
            */
00115 bool        CcdPhysicsController::SynchronizeMotionStates(float time)
{
      //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)

      if (!m_body->IsStatic())
      {
            const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
            m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
            
            const SimdQuaternion& worldquat = m_body->getOrientation();
            m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);

            m_MotionState->calculateWorldTransformations();

            float scale[3];
            m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
            SimdVector3 scaling(scale[0],scale[1],scale[2]);
            GetCollisionShape()->setLocalScaling(scaling);
      } else
      {
            SimdVector3 worldPos;
            SimdQuaternion worldquat;

            m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
            m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
            SimdTransform oldTrans = m_body->getCenterOfMassTransform();
            SimdTransform newTrans(worldquat,worldPos);
                        
            m_body->setCenterOfMassTransform(newTrans);
            //need to keep track of previous position for friction effects...
            
            m_MotionState->calculateWorldTransformations();

            float scale[3];
            m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
            SimdVector3 scaling(scale[0],scale[1],scale[2]);
            GetCollisionShape()->setLocalScaling(scaling);
      }
      return true;

}

            /**
                  WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
            */
            
00161 void        CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
{

}
void        CcdPhysicsController::WriteDynamicsToMotionState()
{
}
            // controller replication
void        CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
{
      m_MotionState = motionstate;

      

      m_body = 0;
      CreateRigidbody();
      
      m_cci.m_physicsEnv->addCcdPhysicsController(this);


/*    SM_Object* dynaparent=0;
      SumoPhysicsController* sumoparentctrl = (SumoPhysicsController* )parentctrl;
      
      if (sumoparentctrl)
      {
            dynaparent = sumoparentctrl->GetSumoObject();
      }
      
      SM_Object* orgsumoobject = m_sumoObj;
      
      
      m_sumoObj   =     new SM_Object(
            orgsumoobject->getShapeHandle(), 
            orgsumoobject->getMaterialProps(),              
            orgsumoobject->getShapeProps(),
            dynaparent);
      
      m_sumoObj->setRigidBody(orgsumoobject->isRigidBody());
      
      m_sumoObj->setMargin(orgsumoobject->getMargin());
      m_sumoObj->setPosition(orgsumoobject->getPosition());
      m_sumoObj->setOrientation(orgsumoobject->getOrientation());
      //if it is a dyna, register for a callback
      m_sumoObj->registerCallback(*this);
      
      m_sumoScene->add(* (m_sumoObj));
      */



}

void CcdPhysicsController::SetMargin(float margin)
{
      if (m_body && m_body->GetCollisionShape())
      {
            m_body->GetCollisionShape()->SetMargin(margin);
      }


}

float CcdPhysicsController::GetMargin() const
{
      if (m_body && m_body->GetCollisionShape())
      {
            return m_body->GetCollisionShape()->GetMargin();
      }

      return 0.f;

}


            // kinematic methods
void        CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
{
      if (m_body)
      {
            m_body->activate();

            SimdVector3 dloc(dlocX,dlocY,dlocZ);
            SimdTransform xform = m_body->getCenterOfMassTransform();

            if (local)
            {
                  dloc = xform.getBasis()*dloc;
            }

            xform.setOrigin(xform.getOrigin() + dloc);
            m_body->setCenterOfMassTransform(xform);
      }

}

void        CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
{
      if (m_body )
      {
            m_body->activate();

            SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
                                                rotval[4],rotval[5],rotval[6],
                                                rotval[8],rotval[9],rotval[10]);


            SimdMatrix3x3 currentOrn;
            GetWorldOrientation(currentOrn);

            SimdTransform xform = m_body->getCenterOfMassTransform();

            xform.setBasis(xform.getBasis()*(local ? 
            drotmat : (currentOrn.inverse() * drotmat * currentOrn)));

            m_body->setCenterOfMassTransform(xform);
      }

}

void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
{
      float orn[4];
      m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
      SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
      mat.setRotation(quat);
}

void        CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
{
      SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation();
      quatImag0 = q[0];
      quatImag1 = q[1];
      quatImag2 = q[2];
      quatReal = q[3];
}
void        CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
{
      m_body->activate();

      SimdTransform xform  = m_body->getCenterOfMassTransform();
      xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
      m_body->setCenterOfMassTransform(xform);

}

void        CcdPhysicsController::setPosition(float posX,float posY,float posZ)
{
      m_body->activate();

      SimdTransform xform  = m_body->getCenterOfMassTransform();
      xform.setOrigin(SimdVector3(posX,posY,posZ));
      m_body->setCenterOfMassTransform(xform);

}
void        CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
{
}

void        CcdPhysicsController::getPosition(PHY__Vector3& pos) const
{
      const SimdTransform& xform = m_body->getCenterOfMassTransform();
      pos[0] = xform.getOrigin().x();
      pos[1] = xform.getOrigin().y();
      pos[2] = xform.getOrigin().z();
}

void        CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
{
      if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
            !SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
            !SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
      {
            m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);

            if (m_body && m_body->GetCollisionShape())
            {
                  m_body->GetCollisionShape()->setLocalScaling(m_cci.m_scaling);
                  m_body->GetCollisionShape()->CalculateLocalInertia(m_cci.m_mass, m_cci.m_localInertiaTensor);
                  m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
            }
      }
}
            
            // physics methods
void        CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
{
      SimdVector3 torque(torqueX,torqueY,torqueZ);
      SimdTransform xform = m_body->getCenterOfMassTransform();
      if (local)
      {
            torque      = xform.getBasis()*torque;
      }
      m_body->applyTorque(torque);
}

void        CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
{
      SimdVector3 force(forceX,forceY,forceZ);
      SimdTransform xform = m_body->getCenterOfMassTransform();
      if (local)
      {
            force = xform.getBasis()*force;
      }
      m_body->applyCentralForce(force);
}
void        CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
{
      SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
      if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
      {
            m_body->activate();
      }

      {
            SimdTransform xform = m_body->getCenterOfMassTransform();
            if (local)
            {
                  angvel      = xform.getBasis()*angvel;
            }

            m_body->setAngularVelocity(angvel);
      }

}
void        CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
{

      SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
      if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
      {
            m_body->activate();
      }
      
      {
            SimdTransform xform = m_body->getCenterOfMassTransform();
            if (local)
            {
                  linVel      = xform.getBasis()*linVel;
            }
            m_body->setLinearVelocity(linVel);
      }
}
void        CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
{
      SimdVector3 impulse(impulseX,impulseY,impulseZ);

      if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
      {
            m_body->activate();
            
            SimdVector3 pos(attachX,attachY,attachZ);

            m_body->activate();

            m_body->applyImpulse(impulse,pos);
      }

}
void        CcdPhysicsController::SetActive(bool active)
{
}
            // reading out information from physics
void        CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
{
      const SimdVector3& linvel = this->m_body->getLinearVelocity();
      linvX = linvel.x();
      linvY = linvel.y();
      linvZ = linvel.z();

}

void        CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
{
      const SimdVector3& angvel= m_body->getAngularVelocity();
      angVelX = angvel.x();
      angVelY = angvel.y();
      angVelZ = angvel.z();
}

void        CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
{
      SimdVector3 pos(posX,posY,posZ);
      SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
      SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
      linvX = linvel.x();
      linvY = linvel.y();
      linvZ = linvel.z();
}
void        CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
{
}

            // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
void        CcdPhysicsController::setRigidBody(bool rigid)
{
      if (!rigid)
      {
            //fake it for now
            SimdVector3 inertia = m_body->getInvInertiaDiagLocal();
            inertia[1] = 0.f;
            m_body->setInvInertiaDiagLocal(inertia);
            m_body->updateInertiaTensor();
      }
}

            // clientinfo for raycasts for example
void*       CcdPhysicsController::getNewClientInfo()
{
      return m_newClientInfo;
}
void        CcdPhysicsController::setNewClientInfo(void* clientinfo)
{
      m_newClientInfo = clientinfo;
}


void  CcdPhysicsController::UpdateDeactivation(float timeStep)
{
      if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == DISABLE_DEACTIVATION))
            return;

      if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
            (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
      {
            m_body->m_deactivationTime += timeStep;
      } else
      {
            m_body->m_deactivationTime=0.f;
            m_body->SetActivationState(0);
      }

}

bool CcdPhysicsController::wantsSleeping()
{

      if (m_body->GetActivationState() == DISABLE_DEACTIVATION)
            return false;

      //disable deactivation
      if (gDisableDeactivation || (gDeactivationTime == 0.f))
            return false;

      if ( (m_body->GetActivationState() == ISLAND_SLEEPING) || (m_body->GetActivationState() == WANTS_DEACTIVATION))
            return true;

      if (m_body->m_deactivationTime> gDeactivationTime)
      {
            return true;
      }
      return false;
}

PHY_IPhysicsController* CcdPhysicsController::GetReplica()
{
      //very experimental, shape sharing is not implemented yet.
      //just support SphereShape/ConeShape for now

      CcdConstructionInfo cinfo = m_cci;
      if (cinfo.m_collisionShape)
      {
            switch (cinfo.m_collisionShape->GetShapeType())
            {
            case SPHERE_SHAPE_PROXYTYPE:
                  {
                        SphereShape* orgShape = (SphereShape*)cinfo.m_collisionShape;
                        cinfo.m_collisionShape = new SphereShape(*orgShape);
                        break;
                  }

                  case CONE_SHAPE_PROXYTYPE:
                  {
                        ConeShape* orgShape = (ConeShape*)cinfo.m_collisionShape;
                        cinfo.m_collisionShape = new ConeShape(*orgShape);
                        break;
                  }


            default:
                  {
                        return 0;
                  }
            }
      }

      cinfo.m_MotionState = new DefaultMotionState();

      CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
      return replica;
}

///////////////////////////////////////////////////////////
///A small utility class, DefaultMotionState
///
///////////////////////////////////////////////////////////

00557 DefaultMotionState::DefaultMotionState()
{
      m_worldTransform.setIdentity();
}


DefaultMotionState::~DefaultMotionState()
{

}

void  DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
{
      posX = m_worldTransform.getOrigin().x();
      posY = m_worldTransform.getOrigin().y();
      posZ = m_worldTransform.getOrigin().z();
}

void  DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
{
      scaleX = 1.;
      scaleY = 1.;
      scaleZ = 1.;
}

void  DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)
{
      quatIma0 = m_worldTransform.getRotation().x();
      quatIma1 = m_worldTransform.getRotation().y();
      quatIma2 = m_worldTransform.getRotation().z();
      quatReal = m_worldTransform.getRotation()[3];
}
            
void  DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
{
      SimdPoint3 pos(posX,posY,posZ);
      m_worldTransform.setOrigin( pos );
}

void  DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{
      SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
      m_worldTransform.setRotation( orn );
}
            
void  DefaultMotionState::calculateWorldTransformations()
{

}


Generated by  Doxygen 1.6.0   Back to index