cens_physics.h

Go to the documentation of this file.

// Computational Embodied Neuroscience Simulator (CENS) Library
// Copyright (c) 2010 Francesco Mannella
//
// cens_physics.h
// 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/>.

#ifndef CENS_PHYSICS_H
#define CENS_PHYSICS_H

#include <vector>
#include <map>
#include <string>


// linear algebra
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

// bullet
#include <btBulletDynamicsCommon.h>
#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
#include "BulletSoftBody/btSoftBody.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "BulletDynamics/Vehicle/btRaycastVehicle.h"  
#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" 
#include "BulletDynamics/Vehicle/btWheelInfo.h"

// parameter managing
#include "cens_parameter_manager.h"
static const char* physics_params_filename = "physics_parameters";

class btSoftSoftCollisionAlgorithm;

class btSoftRididCollisionAlgorithm;

namespace cens {

   


    // TYPEDEFS

    typedef std::pair<btCollisionObject *, btCollisionObject * > CENSNCPair;
    
    typedef std::map<  CENSNCPair , bool > CENSNonCollidingTable;


    // STRUCTURES

    struct CENSTouchSensor 
        : public btCollisionWorld::ContactResultCallback 
    {

        CENSTouchSensor(btRigidBody& tgtBody, CENSNonCollidingTable &_table)
            : btCollisionWorld::ContactResultCallback(), body(tgtBody), table(_table), touched(false) { }

        btRigidBody& body; 
        CENSNonCollidingTable &table; 
        bool touched; 
        std::vector<btVector3> position; 
        std::vector<btScalar> pressures; 

        bool isTouched() 
        {
            bool res = touched;
            touched = false;
            pressures.clear();
            position.clear();
            return res;
        }

        btScalar getPressure()
        {
            btScalar res = 0.0;
            
            for(int i=0; i<pressures.size(); i++)
            {
                res += pressures[i]/pressures.size();
            }
            return res;
        }

        virtual bool needsCollision(btBroadphaseProxy* proxy) const 
        {
            // superclass will check m_collisionFilterGroup and m_collisionFilterMask
            if(!btCollisionWorld::ContactResultCallback::needsCollision(proxy))
                return false;

            btCollisionObject *proxy0 = dynamic_cast<btCollisionObject *>(&body);
            btCollisionObject *proxy1 = static_cast<btCollisionObject*>(proxy->m_clientObject);
            
            // CENS filters
            CENSNonCollidingTable::iterator it = table.begin();
            for(;it != table.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) )
                        return false;
                }
            }

            // if passed filters, may also want to avoid contacts between constraints
            return body.checkCollideWithOverride(
                    static_cast<btCollisionObject*>(proxy->m_clientObject));
        }

        virtual btScalar addSingleResult(btManifoldPoint& cp,
                const btCollisionObjectWrapper* colObj0,int partId0,int index0,
                const btCollisionObjectWrapper* colObj1,int partId1,int index1)
        {
            
            touched = false;

            const btRigidBody * body0 = dynamic_cast<const btRigidBody *>(colObj0->m_collisionObject);
            const btRigidBody * body1 = dynamic_cast<const btRigidBody *>(colObj1->m_collisionObject);
            
            btScalar mass0 = (body0->getInvMass()>0) ? 1./body0->getInvMass() : 0;  
            btScalar mass1 = (body1->getInvMass()>0) ? 1./body1->getInvMass() : 0;
            
            btScalar masses = mass0 + mass1; 
            btScalar rest = body0->getRestitution()*body1->getRestitution();
            btVector3 normal = cp.m_normalWorldOnB;
            btVector3 vel0 = body0->getVelocityInLocalPoint(cp.getPositionWorldOnA());
            btVector3 vel1 = body1->getVelocityInLocalPoint(cp.getPositionWorldOnB());
            vel0 = ((mass0 - rest*mass1)*vel0 +mass1*(1 + rest)*vel1)/masses;
            vel1 = ((mass1 - rest*mass0)*vel1 +mass0*(1 + rest)*vel0)/masses;

            // we subtract the velocities projected on the surface normal
            btScalar pressure = ((normal.dot(vel0)/normal.dot(normal))*normal - 
                    (normal.dot(vel1)/normal.dot(normal))*normal ).length();

            if(body0==&body) 
            {
                position.push_back(cp.m_localPointA);
                pressures.push_back( pressure );
                touched = true;
            } else if(body1==&body) 
            {
                position.push_back(cp.m_localPointB);
                pressures.push_back(cp.getDistance());
                touched = true;
            }

            return 0; 
        }

    };



    class CENSPhysics {

        public:

            CENSPhysics();

            virtual ~CENSPhysics();

            virtual void initCENSPhysics();

            virtual void cens_physics_step( );

            virtual void initObjects() {};

            virtual btDynamicsWorld * getDynamicsWorld() { return m_phDynamicsWorld; }

            virtual btCollisionDispatcher * getDispatcher() { return m_phDispatcher; }


            virtual btRigidBody* localCreateRigidBody(
                    float mass, 
                    const btTransform& startTransform,
                    btCollisionShape* shape );

            virtual btSoftBody* localCreateSoftBody(
                    float mass, 
                    btCollisionShape* shape );

            virtual void setCollisionFilter( 
                    CENSNonCollidingTable  _non_colliding_table );


            virtual CENSTouchSensor *addTouchSensor(btRigidBody *body);


            btScalar getStep() { return m_phStep; };
            btScalar getSubstep() { return m_phSubstep; };
            Vector3f getGravity() { return m_phGravity; };

        protected:

            // FRIENDS
            friend void collisionFilteringCallback(
                    btBroadphasePair& collisionPair,
                    btCollisionDispatcher& dispatcher, 
                    const btDispatcherInfo& dispatchInfo);

            friend void internalTickCallback(
                    btDynamicsWorld *world, 
                    btScalar timestep);

            // METHODS
            btTransform &getTransformFromBody(btRigidBody *body);
            btTransform &getTransformFromBody(btSoftBody *body);

            // MEMBERS
            
            btSoftRigidDynamicsWorld* m_phDynamicsWorld;
            
            btAlignedObjectArray<btCollisionShape*>     m_phCollisionShapes;

            btAlignedObjectArray<CENSTouchSensor *>     m_phTouchSensors; 
            
            // non colliding bodies
            static CENSNonCollidingTable m_phNonCollidingTable;   

            // bullet related variables
            btAlignedObjectArray<btSoftSoftCollisionAlgorithm*> m_phSoftSoftCollisionAlgorithms;
            btAlignedObjectArray<btSoftRididCollisionAlgorithm*> m_phSoftRigidCollisionAlgorithms;
            btSoftBodyWorldInfo m_phSoftBodyWorldInfo;
            btBroadphaseInterface* m_phBroadphase;
            btCollisionDispatcher* m_phDispatcher;
            btConstraintSolver* m_phSolver;
            btDefaultCollisionConfiguration* m_phCollisionConfiguration;
            btScalar m_phDefaultContactProcessingThreshold;
            Vector3f m_phGravity;
            double m_phStep;
            double m_phSubstep;    

            CENSParameterManager m_phParameterManager;

    };


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


    void internalTickCallback(btDynamicsWorld *world, btScalar timestep);
    

    class CENSHingeConstraint : public btHingeConstraint {

        public:

            CENSHingeConstraint (
                    btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, 
                    const btVector3 &pivotInB, btVector3 &axisInA, btVector3 &axisInB,
                    bool useReferenceFrameA=false):
                btHingeConstraint(rbA,rbB,pivotInA,pivotInB,axisInA,axisInB,useReferenceFrameA) {
                    angles[this]=0;
                };

            CENSHingeConstraint (
                    btRigidBody &rbA, const btVector3 &pivotInA, btVector3 &axisInA,
                    bool useReferenceFrameA=false):
                btHingeConstraint(rbA,pivotInA,axisInA,useReferenceFrameA) {
                    angles[this]=0;
                };

            CENSHingeConstraint (
                    btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, 
                    const btTransform &rbBFrame, bool useReferenceFrameA=false):
                btHingeConstraint(rbA,rbB,rbAFrame,rbBFrame,useReferenceFrameA) {
                    angles[this]=0;
                };

            CENSHingeConstraint (
                    btRigidBody &rbA, const btTransform &rbAFrame, 
                    bool useReferenceFrameA=false):
                btHingeConstraint(rbA,rbAFrame,useReferenceFrameA) {
                    angles[this]=0;
                };

            void setPDMotorTarget(float targetAngle, 
                    float kp = 3, float ki = 0, float kd = .5, float dt = .05 ); 

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

    };




    class CENSSliderConstraint : public btGeneric6DofConstraint {

        public:

            CENSSliderConstraint (
                    btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, 
                    const btTransform &rbBFrame, bool useReferenceFrameA=false):
                btGeneric6DofConstraint(rbA,rbB,rbAFrame,rbBFrame,useReferenceFrameA) {
                    positions[this]=0;
                };

            void setMotorTarget(float targetPos, float dt = .05 ); 


            void setPDMotorTarget(float targetPos, 
                    float kp = 3, float ki = 0, float kd = .5, float dt = .05 ); 

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

    };

    

}

#endif // CENS_PHYSICS_H