BGE patch #18051: add localInertia attribute to GameObject.
[blender.git] / source / gameengine / Ketsji / KX_BulletPhysicsController.cpp
index 0c7aa65b2438d130e7c0a1e2cd5a8c9e9c1243be..c621f11994abd5fadb77878e1a1585dbbe0108e0 100644 (file)
@@ -5,8 +5,7 @@
 
 #include "KX_BulletPhysicsController.h"
 
-#include "Dynamics/RigidBody.h"
-
+#include "btBulletDynamicsCommon.h"
 #include "SG_Spatial.h"
 
 #include "KX_GameObject.h"
 #include "KX_ClientObjectInfo.h"
 
 #include "PHY_IPhysicsEnvironment.h"
+#include "CcdPhysicsEnvironment.h"
+#include "BulletSoftBody/btSoftBody.h"
 
 
-KX_BulletPhysicsController::KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna)
-: KX_IPhysicsController(dyna,(PHY_IPhysicsController*)this),
-CcdPhysicsController(ci)
+KX_BulletPhysicsController::KX_BulletPhysicsController (const CcdConstructionInfo& ci, bool dyna, bool compound)
+: KX_IPhysicsController(dyna,compound,(PHY_IPhysicsController*)this),
+CcdPhysicsController(ci),
+m_savedCollisionFlags(0),
+m_bulletChildShape(NULL)
 {
 
 }
        
 KX_BulletPhysicsController::~KX_BulletPhysicsController ()
 {
-
+       // The game object has a direct link to 
+       if (m_pObject)
+       {
+               // If we cheat in SetObject, we must also cheat here otherwise the 
+               // object will still things it has a physical controller
+               // Note that it requires that m_pObject is reset in case the object is deleted
+               // before the controller (usual case, see KX_Scene::RemoveNodeDestructObjec)
+               // The non usual case is when the object is not deleted because its reference is hanging
+               // in a AddObject actuator but the node is deleted. This case is covered here.
+               KX_GameObject* gameobj = (KX_GameObject*)       m_pObject->GetSGClientObject();
+               gameobj->SetPhysicsController(NULL,false);
+       }
 }
 
 void   KX_BulletPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
@@ -59,7 +73,8 @@ void  KX_BulletPhysicsController::SetObject (SG_IObject* object)
 
 }
 
-void   KX_BulletPhysicsController::SetMargin (float collisionMargin)
+
+void   KX_BulletPhysicsController::setMargin (float collisionMargin)
 {
        CcdPhysicsController::SetMargin(collisionMargin);
 }
@@ -87,13 +102,21 @@ void       KX_BulletPhysicsController::ApplyForce(const MT_Vector3& force,bool local)
 MT_Vector3 KX_BulletPhysicsController::GetLinearVelocity()
 {
        float angVel[3];
-       CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
+       //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
+       CcdPhysicsController::GetLinearVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
+       return MT_Vector3(angVel[0],angVel[1],angVel[2]);
+}
+MT_Vector3 KX_BulletPhysicsController::GetAngularVelocity()
+{
+       float angVel[3];
+       //CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);
+       CcdPhysicsController::GetAngularVelocity(angVel[0],angVel[1],angVel[2]);//rcruiz
        return MT_Vector3(angVel[0],angVel[1],angVel[2]);
 }
 MT_Vector3 KX_BulletPhysicsController::GetVelocity(const MT_Point3& pos)
 {
        float linVel[3];
-       CcdPhysicsController::GetLinearVelocity(linVel[0],linVel[1],linVel[2]);
+       CcdPhysicsController::GetVelocity(pos[0], pos[1], pos[2], linVel[0],linVel[1],linVel[2]);
        return MT_Vector3(linVel[0],linVel[1],linVel[2]);
 }
 
@@ -112,9 +135,10 @@ void       KX_BulletPhysicsController::getOrientation(MT_Quaternion& orn)
        CcdPhysicsController::getOrientation(myorn[0],myorn[1],myorn[2],myorn[3]);
        orn = MT_Quaternion(myorn[0],myorn[1],myorn[2],myorn[3]);
 }
-void KX_BulletPhysicsController::setOrientation(const MT_Quaternion& orn)
+void KX_BulletPhysicsController::setOrientation(const MT_Matrix3x3& orn)
 {
-       CcdPhysicsController::setOrientation(orn.x(),orn.y(),orn.z(),orn.w());
+       btMatrix3x3 btmat(orn[0][0], orn[0][1], orn[0][2], orn[1][0], orn[1][1], orn[1][2], orn[2][0], orn[2][1], orn[2][2]);
+       CcdPhysicsController::setWorldOrientation(btmat);
 }
 void KX_BulletPhysicsController::setPosition(const MT_Point3& pos)
 {
@@ -126,13 +150,37 @@ void KX_BulletPhysicsController::setScaling(const MT_Vector3& scaling)
 }
 MT_Scalar      KX_BulletPhysicsController::GetMass()
 {
-
-       MT_Scalar invmass = GetRigidBody()->getInvMass();
+       if (GetSoftBody())
+               return GetSoftBody()->getTotalMass();
+       
+       MT_Scalar invmass = 0.f;
+       if (GetRigidBody())
+               invmass = GetRigidBody()->getInvMass();
        if (invmass)
                return 1.f/invmass;
        return 0.f;
 
 }
+
+MT_Vector3 KX_BulletPhysicsController::GetLocalInertia()
+{
+    MT_Vector3 inertia(0.f, 0.f, 0.f);
+    btVector3 inv_inertia;
+    if (GetRigidBody()) {
+        inv_inertia = GetRigidBody()->getInvInertiaDiagLocal();
+               if (!btFuzzyZero(inv_inertia.getX()) && 
+                       !btFuzzyZero(inv_inertia.getY()) && 
+                       !btFuzzyZero(inv_inertia.getZ()))
+                       inertia = MT_Vector3(1.f/inv_inertia.getX(), 1.f/inv_inertia.getY(), 1.f/inv_inertia.getZ());
+    }
+    return inertia;
+}
+
+MT_Scalar KX_BulletPhysicsController::GetRadius()
+{
+       return MT_Scalar(CcdPhysicsController::GetRadius());
+}
+
 MT_Vector3     KX_BulletPhysicsController::getReactionForce()
 {
        assert(0);
@@ -142,14 +190,184 @@ void     KX_BulletPhysicsController::setRigidBody(bool rigid)
 {
 }
 
-void   KX_BulletPhysicsController::SuspendDynamics()
+/* This function dynamically adds the collision shape of another controller to
+   the current controller shape provided it is a compound shape.
+   The idea is that dynamic parenting on a compound object will dynamically extend the shape
+*/
+void    KX_BulletPhysicsController::AddCompoundChild(KX_IPhysicsController* child)
+{ 
+       if (child == NULL || !IsCompound())
+               return;
+       // other controller must be a bullet controller too
+       // verify that body and shape exist and match
+       KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
+       btRigidBody* rootBody = GetRigidBody();
+       btRigidBody* childBody = childCtrl->GetRigidBody();
+       if (!rootBody || !childBody)
+               return;
+       const btCollisionShape* rootShape = rootBody->getCollisionShape();
+       const btCollisionShape* childShape = childBody->getCollisionShape();
+       if (!rootShape || 
+               !childShape || 
+               rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE ||
+               childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+               return;
+       btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
+       // compute relative transformation between parent and child
+       btTransform rootTrans;
+       btTransform childTrans;
+       rootBody->getMotionState()->getWorldTransform(rootTrans);
+       childBody->getMotionState()->getWorldTransform(childTrans);
+       btVector3 rootScale = rootShape->getLocalScaling();
+       rootScale[0] = 1.0/rootScale[0];
+       rootScale[1] = 1.0/rootScale[1];
+       rootScale[2] = 1.0/rootScale[2];
+       // relative scale = child_scale/parent_scale
+       btVector3 relativeScale = childShape->getLocalScaling()*rootScale;
+       btMatrix3x3 rootRotInverse = rootTrans.getBasis().transpose();  
+       // relative pos = parent_rot^-1 * ((parent_pos-child_pos)/parent_scale)
+       btVector3 relativePos = rootRotInverse*((childTrans.getOrigin()-rootTrans.getOrigin())*rootScale);
+       // relative rot = parent_rot^-1 * child_rot
+       btMatrix3x3 relativeRot = rootRotInverse*childTrans.getBasis();
+       // create a proxy shape info to store the transformation
+       CcdShapeConstructionInfo* proxyShapeInfo = new CcdShapeConstructionInfo();
+       // store the transformation to this object shapeinfo
+       proxyShapeInfo->m_childTrans.setOrigin(relativePos);
+       proxyShapeInfo->m_childTrans.setBasis(relativeRot);
+       proxyShapeInfo->m_childScale.setValue(relativeScale[0], relativeScale[1], relativeScale[2]);
+       // we will need this to make sure that we remove the right proxy later when unparenting
+       proxyShapeInfo->m_userData = childCtrl;
+       proxyShapeInfo->SetProxy(childCtrl->GetShapeInfo()->AddRef());
+       // add to parent compound shapeinfo
+       GetShapeInfo()->AddShape(proxyShapeInfo);
+       // create new bullet collision shape from the object shapeinfo and set scaling
+       btCollisionShape* newChildShape = proxyShapeInfo->CreateBulletShape();
+       newChildShape->setLocalScaling(relativeScale);
+       // add bullet collision shape to parent compound collision shape
+       compoundShape->addChildShape(proxyShapeInfo->m_childTrans,newChildShape);
+       // remember we created this shape
+       childCtrl->m_bulletChildShape = newChildShape;
+       // recompute inertia of parent
+       if (!rootBody->isStaticOrKinematicObject())
+       {
+               btVector3 localInertia;
+               float mass = 1.f/rootBody->getInvMass();
+               compoundShape->calculateLocalInertia(mass,localInertia);
+               rootBody->setMassProps(mass,localInertia);
+       }
+       // must update the broadphase cache,
+       GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
+       // remove the children
+       GetPhysicsEnvironment()->disableCcdPhysicsController(childCtrl);
+}
+
+/* Reverse function of the above, it will remove a shape from a compound shape
+   provided that the former was added to the later using  AddCompoundChild()
+*/
+void    KX_BulletPhysicsController::RemoveCompoundChild(KX_IPhysicsController* child)
+{ 
+       if (child == NULL || !IsCompound())
+               return;
+       // other controller must be a bullet controller too
+       // verify that body and shape exist and match
+       KX_BulletPhysicsController* childCtrl = dynamic_cast<KX_BulletPhysicsController*>(child);
+       btRigidBody* rootBody = GetRigidBody();
+       btRigidBody* childBody = childCtrl->GetRigidBody();
+       if (!rootBody || !childBody)
+               return;
+       const btCollisionShape* rootShape = rootBody->getCollisionShape();
+       if (!rootShape || 
+               rootShape->getShapeType() != COMPOUND_SHAPE_PROXYTYPE)
+               return;
+       btCompoundShape* compoundShape = (btCompoundShape*)rootShape;
+       // retrieve the shapeInfo
+       CcdShapeConstructionInfo* childShapeInfo = childCtrl->GetShapeInfo();
+       CcdShapeConstructionInfo* rootShapeInfo = GetShapeInfo();
+       // and verify that the child is part of the parent
+       int i = rootShapeInfo->FindChildShape(childShapeInfo, childCtrl);
+       if (i < 0)
+               return;
+       rootShapeInfo->RemoveChildShape(i);
+       if (childCtrl->m_bulletChildShape)
+       {
+               int numChildren = compoundShape->getNumChildShapes();
+               for (i=0; i<numChildren; i++)
+               {
+                       if (compoundShape->getChildShape(i) == childCtrl->m_bulletChildShape)
+                       {
+                               compoundShape->removeChildShapeByIndex(i);
+                               compoundShape->recalculateLocalAabb();
+                               break;
+                       }
+               }
+               delete childCtrl->m_bulletChildShape;
+               childCtrl->m_bulletChildShape = NULL;
+       }
+       // recompute inertia of parent
+       if (!rootBody->isStaticOrKinematicObject())
+       {
+               btVector3 localInertia;
+               float mass = 1.f/rootBody->getInvMass();
+               compoundShape->calculateLocalInertia(mass,localInertia);
+               rootBody->setMassProps(mass,localInertia);
+       }
+       // must update the broadphase cache,
+       GetPhysicsEnvironment()->refreshCcdPhysicsController(this);
+       // reactivate the children
+       GetPhysicsEnvironment()->enableCcdPhysicsController(childCtrl);
+}
+
+void KX_BulletPhysicsController::SetMass(MT_Scalar newmass)
 {
-       GetRigidBody()->SetActivationState(DISABLE_SIMULATION);
+       btRigidBody *body = GetRigidBody();
+       if (body && body->getActivationState() != DISABLE_SIMULATION && 
+               newmass>MT_EPSILON && GetMass()>MT_EPSILON)
+       {
+               btVector3 grav = body->getGravity();
+               btVector3 accel = grav / GetMass();
+               
+               btBroadphaseProxy* handle = body->getBroadphaseHandle();
+               GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
+                       newmass,
+                       body->getCollisionFlags(),
+                       handle->m_collisionFilterGroup, 
+                       handle->m_collisionFilterMask);
+               body->setGravity(accel);
+       }
+}
 
+void   KX_BulletPhysicsController::SuspendDynamics(bool ghost)
+{
+       btRigidBody *body = GetRigidBody();
+       if (body && body->getActivationState() != DISABLE_SIMULATION)
+       {
+               btBroadphaseProxy* handle = body->getBroadphaseHandle();
+               m_savedCollisionFlags = body->getCollisionFlags();
+               m_savedMass = GetMass();
+               m_savedCollisionFilterGroup = handle->m_collisionFilterGroup;
+               m_savedCollisionFilterMask = handle->m_collisionFilterMask;
+               m_savedActivationState = body->getActivationState();
+               body->forceActivationState(DISABLE_SIMULATION);
+               GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
+                       0.0,
+                       btCollisionObject::CF_STATIC_OBJECT|((ghost)?btCollisionObject::CF_NO_CONTACT_RESPONSE:(m_savedCollisionFlags&btCollisionObject::CF_NO_CONTACT_RESPONSE)),
+                       btBroadphaseProxy::StaticFilter, 
+                       btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+       }
 }
+
 void   KX_BulletPhysicsController::RestoreDynamics()
 {
-       GetRigidBody()->ForceActivationState(ACTIVE_TAG);
+       btRigidBody *body = GetRigidBody();
+       if (body && body->getActivationState() == DISABLE_SIMULATION)
+       {
+               GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
+                       m_savedMass,
+                       m_savedCollisionFlags,
+                       m_savedCollisionFilterGroup,
+                       m_savedCollisionFilterMask);
+               body->forceActivationState(m_savedActivationState);
+       }
 }
 
 SG_Controller* KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
@@ -161,6 +379,9 @@ SG_Controller*      KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
        //parentcontroller is here be able to avoid collisions between parent/child
 
        PHY_IPhysicsController* parentctrl = NULL;
+       KX_BulletPhysicsController* parentKxCtrl = NULL;
+       CcdPhysicsController* ccdParent = NULL;
+
        
        if (destnode != destnode->GetRootSGParent())
        {
@@ -177,17 +398,21 @@ SG_Controller*    KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
                        childit!= destnode->GetSGChildren().end();
                        ++childit
                                ) {
-                               KX_GameObject *clientgameobj = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
-                               if (clientgameobj)
+                               KX_GameObject *clientgameobj_child = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
+                               if (clientgameobj_child)
                                {
-                                       parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
+                                       parentKxCtrl = (KX_BulletPhysicsController*)clientgameobj_child->GetPhysicsController();
+                                       parentctrl = parentKxCtrl;
+                                       ccdParent = parentKxCtrl;
                                }
                        }
                }
        }
 
+       physicsreplica->setParentCtrl(ccdParent);
        physicsreplica->PostProcessReplica(motionstate,parentctrl);
-
+       physicsreplica->m_userdata = (PHY_IPhysicsController*)physicsreplica;
+       physicsreplica->m_bulletChildShape = NULL;
        return physicsreplica;
        
 }
@@ -196,6 +421,39 @@ SG_Controller*     KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
 
 void   KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
 {
+       if (GetRigidBody())
+               GetRigidBody()->activate(true);
+
+       if (!m_bDyna)
+       {
+               GetCollisionObject()->setCollisionFlags(GetRigidBody()->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
+       } else
+       {
+               if (!nondynaonly)
+               {
+                       /*
+                       btTransform worldTrans;
+                       if (GetRigidBody())
+                       {
+                               GetRigidBody()->getMotionState()->getWorldTransform(worldTrans);
+                               GetRigidBody()->setCenterOfMassTransform(worldTrans);
+                       }
+                       */
+                       /*
+                       scaling?
+                       if (m_bDyna)
+                       {
+                               m_sumoObj->setScaling(MT_Vector3(1,1,1));
+                       } else
+                       {
+                               MT_Vector3 scale;
+                               GetWorldScaling(scale);
+                               m_sumoObj->setScaling(scale);
+                       }
+                       */
+
+               }
+       }
 }
 
 // todo: remove next line !