KX_BulletPhysicsController.cpp

Go to the documentation of this file.

//under visual studio the #define in KX_ConvertPhysicsObject.h is quicker for recompilation
#include "KX_ConvertPhysicsObject.h"

#ifdef USE_BULLET

#include "KX_BulletPhysicsController.h"

#include "btBulletDynamicsCommon.h"
#include "SG_Spatial.h"

#include "KX_GameObject.h"
#include "KX_MotionState.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, bool sensor, bool compound)
: KX_IPhysicsController(dyna,sensor,compound,(PHY_IPhysicsController*)this),
CcdPhysicsController(ci),
m_savedCollisionFlags(0),
m_savedCollisionFilterGroup(0),
m_savedCollisionFilterMask(0),
m_savedMass(0.0),
m_savedDyna(false),
m_suspended(false),
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)
{
        CcdPhysicsController::resolveCombinedVelocities(linvelX,linvelY,linvelZ,angVelX,angVelY,angVelZ);

}


        //      KX_IPhysicsController interface

void    KX_BulletPhysicsController::applyImpulse(const MT_Point3& attach, const MT_Vector3& impulse)
{
                CcdPhysicsController::applyImpulse(attach[0],attach[1],attach[2],impulse[0],impulse[1],impulse[2]);

}

float KX_BulletPhysicsController::GetLinVelocityMin()
{
        return (float)CcdPhysicsController::GetLinVelocityMin();
}
void  KX_BulletPhysicsController::SetLinVelocityMin(float val)
{
        CcdPhysicsController::SetLinVelocityMin(val);
}

float KX_BulletPhysicsController::GetLinVelocityMax()
{
        return (float)CcdPhysicsController::GetLinVelocityMax();
}
void  KX_BulletPhysicsController::SetLinVelocityMax(float val)
{
        CcdPhysicsController::SetLinVelocityMax(val);
}

void    KX_BulletPhysicsController::SetObject (SG_IObject* object)
{
        SG_Controller::SetObject(object);

        // cheating here...
        //should not be necessary, is it for duplicates ?

        KX_GameObject* gameobj = (KX_GameObject*)       object->GetSGClientObject();
        gameobj->SetPhysicsController(this,gameobj->IsDynamic());
        CcdPhysicsController::setNewClientInfo(gameobj->getClientInfo());

        if (m_bSensor)
        {
                // use a different callback function for sensor object, 
                // bullet will not synchronize, we must do it explicitely
                SG_Callbacks& callbacks = gameobj->GetSGNode()->GetCallBackFunctions();
                callbacks.m_updatefunc = KX_GameObject::SynchronizeTransformFunc;
        } 
}

MT_Scalar KX_BulletPhysicsController::GetRadius()
{
        return MT_Scalar(CcdPhysicsController::GetRadius());
}

void    KX_BulletPhysicsController::setMargin (float collisionMargin)
{
        CcdPhysicsController::SetMargin(collisionMargin);
}
void    KX_BulletPhysicsController::RelativeTranslate(const MT_Vector3& dloc,bool local)
{
        CcdPhysicsController::RelativeTranslate(dloc[0],dloc[1],dloc[2],local);

}

void    KX_BulletPhysicsController::RelativeRotate(const MT_Matrix3x3& drot,bool local)
{
        float   rotval[12];
        drot.getValue(rotval);
        CcdPhysicsController::RelativeRotate(rotval,local);
}

void    KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
{
                CcdPhysicsController::ApplyTorque(torque.x(),torque.y(),torque.z(),local);
}
void    KX_BulletPhysicsController::ApplyForce(const MT_Vector3& force,bool local)
{
        CcdPhysicsController::ApplyForce(force.x(),force.y(),force.z(),local);
}
MT_Vector3 KX_BulletPhysicsController::GetLinearVelocity()
{
        float angVel[3];
        //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::GetVelocity(pos[0], pos[1], pos[2], linVel[0],linVel[1],linVel[2]);
        return MT_Vector3(linVel[0],linVel[1],linVel[2]);
}

void    KX_BulletPhysicsController::SetAngularVelocity(const MT_Vector3& ang_vel,bool local)
{
        CcdPhysicsController::SetAngularVelocity(ang_vel.x(),ang_vel.y(),ang_vel.z(),local);

}
void    KX_BulletPhysicsController::SetLinearVelocity(const MT_Vector3& lin_vel,bool local)
{
        CcdPhysicsController::SetLinearVelocity(lin_vel.x(),lin_vel.y(),lin_vel.z(),local);
}
void    KX_BulletPhysicsController::getOrientation(MT_Quaternion& orn)
{
        float myorn[4];
        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_Matrix3x3& orn)
{
        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)
{
        CcdPhysicsController::setPosition(pos.x(),pos.y(),pos.z());
}
void KX_BulletPhysicsController::setScaling(const MT_Vector3& scaling)
{
        CcdPhysicsController::setScaling(scaling.x(),scaling.y(),scaling.z());
}
void KX_BulletPhysicsController::SetTransform()
{
        btVector3 pos;
        btVector3 scale;
        float ori[12];
        m_MotionState->getWorldPosition(pos.m_floats[0],pos.m_floats[1],pos.m_floats[2]);
        m_MotionState->getWorldScaling(scale.m_floats[0],scale.m_floats[1],scale.m_floats[2]);
        m_MotionState->getWorldOrientation(ori);
        btMatrix3x3 rot(ori[0], ori[4], ori[8],
                                        ori[1], ori[5], ori[9],
                                        ori[2], ori[6], ori[10]);
        CcdPhysicsController::forceWorldTransform(rot, pos);
}

MT_Scalar       KX_BulletPhysicsController::GetMass()
{
        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_Vector3      KX_BulletPhysicsController::getReactionForce()
{
        assert(0);
        return MT_Vector3(0.f,0.f,0.f);
}
void    KX_BulletPhysicsController::setRigidBody(bool rigid)
{
}

/* 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 (increments ref count)
        GetShapeInfo()->AddShape(proxyShapeInfo);
        // create new bullet collision shape from the object shapeinfo and set scaling
        btCollisionShape* newChildShape = proxyShapeInfo->CreateBulletShape(childCtrl->GetMargin(), childCtrl->getConstructionInfo().m_bGimpact, true);
        newChildShape->setLocalScaling(relativeScale);
        // add bullet collision shape to parent compound collision shape
        compoundShape->addChildShape(proxyShapeInfo->m_childTrans,newChildShape);
        // proxyShapeInfo is not needed anymore, release it
        proxyShapeInfo->Release();
        // 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)
{
        btRigidBody *body = GetRigidBody();
        if (body && !m_suspended && 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 && !m_suspended && !IsSensor())
        {
                btBroadphaseProxy* handle = body->getBroadphaseHandle();
                m_savedCollisionFlags = body->getCollisionFlags();
                m_savedMass = GetMass();
                m_savedDyna = m_bDyna;
                m_savedCollisionFilterGroup = handle->m_collisionFilterGroup;
                m_savedCollisionFilterMask = handle->m_collisionFilterMask;
                m_suspended = true;
                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);
                m_bDyna = false;
        }
}

void    KX_BulletPhysicsController::RestoreDynamics()
{
        btRigidBody *body = GetRigidBody();
        if (body && m_suspended)
        {
                // before make sure any position change that was done in this logic frame are accounted for
                SetTransform();
                GetPhysicsEnvironment()->updateCcdPhysicsController(this, 
                        m_savedMass,
                        m_savedCollisionFlags,
                        m_savedCollisionFilterGroup,
                        m_savedCollisionFilterMask);
                body->activate();
                m_bDyna = m_savedDyna;
                m_suspended = false;
        }
}

SG_Controller*  KX_BulletPhysicsController::GetReplica(class SG_Node* destnode)
{
        PHY_IMotionState* motionstate = new KX_MotionState(destnode);

        KX_BulletPhysicsController* physicsreplica = new KX_BulletPhysicsController(*this);

        //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())
        {
                KX_GameObject* clientgameobj = (KX_GameObject*) destnode->GetRootSGParent()->GetSGClientObject();
                if (clientgameobj)
                {
                        parentctrl = (KX_BulletPhysicsController*)clientgameobj->GetPhysicsController();
                } else
                {
                        // it could be a false node, try the children
                        NodeList::const_iterator childit;
                        for (
                                childit = destnode->GetSGChildren().begin();
                        childit!= destnode->GetSGChildren().end();
                        ++childit
                                ) {
                                KX_GameObject *clientgameobj_child = static_cast<KX_GameObject*>( (*childit)->GetSGClientObject());
                                if (clientgameobj_child)
                                {
                                        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;
        
}



void    KX_BulletPhysicsController::SetSumoTransform(bool nondynaonly)
{

        if (!m_bDyna && !m_bSensor)
        {
                btCollisionObject* object = GetRigidBody();
                object->setActivationState(ACTIVE_TAG);
                object->setCollisionFlags(object->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 !
void    KX_BulletPhysicsController::SetSimulatedTime(double time)
{
}
        
// call from scene graph to update
bool KX_BulletPhysicsController::Update(double time)
{
        return false;

        // todo: check this code
        //if (GetMass())
        //{
        //      return false;//true;
//      }
//      return false;
}


const char* KX_BulletPhysicsController::getName()
{
        if (m_pObject)
        {
                KX_GameObject* gameobj = (KX_GameObject*)       m_pObject->GetSGClientObject();
                return gameobj->GetName();
        }
        return 0;
}

#endif // USE_BULLET