bugfix: adding static objects will 'replace' previously added static object in game...
[blender-staging.git] / source / gameengine / Physics / Bullet / CcdPhysicsController.cpp
index 0db5ce4e6689562aa6eb89eec5a6ded1ea4ccb18..5a45ce020cce3cc8e940f416abba587d966b594f 100644 (file)
@@ -1,26 +1,41 @@
+/*
+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 "btBulletDynamicsCommon.h"
 
-#include "Dynamics/RigidBody.h"
 #include "PHY_IMotionState.h"
-#include "BroadphaseCollision/BroadphaseProxy.h"
-#include "CollisionShapes/ConvexShape.h"
 #include "CcdPhysicsEnvironment.h"
 
 
 class BP_Proxy;
 
-///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
+///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
 
 //'temporarily' global variables
-float  gDeactivationTime = 2.f;
-bool   gDisableDeactivation = false;
+//float        gDeactivationTime = 2.f;
+//bool gDisableDeactivation = false;
+extern float gDeactivationTime;
+extern bool gDisableDeactivation;
+
 
 float gLinearSleepingTreshold = 0.8f;
 float gAngularSleepingTreshold = 1.0f;
 
-#include "Dynamics/MassProps.h"
 
-SimdVector3 startVel(0,0,0);//-10000);
+btVector3 startVel(0,0,0);//-10000);
 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
 :m_cci(ci)
 {
@@ -28,13 +43,11 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
        m_newClientInfo = 0;
        
        m_MotionState = ci.m_MotionState;
-
-
-       m_broadphaseHandle = ci.m_broadphaseHandle;
-
-       m_collisionShape = ci.m_collisionShape;
-
+       m_bulletMotionState = 0;
+       
+       
        CreateRigidbody();
+       
 
        
        #ifdef WIN32
@@ -44,39 +57,93 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
 
 }
 
-void CcdPhysicsController::CreateRigidbody()
+btTransform    CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
 {
-       SimdTransform trans;
+       btTransform trans;
        float tmp[3];
-       m_MotionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
-       trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
+       motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
+       trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
 
-       SimdQuaternion orn;
-       m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
+       btQuaternion orn;
+       motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
        trans.setRotation(orn);
+       return trans;
+
+}
 
-       MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
+class  BlenderBulletMotionState : public btMotionState
+{
+       PHY_IMotionState*       m_blenderMotionState;
+
+public:
+
+       BlenderBulletMotionState(PHY_IMotionState* bms)
+               :m_blenderMotionState(bms)
+       {
+
+       }
+
+       virtual void    getWorldTransform(btTransform& worldTrans ) const
+       {
+               float pos[3];
+               float quatOrn[4];
+
+               m_blenderMotionState->getWorldPosition(pos[0],pos[1],pos[2]);
+               m_blenderMotionState->getWorldOrientation(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3]);
+               worldTrans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
+               worldTrans.setBasis(btMatrix3x3(btQuaternion(quatOrn[0],quatOrn[1],quatOrn[2],quatOrn[3])));
+       }
+
+       virtual void    setWorldTransform(const btTransform& worldTrans)
+       {
+               m_blenderMotionState->setWorldPosition(worldTrans.getOrigin().getX(),worldTrans.getOrigin().getY(),worldTrans.getOrigin().getZ());
+               btQuaternion rotQuat = worldTrans.getRotation();
+               m_blenderMotionState->setWorldOrientation(rotQuat[0],rotQuat[1],rotQuat[2],rotQuat[3]);
+               m_blenderMotionState->calculateWorldTransformations();
+       }
+
+};
+
+
+void CcdPhysicsController::CreateRigidbody()
+{
+
+       btTransform trans = GetTransformFromMotionState(m_MotionState);
+
+       m_bulletMotionState = new BlenderBulletMotionState(m_MotionState);
+
+       m_body = new btRigidBody(m_cci.m_mass,
+               m_bulletMotionState,
+               m_cci.m_collisionShape,
+               m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor,
+               m_cci.m_linearDamping,m_cci.m_angularDamping,
+               m_cci.m_friction,m_cci.m_restitution);
 
-       m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
        
+
        //
        // init the rigidbody properly
        //
        
-       m_body->setMassProps(m_cci.m_mass, m_cci.m_localInertiaTensor * m_cci.m_inertiaFactor);
+       //setMassProps this also sets collisionFlags
+       //convert collision flags!
+
+       m_body->setCollisionFlags(m_body->getCollisionFlags() | 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
-       //delete m_collisionShape;
-       m_cci.m_physicsEnv->removeCcdPhysicsController(this);
+       if (m_cci.m_physicsEnv)
+               m_cci.m_physicsEnv->removeCcdPhysicsController(this);
+
        delete m_MotionState;
+       if (m_bulletMotionState)
+               delete m_bulletMotionState;
        delete m_body;
 }
 
@@ -85,20 +152,44 @@ CcdPhysicsController::~CcdPhysicsController()
                */
 bool           CcdPhysicsController::SynchronizeMotionStates(float time)
 {
-       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]);
+       //sync non-static to motionstate, and static from motionstate (todo: add kinematic etc.)
 
-       m_MotionState->calculateWorldTransformations();
+       if (!m_body->isStaticObject())
+       {
+               const btVector3& worldPos = m_body->getCenterOfMassPosition();
+               m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
+               
+               const btQuaternion& worldquat = m_body->getOrientation();
+               m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
 
-       float scale[3];
-       m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
-       SimdVector3 scaling(scale[0],scale[1],scale[2]);
-       m_collisionShape->setLocalScaling(scaling);
+               m_MotionState->calculateWorldTransformations();
 
+               float scale[3];
+               m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
+               btVector3 scaling(scale[0],scale[1],scale[2]);
+               GetCollisionShape()->setLocalScaling(scaling);
+       } else
+       {
+               btVector3 worldPos;
+               btQuaternion worldquat;
+
+/*             m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
+               m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
+               btTransform oldTrans = m_body->getCenterOfMassTransform();
+               btTransform 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]);
+               btVector3 scaling(scale[0],scale[1],scale[2]);
+               GetCollisionShape()->setLocalScaling(scaling);
+       }
        return true;
+
 }
 
                /**
@@ -115,12 +206,13 @@ void              CcdPhysicsController::WriteDynamicsToMotionState()
                // controller replication
 void           CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
 {
-
        m_MotionState = motionstate;
-       m_broadphaseHandle = 0;
+
+       
+
        m_body = 0;
        CreateRigidbody();
-                       
+       
        m_cci.m_physicsEnv->addCcdPhysicsController(this);
 
 
@@ -159,16 +251,26 @@ void              CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
                // kinematic methods
 void           CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
 {
-       SimdVector3 dloc(dlocX,dlocY,dlocZ);
-       SimdTransform xform = m_body->getCenterOfMassTransform();
-
-       if (local)
+       if (m_body)
        {
-               dloc = xform.getBasis()*dloc;
-       }
+               m_body->activate(true);
+               if (m_body->isStaticObject())
+               {
+                       m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+               }
+
+
+               btVector3 dloc(dlocX,dlocY,dlocZ);
+               btTransform xform = m_body->getCenterOfMassTransform();
+
+               if (local)
+               {
+                       dloc = xform.getBasis()*dloc;
+               }
 
-       xform.setOrigin(xform.getOrigin() + dloc);
-       this->m_body->setCenterOfMassTransform(xform);
+               xform.setOrigin(xform.getOrigin() + dloc);
+               m_body->setCenterOfMassTransform(xform);
+       }
 
 }
 
@@ -176,15 +278,21 @@ void              CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
 {
        if (m_body )
        {
-               SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
+               m_body->activate(true);
+               if (m_body->isStaticObject())
+               {
+                       m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+               }
+
+               btMatrix3x3 drotmat(    rotval[0],rotval[1],rotval[2],
                                                                rotval[4],rotval[5],rotval[6],
                                                                rotval[8],rotval[9],rotval[10]);
 
 
-               SimdMatrix3x3 currentOrn;
+               btMatrix3x3 currentOrn;
                GetWorldOrientation(currentOrn);
 
-               SimdTransform xform = m_body->getCenterOfMassTransform();
+               btTransform xform = m_body->getCenterOfMassTransform();
 
                xform.setBasis(xform.getBasis()*(local ? 
                drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
@@ -194,17 +302,17 @@ void              CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
 
 }
 
-void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
+void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& 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]);
+       btQuaternion 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();
+       btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
        quatImag0 = q[0];
        quatImag1 = q[1];
        quatImag2 = q[2];
@@ -212,21 +320,34 @@ void              CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,flo
 }
 void           CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
 {
-       m_body->activate();
+       m_body->activate(true);
+       if (m_body->isStaticObject())
+       {
+               m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+       }
 
-       SimdTransform xform  = m_body->getCenterOfMassTransform();
-       xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
+       m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
+       btTransform xform  = m_body->getCenterOfMassTransform();
+       xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
        m_body->setCenterOfMassTransform(xform);
+       m_bulletMotionState->setWorldTransform(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->activate(true);
+       if (m_body->isStaticObject())
+       {
+               m_body->setCollisionFlags(m_body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+       }
+       
+       m_MotionState->setWorldPosition(posX,posY,posZ);
+       btTransform xform  = m_body->getCenterOfMassTransform();
+       xform.setOrigin(btVector3(posX,posY,posZ));
        m_body->setCenterOfMassTransform(xform);
+       m_bulletMotionState->setWorldTransform(xform);
+
 
 }
 void           CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
@@ -235,7 +356,7 @@ void                CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvel
 
 void           CcdPhysicsController::getPosition(PHY__Vector3& pos) const
 {
-       const SimdTransform& xform = m_body->getCenterOfMassTransform();
+       const btTransform& xform = m_body->getCenterOfMassTransform();
        pos[0] = xform.getOrigin().x();
        pos[1] = xform.getOrigin().y();
        pos[2] = xform.getOrigin().z();
@@ -243,17 +364,23 @@ void              CcdPhysicsController::getPosition(PHY__Vector3& pos) const
 
 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))
+       if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
+               !btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
+               !btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
        {
-               m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);
+               m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
 
-               if (m_body && m_body->GetCollisionShape())
+               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);
+                       m_body->getCollisionShape()->setLocalScaling(m_cci.m_scaling);
+                       
+                       //printf("no inertia recalc for fixed objects with mass=0\n");
+                       if (m_cci.m_mass)
+                       {
+                               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);
+                       } 
+                       
                }
        }
 }
@@ -261,8 +388,12 @@ void               CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
                // physics methods
 void           CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
 {
-       SimdVector3 torque(torqueX,torqueY,torqueZ);
-       SimdTransform xform = m_body->getCenterOfMassTransform();
+       btVector3 torque(torqueX,torqueY,torqueZ);
+       btTransform xform = m_body->getCenterOfMassTransform();
+       if (torque.length2() > (SIMD_EPSILON*SIMD_EPSILON))
+       {
+               m_body->activate();
+       }
        if (local)
        {
                torque  = xform.getBasis()*torque;
@@ -272,8 +403,15 @@ void               CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torque
 
 void           CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
 {
-       SimdVector3 force(forceX,forceY,forceZ);
-       SimdTransform xform = m_body->getCenterOfMassTransform();
+       btVector3 force(forceX,forceY,forceZ);
+       
+       if (force.length2() > (SIMD_EPSILON*SIMD_EPSILON))
+       {
+               m_body->activate();
+       }
+
+
+       btTransform xform = m_body->getCenterOfMassTransform();
        if (local)
        {
                force   = xform.getBasis()*force;
@@ -282,35 +420,55 @@ void              CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bo
 }
 void           CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
 {
-       SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
-       SimdTransform xform = m_body->getCenterOfMassTransform();
-       if (local)
+       btVector3 angvel(ang_velX,ang_velY,ang_velZ);
+       if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
        {
-               angvel  = xform.getBasis()*angvel;
+               m_body->activate(true);
        }
 
-       m_body->setAngularVelocity(angvel);
+       {
+               btTransform 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);
-       SimdTransform xform = m_body->getCenterOfMassTransform();
-       if (local)
+
+       btVector3 linVel(lin_velX,lin_velY,lin_velZ);
+       if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
        {
-               linVel  = xform.getBasis()*linVel;
+               m_body->activate(true);
+       }
+       
+       {
+               btTransform xform = m_body->getCenterOfMassTransform();
+               if (local)
+               {
+                       linVel  = xform.getBasis()*linVel;
+               }
+               m_body->setLinearVelocity(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);
-       SimdVector3 pos(attachX,attachY,attachZ);
+       btVector3 impulse(impulseX,impulseY,impulseZ);
+
+       if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
+       {
+               m_body->activate();
+               
+               btVector3 pos(attachX,attachY,attachZ);
 
-       m_body->activate();
+               m_body->activate();
 
-       m_body->applyImpulse(impulse,pos);
+               m_body->applyImpulse(impulse,pos);
+       }
 
 }
 void           CcdPhysicsController::SetActive(bool active)
@@ -319,7 +477,7 @@ 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();
+       const btVector3& linvel = this->m_body->getLinearVelocity();
        linvX = linvel.x();
        linvY = linvel.y();
        linvZ = linvel.z();
@@ -328,7 +486,7 @@ void                CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& l
 
 void           CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
 {
-       const SimdVector3& angvel= m_body->getAngularVelocity();
+       const btVector3& angvel= m_body->getAngularVelocity();
        angVelX = angvel.x();
        angVelY = angvel.y();
        angVelZ = angvel.z();
@@ -336,9 +494,9 @@ void                CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,flo
 
 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);
+       btVector3 pos(posX,posY,posZ);
+       btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
+       btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
        linvX = linvel.x();
        linvY = linvel.y();
        linvZ = linvel.z();
@@ -350,6 +508,14 @@ void               CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float&
                // 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
+               btVector3 inertia = m_body->getInvInertiaDiagLocal();
+               inertia[1] = 0.f;
+               m_body->setInvInertiaDiagLocal(inertia);
+               m_body->updateInertiaTensor();
+       }
 }
 
                // clientinfo for raycasts for example
@@ -365,36 +531,106 @@ void             CcdPhysicsController::setNewClientInfo(void* clientinfo)
 
 void   CcdPhysicsController::UpdateDeactivation(float timeStep)
 {
-       if ( (m_body->GetActivationState() == 2))
-               return;
-       
+       m_body->updateDeactivation( timeStep);
+}
 
-       if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
-               (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
-       {
-               m_body->m_deactivationTime += timeStep;
-       } else
+bool CcdPhysicsController::wantsSleeping()
+{
+
+       return m_body->wantsSleeping();
+}
+
+PHY_IPhysicsController*        CcdPhysicsController::GetReplica()
+{
+       //very experimental, shape sharing is not implemented yet.
+       //just support btSphereShape/ConeShape for now
+
+       CcdConstructionInfo cinfo = m_cci;
+       if (cinfo.m_collisionShape)
        {
-               m_body->m_deactivationTime=0.f;
-               m_body->SetActivationState(0);
+               switch (cinfo.m_collisionShape->getShapeType())
+               {
+               case SPHERE_SHAPE_PROXYTYPE:
+                       {
+                               btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
+                               cinfo.m_collisionShape = new btSphereShape(*orgShape);
+                               break;
+                       }
+
+                       case CONE_SHAPE_PROXYTYPE:
+                       {
+                               btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
+                               cinfo.m_collisionShape = new btConeShape(*orgShape);
+                               break;
+                       }
+
+
+               default:
+                       {
+                               return 0;
+                       }
+               }
        }
 
+       cinfo.m_MotionState = new DefaultMotionState();
+
+       CcdPhysicsController* replica = new CcdPhysicsController(cinfo);
+       return replica;
 }
 
-bool CcdPhysicsController::wantsSleeping()
+///////////////////////////////////////////////////////////
+///A small utility class, DefaultMotionState
+///
+///////////////////////////////////////////////////////////
+
+DefaultMotionState::DefaultMotionState()
 {
+       m_worldTransform.setIdentity();
+       m_localScaling.setValue(1.f,1.f,1.f);
+}
 
-       //disable deactivation
-       if (gDisableDeactivation || (gDeactivationTime == 0.f))
-               return false;
-       //2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION
-       if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3))
-               return true;
 
-       if (m_body->m_deactivationTime> gDeactivationTime)
-       {
-               return true;
-       }
-       return false;
+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 = m_localScaling.getX();
+       scaleY = m_localScaling.getY();
+       scaleZ = m_localScaling.getZ();
+}
+
+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)
+{
+       btPoint3 pos(posX,posY,posZ);
+       m_worldTransform.setOrigin( pos );
+}
+
+void   DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
+{
+       btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
+       m_worldTransform.setRotation( orn );
+}
+               
+void   DefaultMotionState::calculateWorldTransformations()
+{
+
 }