Bullet 刚体与软体的碰撞 collisions between rigidbody and softbody in Bullet

上一篇文章做了一个bullet刚体自由落体的模拟，这篇文章模拟软体与刚体的碰撞，并且让软体保持碰撞后的形状。关于软体的参数配置如下：

btSoftBody::Config :
btSoftBody::Config::aeromodel; // Aerodynamic model (default: V_Point)
define what kind feature is used to compute aerodynamic forces.
btSoftBody::Config::kVCF; // Velocities correction factor (Baumgarte)
define the amount of correction per time step for drift solver (sometimes referred as ERP in rigid bodies solvers).
btSoftBody::Config::kDP; // Damping coefficient [0,1]
damping, zero = no damping, one= full damping.
btSoftBody::Config::kDG; // Drag coefficient [0,+inf]
[aerodynamic]=> is referred in http://en.wikipedia.org/wiki/Drag_%28force%29 as ‘Cd‘, kDG=0 mean no drag.
btSoftBody::Config::kLF; // Lift coefficient [0,+inf]
[aerodynamic]=> is a factor of the lift force (http://en.wikipedia.org/wiki/Lift_%28force%29) kLF=0 mean no lift
btSoftBody::Config::kPR; // Pressure coefficient [-inf,+inf]
[aerodynamic]=> is a factor of pressure.
btSoftBody::Config::kVC; // Volume conversation coefficient [0,+inf]
when ‘setPose(true,...)‘ as been called, define the magnitude of the force used to conserve volume.
btSoftBody::Config::kDF; // Dynamic friction coefficient [0,1]
friction, kDF=0 mean sliding, kDF=1 mean sticking.
btSoftBody::Config::kMT; // Pose matching coefficient [0,1]
when ‘setPose(...,true)‘ as been called, define the factor used for pose matching.
btSoftBody::Config::kCHR; // Rigid contacts hardness [0,1]
define how ‘soft‘ contact with rigid bodies are, kCHR=0 mean no penetration correction, 1 mean full correction.
btSoftBody::Config::kKHR; // Kinetic contacts hardness [0,1]
define how ‘soft‘ contact with kinetic/static bodies are, kKHR=0 mean no penetration correction, 1 mean full correction.
btSoftBody::Config::kSHR; // Soft contacts hardness [0,1]
define how ‘soft‘ contact with other soft bodies are, kSHR=0 mean no penetration correction, 1 mean full correction.
btSoftBody::Config::kAHR; // Anchors hardness [0,1]
define how ‘soft‘ anchor constraint (joint) are, kAHR=0 mean no drift correction, 1 mean full correction.
btSoftBody::Config::maxvolume; // Maximum volume ratio for pose
--unused--
btSoftBody::Config::timescale; // Time scale
factor of time step, can be used to speed up, or slow down simulation, default=1.
btSoftBody::Config::viterations; // Velocities solver iterations
number of iterations for velocity solvers (if any).
btSoftBody::Config::piterations; // Positions solver iterations
number of iterations for position solvers (if any).
btSoftBody::Config::diterations; // Drift solver iterations
number of iterations for drift solvers (if any).
btSoftBody::Config::collisions; // Collisions flags
see btSoftBody::fCollision.
btSoftBody::Config::m_vsequence,m_psequence,m_dsequence;
define the order and type of solvers to apply for velocities, positions and drift, see btSoftBody::eVSolver for velocities, and btSoftBody::ePSolver for positions and drift.

为了达到该效果，软体部分主要的参数设置是

m_softBodyWorldInfo.m_gravity.setValue(0,0,0); 把软体世界的重力设为（0，0，0），如果不设为0，又想达到上述效果，目前还不知道可行办法。

sphere->setPose(true, true); 两个参数分别为，是否维持软体的体积和形状

sphere->m_cfg.kVC = 0.001; //Volume conversation，当打开维持体积参数后，该参数用于设置维持系数，[0，1]，越大表示维持程度越大

sphere->m_cfg.kMT = 0.0; //pose matching 当打开维持形状参数后，该参数用于设置维持系数，[0，1]，越大表示维持程度越大

Bullet.cpp

#include <GLUT/glut.h>
#include <cstdlib> /* for exit */
#include <vector>

#include <btBulletDynamicsCommon.h>
#include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btSoftBody.h"

#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btConvexHull.h"
#include "LinearMath/btAlignedObjectArray.h"

using namespace std;

float zoom = 800.f;
float rotx = 20;
float roty = 0;
float tx = 0;
float ty = 0;
int lastx=0;
int lasty=0;
unsigned char Buttons[3] = {0};
float lightPosition[] = { -200, 300, 300, 1.0f};
float ambientLight[] = { 0.2f, 0.2f, 0.2f, 1.0f };
float diffuseLight[] = { 0.8f, 0.8f, 0.8, 1.0f };
float specularLight[] = { 0.5f, 0.5f, 0.5f, 1.0f };

btSoftRigidDynamicsWorld* mp_btDynamicsWorld = NULL;
btRigidBody *rigid = NULL;
btRigidBody *ground = NULL;
btSoftBodyWorldInfo m_softBodyWorldInfo;
btSoftBody *sphere = NULL;
void InitWorld()
{
    btVector3 worldAabbMin(-10000,-10000,-10000);
    btVector3 worldAabbMax(10000,10000,10000);
    
    int maxProxies = 100000;
    
    btAxisSweep3* broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
    m_softBodyWorldInfo.m_broadphase = broadphase;

    // 
    btDefaultCollisionConfiguration* collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
    m_softBodyWorldInfo.m_dispatcher = dispatcher;

    
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

   
    mp_btDynamicsWorld = new btSoftRigidDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);

    m_softBodyWorldInfo.m_sparsesdf.Initialize();
    m_softBodyWorldInfo.air_density     =   (btScalar)1.2;
    m_softBodyWorldInfo.water_density   =   0;
    m_softBodyWorldInfo.water_offset    =   0;
    m_softBodyWorldInfo.water_normal    =   btVector3(0,0,0);
    mp_btDynamicsWorld->setGravity(btVector3(0,-500,0));
    m_softBodyWorldInfo.m_gravity.setValue(0,0,0);
}

void InitObject()
{
    //init rigid
    //btCollisionShape *collisionShape = new btBoxShape(btVector3(50,50,50));
    btCollisionShape *collisionShape = new btSphereShape(50);
    //initial position
    btVector3 pos = btVector3(40, 300, 40);
    btQuaternion qrot(0, 0, 0, 1);
    
    btDefaultMotionState* motion_state = new btDefaultMotionState(btTransform(qrot, pos));
    
    btScalar mass = btScalar(100000);
    btVector3 inertia = btVector3(0, 0, 0);//guan xing
    collisionShape->calculateLocalInertia(mass, inertia);
    
    rigid = new btRigidBody(mass, motion_state, collisionShape, inertia);
    
    btScalar restitution = btScalar(0.5);
    rigid->setRestitution(restitution);
    //default 0.5
    btScalar friction = btScalar(1);
    rigid->setFriction(friction);
    
    mp_btDynamicsWorld->addRigidBody(rigid);
    
    //init ground
    btCollisionShape *groundShape = new btBoxShape(btVector3(500,5,500)); //half size
    
    btVector3 groundpos = btVector3(0,0,0);
    btQuaternion groundrot(0, 0, 0, 1);
    btDefaultMotionState* groundMotion = new btDefaultMotionState(btTransform(groundrot, groundpos));
    ground  = new btRigidBody(0.0, groundMotion, groundShape);//mass = 0 means it is a static object
    btScalar rest = btScalar(1);
    ground->setRestitution(rest);
    mp_btDynamicsWorld->addRigidBody(ground);


    //softbody
    sphere = btSoftBodyHelpers::CreateEllipsoid(m_softBodyWorldInfo, 
        btVector3(0, 60, 0),
        btVector3(1, 1, 1)*60,
        10000);

    sphere->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
    sphere->setTotalMass(1000000,true);
    sphere->setPose(true, true);
    
    sphere->m_materials[0]->m_kLST = 1;
    //sphere->m_cfg.kVCF = 0;
    sphere->m_cfg.kDP = 1;
    sphere->m_cfg.kDG = 0;
    sphere->m_cfg.kLF = 0;
    sphere->m_cfg.kPR = 100;
    sphere->m_cfg.kVC = 0.001; //Volume conversation
    sphere->m_cfg.kDF = 0;//1
    sphere->m_cfg.kMT = 0.0; //pose matching
    sphere->m_cfg.kCHR = 0;
    sphere->m_cfg.kKHR = 0;
    sphere->m_cfg.kSHR = 0;
    sphere->m_cfg.kAHR = 0;
    mp_btDynamicsWorld->addSoftBody(sphere);
}

void DeleteBullet()
{
    //rigid
    delete rigid->getMotionState();
    mp_btDynamicsWorld->removeRigidBody(rigid);
    delete rigid;
    rigid = NULL;
    
    //ground
    delete ground->getMotionState();
    mp_btDynamicsWorld->removeRigidBody(ground);
    delete ground;
    ground = NULL;
    
    //softbody
    mp_btDynamicsWorld->removeSoftBody(sphere);
    delete sphere;
    sphere = NULL;
    //world
    delete mp_btDynamicsWorld->getBroadphase();
    delete mp_btDynamicsWorld;
    mp_btDynamicsWorld = NULL;
}

void DrawGrid(int _halfLen, int _gridNum)
{
    glColor3f(1.0f,1.0f,1.0f);
    
    // draw grid
    glLineWidth(2);
    glBegin(GL_LINES);
    for(int i = -_halfLen;i <= _halfLen; i += (_halfLen/_gridNum)) {
        glVertex3f(i,0,-_halfLen);
        glVertex3f(i,0,_halfLen);
        
        glVertex3f(_halfLen,0,i);
        glVertex3f(-_halfLen,0,i);
    }
    glEnd();

}
void DrawCoordinate(float _flengthX, float _flengthY, float _flengthZ)
{
    glLineWidth(5);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    glVertex3f(0,0,0);
    glVertex3f(_flengthX,0,0);
    glEnd();
    
    glBegin(GL_LINES);
    glColor3f(0,1,0);
    glVertex3f(0,0,0);
    glVertex3f(0,_flengthY,0);
    glEnd();
    
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    glVertex3f(0,0,0);
    glVertex3f(0,0,_flengthZ);
    glEnd();
}

void DrawBulletObject()
{
    //rigid
    btTransform trans = rigid->getWorldTransform();
    btScalar m[16];
    trans.getOpenGLMatrix(m);
    glColor3f(0, 0, 1);
    glPushMatrix();
    
    glMultMatrixf((GLfloat*)m);
    //glutSolidCube(100); //total length
    glutSolidSphere(50,50,50);
    glPopMatrix();
    
    //ground
    glColor3f(0, 1, 0);
    glPushMatrix();
    //glTranslatef(0, -50, 0);
    glScalef(1, 0.001, 1);
    glutSolidCube(1000); //size
    glPopMatrix();
    
    //softbody
    btTransform meshTransform = sphere->getWorldTransform();
    GLfloat tempForm[16];
    meshTransform.getOpenGLMatrix(tempForm);

    glPushMatrix();
    glMultMatrixf(tempForm);

    int numFaces = sphere->m_faces.size();
    for (int i=0; i< numFaces; i++)
    {
        btSoftBody::Node*   node_0=sphere->m_faces[i].m_n[0];
        btSoftBody::Node*   node_1=sphere->m_faces[i].m_n[1];
        btSoftBody::Node*   node_2=sphere->m_faces[i].m_n[2];

        btVector3 p0;
        p0 = node_0->m_x;
        btVector3 p1;
        p1 = node_1->m_x;
        btVector3 p2;
        p2 = node_2->m_x;

        //Calculate the normals for the 2 triangles and add on
        btVector3 tpt1, tpt2;
        tpt1 = p1-p0;
        tpt2 = p2-p0;
        btVector3 normal= tpt1.cross(tpt2);

        glBegin(GL_TRIANGLES);
        glNormal3fv(normal);
        glColor3f(0,1,1);
        glVertex3f(p0.getX()*1,p0.getY()*1,p0.getZ()*1);
        glVertex3f(p1.getX()*1,p1.getY()*1,p1.getZ()*1);
        glVertex3f(p2.getX()*1,p2.getY()*1,p2.getZ()*1);
        glEnd();
    }
    glPopMatrix(); 
}

void Simulate()
{
    double dt = 1.f/60.0f;
    if(mp_btDynamicsWorld)
        mp_btDynamicsWorld->stepSimulation(dt,1);
}
//-------------------------------------------------------------------------------
///
void Display()
{
    Simulate();
    
    glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
    
    
    glLoadIdentity();
    
    glTranslatef(0,0,-zoom);
    glTranslatef(tx,ty,0);
    glRotatef(rotx,1,0,0);
    glRotatef(roty,0,1,0);
    
    
    glLightfv(GL_LIGHT1, GL_AMBIENT, ambientLight);
    glLightfv(GL_LIGHT1, GL_DIFFUSE, diffuseLight);
    glLightfv(GL_LIGHT1, GL_SPECULAR, specularLight);
    glLightfv(GL_LIGHT1, GL_POSITION, lightPosition);
    
    glEnable(GL_LIGHT1);
    glEnable(GL_LIGHTING);

    
    glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
    glEnable(GL_COLOR_MATERIAL);
    
    DrawBulletObject();

    glDisable( GL_LIGHTING );
    glDisable(GL_COLOR_MATERIAL);
    
    //DrawGrid(1000, 10);
    //DrawCoordinate(1000,1000,1000);
    
    glutPostRedisplay();
    glutSwapBuffers();
}

void Init()
{
    glShadeModel(GL_SMOOTH);
    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
    glClearDepth(1.0f);
    glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
    glDepthFunc(GL_LEQUAL);
    glEnable(GL_DEPTH_TEST);
    
    glEnable(GL_CULL_FACE);
    
    glEnable(GL_NORMALIZE);
    
    
    InitWorld();
    InitObject();
}


void Reshape(int w, int h)
{
    // prevent divide by 0 error when minimised
    if(w==0)
        h = 1;
    
    glViewport(0,0,w,h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45,(float)w/h,0.1,5000);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
}


//-------------------------------------------------------------------------------
//
void Motion(int x,int y)
{
    int diffx=x-lastx;
    int diffy=y-lasty;
    lastx=x;
    lasty=y;
    
    if( Buttons[2] )
    {
        zoom -= (float)  2* diffx*2;
    }
    else
        if( Buttons[0] )
        {
            rotx += (float) 1 * diffy;
            roty += (float) 1 * diffx;
        }
        else
            if( Buttons[1] )
            {
                tx += (float) 1 * diffx;
                ty -= (float) 1 * diffy;
            }
    glutPostRedisplay();
}

void Mouse(int b,int s,int x,int y)
{
    lastx=x;
    lasty=y;
    switch(b)
    {
        case GLUT_LEFT_BUTTON:
            Buttons[0] = ((GLUT_DOWN==s)?1:0);
            break;
        case GLUT_MIDDLE_BUTTON:
            Buttons[1] = ((GLUT_DOWN==s)?1:0);
            break;
        case GLUT_RIGHT_BUTTON:
            Buttons[2] = ((GLUT_DOWN==s)?1:0);
            break;
        default:
            break;
    }
    glutPostRedisplay();
}


void Keyboard(unsigned char key, int x, int y)
{
    switch(key) {
        case 'q':
        case 'Q':
        case 27: // ESC key
            exit(0);
        break;
        case 'r':
            DeleteBullet();
            InitWorld();
            InitObject();
        break;
        default:
        break;
    }
}

int main(int argc,char** argv)
{
    glutInit(&argc,argv);
    glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);
    glutInitWindowSize(640,480);
    glutInitWindowPosition(100,100);
    glutCreateWindow("Bullet Framework");
    glutDisplayFunc(Display);
    glutReshapeFunc(Reshape);
    glutMouseFunc(Mouse);
    glutMotionFunc(Motion);
    glutKeyboardFunc(Keyboard);
    Init();
    
    glutMainLoop();
    
    return 0;
}

cmake_minimum_required (VERSION 2.6)

project(Bullet)
add_executable(Bullet Bullet.cpp)

find_package(OpenGL REQUIRED)
include_directories(${OPENGL_INCLUDE_DIR})

find_package(GLUT REQUIRED)
include_directories(${GLUT_INCLUDE_DIR})

SET(BULLETHEADER "/Users/yang/DEVELOPMENT/ProgramFiles/bullet-2.82-r2704/src")

include_directories("${BULLETHEADER}")

IF(APPLE)
   #SET(GUI_TYPE MACOSX_BUNDLE)
   FIND_LIBRARY(BULLETDYNAMICS_LIBRARY BulletDynamics)
   FIND_LIBRARY(BULLETCOLLISION_LIBRARY BulletCollision )
   FIND_LIBRARY(LINEARMATH_LIBRARY LinearMath )
   FIND_LIBRARY(BULLETSOFTBODY_LIBRARY BulletSoftBody)
ENDIF (APPLE)

target_link_libraries(Bullet ${GLUT_LIBRARY} ${OPENGL_LIBRARY})
target_link_libraries(Bullet ${BULLETDYNAMICS_LIBRARY} ${BULLETCOLLISION_LIBRARY} ${LINEARMATH_LIBRARY} ${BULLETSOFTBODY_LIBRARY})