cens_physics.cpp

Go to the documentation of this file.

// Computational Embodied Neuroscience Simulator (CENS) Library
// Copyright (c) 2010 Francesco Mannella
//
// cens_physics.cpp
// Copyright (c) 2010 Francesco Mannella
//
// This file is part of CENS library.
//
// CENS library is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// CENS library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with CENS library.  If not, see <http://www.gnu.org/licenses/>.

#include "cens_physics.h"
#include "cens_utils.h"

namespace cens {


    
    CENSNonCollidingTable CENSPhysics::m_phNonCollidingTable;
   


    CENSPhysics::CENSPhysics(): 
        m_phDynamicsWorld(0),
        m_phDefaultContactProcessingThreshold(BT_LARGE_FLOAT),
        m_phParameterManager(physics_params_filename)
    {
        initCENSPhysics();
    }


    CENSPhysics::~CENSPhysics() 
    {

        //cleanup in the reverse order of creation/initialization

        //remove the rigidbodies from the dynamics world and delete them
        for (int i=m_phDynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
        {

            btCollisionObject* obj = 
                m_phDynamicsWorld->getCollisionObjectArray()[i];
            btRigidBody* body = btRigidBody::upcast(obj);
            if (body && body->getMotionState())
            {
                delete body->getMotionState();
            }
            m_phDynamicsWorld->removeCollisionObject( obj );
            delete obj;

        }

        // delete dynamic memories
        
        for( int i=0; i<m_phTouchSensors.size(); i++) {
            delete m_phTouchSensors[i];
        }

         for( int i=0; i<m_phCollisionShapes.size(); i++) {
            delete m_phCollisionShapes[i];
        }

       delete m_phDynamicsWorld;
        delete m_phSolver;
        delete m_phBroadphase;
        delete m_phDispatcher;
        delete m_phCollisionConfiguration;

    }


    void CENSPhysics::initCENSPhysics()
    {

        // PARAMETERS //////////////////////////////////////////////

        m_phParameterManager.addParameter("GRAVITY",m_phGravity);
        m_phParameterManager.addParameter("STEP",m_phStep);
        m_phParameterManager.addParameter("SUBSTEP",m_phSubstep);

        if (!m_phParameterManager.loadParameters())
        {
            m_phGravity << 0.0, 0.0, -9.8;
            m_phStep = 0.05;
            m_phSubstep = 20;

            m_phParameterManager.saveParameters();
        }

        //the default btDefaultCollisionConfiguration
        m_phCollisionConfiguration = 
            new btSoftBodyRigidBodyCollisionConfiguration();

        //For parallel processing you can use a 
        //diffent dispatcher (see Extras/BulletMultiThreaded)
        m_phDispatcher = 
            new btCollisionDispatcher(m_phCollisionConfiguration);
        m_phSoftBodyWorldInfo.m_dispatcher = m_phDispatcher;

        m_phBroadphase = new btDbvtBroadphase();
        m_phSoftBodyWorldInfo.m_broadphase = m_phBroadphase;

        //For parallel processing you can use a 
        //different solver (see Extras/BulletMultiThreaded)
        btSequentialImpulseConstraintSolver* sol = 
            new btSequentialImpulseConstraintSolver;
        m_phSolver = sol;

        //Initialize the dynamicsWorld
        m_phDynamicsWorld =
            new btSoftRigidDynamicsWorld(
                    m_phDispatcher,
                    m_phBroadphase,
                    m_phSolver,
                    m_phCollisionConfiguration);

        // set tick callback
        //m_phDynamicsWorld->setInternalTickCallback(internalTickCallback);

        //set gravity
        m_phDynamicsWorld->getDispatchInfo().m_enableSPU = true;
        m_phDynamicsWorld->setGravity(
                btVector3(
                    m_phGravity.x(), 
                    m_phGravity.y(),
                    m_phGravity.z()) );

        //set soft body gravity
        m_phSoftBodyWorldInfo.m_gravity.setValue(
                m_phGravity.x(), 
                m_phGravity.y(), 
                m_phGravity.z() );
        m_phSoftBodyWorldInfo.m_sparsesdf.Initialize();

        initObjects();

    }


    void CENSPhysics::cens_physics_step( ) 
    {
        m_phDynamicsWorld->stepSimulation( m_phStep , m_phSubstep );
        
        for(int i=0; i<m_phTouchSensors.size(); i++)
        {
            CENSTouchSensor *sensor = m_phTouchSensors[i];

            if(sensor->body.isInWorld())
                m_phDynamicsWorld->contactTest(&(sensor->body),*sensor);
        }
    }


    btRigidBody* CENSPhysics::localCreateRigidBody(
            float mass, 
            const btTransform& startTransform,
            btCollisionShape* shape)
    {

        btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

        //rigidbody is dynamic if and only if mass 
        //is non zero, otherwise static
        bool isDynamic = (mass != 0.f);

        btVector3 localInertia(0,0,0);
        if (isDynamic)
            shape->calculateLocalInertia(mass,localInertia);

        //using motionstate is recommended, 
        //it provides interpolation capabilities, 
        //and only synchronizes 'active' objects

        btDefaultMotionState* myMotionState = 
            new btDefaultMotionState(startTransform);

        btRigidBody::btRigidBodyConstructionInfo cInfo(
                mass,
                myMotionState,
                shape,
                localInertia);

        btRigidBody* body = 
            new btRigidBody(cInfo);

        body->setContactProcessingThreshold(
                m_phDefaultContactProcessingThreshold);

        m_phDynamicsWorld->addRigidBody(body);

        return body;

    }

    btSoftBody* CENSPhysics::localCreateSoftBody(
            float mass, 
            btCollisionShape* shape ) {

        if( shape->isConvex() ) {

            const btConvexShape* convexShape =
                static_cast<const btConvexShape*>(shape);
            btShapeHull hull(convexShape);

            if(hull.buildHull(convexShape->getMargin())) {

                btSoftBody *body = btSoftBodyHelpers::CreateFromConvexHull(     
                        m_phSoftBodyWorldInfo, hull.getVertexPointer(), hull.numVertices() );   

                body->setTotalMass(mass,true); 

                m_phDynamicsWorld->addSoftBody(body);

                return body;
            }

        }

        return 0;

    }  


    btTransform &CENSPhysics::getTransformFromBody(btRigidBody *body) {


        return body->getWorldTransform();

    }


    btTransform &CENSPhysics::getTransformFromBody(btSoftBody *body) {

        return body->getWorldTransform();

    }
    void CENSPhysics::setCollisionFilter( 
            CENSNonCollidingTable _non_colliding_table ) {

        CENSNonCollidingTable::iterator it =  _non_colliding_table.begin();
        for(;it != _non_colliding_table.end(); ++it)
        {
            m_phNonCollidingTable[it->first]=it->second;
        }

        m_phDispatcher->setNearCallback(
                (void (*)(btBroadphasePair&, btCollisionDispatcher&, 
                          const btDispatcherInfo&))(collisionFilteringCallback));
    }


    CENSTouchSensor *CENSPhysics::addTouchSensor(btRigidBody *body)
    {
        // create
        CENSTouchSensor *sensor = new CENSTouchSensor(*body,m_phNonCollidingTable);
        // add to list
        m_phTouchSensors.push_back(sensor);

        return sensor;
    }


    void collisionFilteringCallback(btBroadphasePair& collisionPair,
            btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo) {

        btCollisionObject *proxy0 = (btCollisionObject *)(collisionPair.m_pProxy0->m_clientObject);
        btCollisionObject *proxy1 = (btCollisionObject *)(collisionPair.m_pProxy1->m_clientObject);
        bool dispatch=true;

        CENSNonCollidingTable &nct = CENSPhysics::m_phNonCollidingTable;

        CENSNonCollidingTable::iterator it =  nct.begin();
        for(;it != nct.end(); ++it)
        {
            bool enabled = it->second;
            if(not enabled)
            {
                btCollisionObject *ncb0 = it->first.first;
                btCollisionObject *ncb1 = it->first.second;
                
                if( (ncb0 == proxy0) and (ncb1 == proxy1) or
                        (ncb0 == proxy1) and (ncb1 == proxy0) )
                    dispatch=false;
            }
        }

        // Do your collision logic here
        // Only dispatch the Bullet collision information 
        //   if you want the physics to continue
        if(dispatch)
            dispatcher.defaultNearCallback(collisionPair, dispatcher, dispatchInfo);
    }

 
    void internalTickCallback(btDynamicsWorld *world, btScalar timestep)
    {
    }
    

    std::map<CENSHingeConstraint *, float> CENSHingeConstraint::angles;

    void CENSHingeConstraint::setPDMotorTarget(float targetAngle, 
            float kp ,  float ki , float kd , float dt ) {

        // control limits
        if (getLowerLimit() < getUpperLimit())
        {
            if (targetAngle < getLowerLimit())
                targetAngle = getLowerLimit();
            else if (targetAngle > getUpperLimit())
                targetAngle = getUpperLimit();
        }

        btScalar curAngle  = getHingeAngle(
                m_rbA.getCenterOfMassTransform(),
                m_rbB.getCenterOfMassTransform());
        
      
        static btScalar prev_error = 0;
        static btScalar integral = 0;
      
        // compute PID current torque
        btScalar error = targetAngle - curAngle;
        integral += error*dt;
        btScalar derivative =  (error - prev_error)/dt;
        prev_error = error;

        btScalar torque = kp*error +ki*integral + kd*derivative;

        //compute K 
        btVector3 axisA =  
            m_rbA.getCenterOfMassTransform().getBasis() * 
            getAFrame().getBasis().getColumn(2);
        float kHinge = 
            1.0f / 
            (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
             getRigidBodyB().computeAngularImpulseDenominator(axisA));

        //compute and set current velocity
        float velocity = torque/kHinge;
        enableAngularMotor(true, velocity, getMaxMotorImpulse() );

        // update angle storage
        angles[this] = curAngle;

    }


    std::map<CENSSliderConstraint *, float> CENSSliderConstraint::positions;

    void  CENSSliderConstraint::setMotorTarget(float targetPos, float dt ) {

        // control limits
        btScalar lowerLimit = getTranslationalLimitMotor()->m_lowerLimit[0];
        btScalar upperLimit = getTranslationalLimitMotor()->m_upperLimit[0];
        if (lowerLimit < upperLimit)
        {
            if (targetPos < lowerLimit)
                targetPos = lowerLimit;
            else if (targetPos > upperLimit)
                targetPos = upperLimit;
        }

        // compute linear velocity
        calculateTransforms();
        btScalar curPos = getRelativePivotPosition(0);
        
        btScalar dPos = targetPos - curPos;
        btScalar velocity = dPos / dt;

        getTranslationalLimitMotor()->m_enableMotor[0] = true;
        getTranslationalLimitMotor()->m_targetVelocity = btVector3(-velocity,0,0);

        positions[this] = curPos;

    }


    void CENSSliderConstraint::setPDMotorTarget(float targetPos, 
            float kp , float ki, float kd , float dt ) {

        // control limits
        btScalar lowerLimit = getTranslationalLimitMotor()->m_lowerLimit[0];
        btScalar upperLimit = getTranslationalLimitMotor()->m_upperLimit[0];
        if (lowerLimit < upperLimit)
        {
            if (targetPos < lowerLimit)
                targetPos = lowerLimit;
            else if (targetPos > upperLimit)
                targetPos = upperLimit;
        }

        calculateTransforms();
        btScalar curPos = getRelativePivotPosition(0);

        static btScalar prev_error = 0;
        static btScalar integral = 0;

        // compute PID current force
        btScalar error = targetPos - curPos;
        integral += error*dt;
        btScalar derivative =  (error - prev_error)/dt;
        prev_error = error;
 
        btScalar force = kp*error +ki*integral + kd*derivative;


        //compute and set current velocity on the x axis
        float velocity =  -force ;

        getTranslationalLimitMotor()->m_enableMotor[0] = true;
        getTranslationalLimitMotor()->m_targetVelocity =  btVector3(velocity,0,0);

        // // update pos storage
        positions[this] = curPos;

    }

}