Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added several updates for btSoftBody: convex cluster collision detection, new constraints, new demos (only enabled in SoftBodyDemo, todo for AllBulletDemos) etc.

Browse Source 
Thanks a lot to Nathanael Presson for this update.
This commit is contained in:
erwin.coumans
2008-07-22 02:22:01 +00:00
parent fe5033119b
commit d71f8d6623
9 changed files with 3223 additions and 1347 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Demos/OpenGL/DemoApplication.cpp
View File

@@ -180,6 +180,7 @@ void DemoApplication::updateCamera() {
m_cameraPosition[0] = eyePos.getX();
m_cameraPosition[1] = eyePos.getY();
m_cameraPosition[2] = eyePos.getZ();
m_cameraPosition += m_cameraTargetPosition;
if (m_glutScreenWidth > m_glutScreenHeight)
{

424
Demos/SoftDemo/SoftDemo.cpp
View File

@@ -129,10 +129,16 @@ void SoftDemo::clientMoveAndDisplay()
m_goal=rayFrom+rayDir*hit;
}
}
m_node->m_v+=(m_goal-m_node->m_x)/dt;
btVector3 delta=m_goal-m_node->m_x;
static const btScalar maxdrag=10;
if(delta.length2()>(maxdrag*maxdrag))
{
delta=delta.normalized()*maxdrag;
}
m_node->m_v+=delta/dt;
}
//#define FIXED_STEP
#define FIXED_STEP
#ifdef FIXED_STEP
m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0);
@@ -296,7 +302,7 @@ static void Ctor_RbUpStack(SoftDemo* pdemo,int count)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,1+6*i,0));
startTransform.setOrigin(btVector3(0,2+6*i,0));
pdemo->localCreateRigidBody(mass,startTransform,shape[i%nshapes]);
}
}
@@ -315,13 +321,14 @@ static void Ctor_BigBall(SoftDemo* pdemo,btScalar mass=10)
//
// Big plate
//
static void Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15)
static btRigidBody* Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15,btScalar height=4)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,4,0.5));
startTransform.setOrigin(btVector3(0,height,0.5));
btRigidBody* body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5)));
body->setFriction(1);
return(body);
}
//
@@ -359,6 +366,7 @@ static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVecto
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
}
//
@@ -598,7 +606,9 @@ static void Init_Aero(SoftDemo* pdemo)
btVector3(+s,h,+s),
segments,segments,
0,true);
psb->generateBendingConstraints(2);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.kLF = 0.004;
psb->m_cfg.kDG = 0.0003;
psb->m_cfg.aeromodel = btSoftBody::eAeroModel::V_TwoSided;
@@ -779,7 +789,7 @@ static void Init_Bunny(SoftDemo* pdemo)
psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints();
psb->scale(btVector3(6,6,6));
psb->setTotalMass(100,true);
psb->setTotalMass(100,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
pdemo->m_cutting=true;
@@ -795,12 +805,12 @@ static void Init_BunnyMatch(SoftDemo* pdemo)
&gIndicesBunny[0][0],
BUNNY_NUM_TRIANGLES);
psb->m_cfg.kDF = 0.5;
psb->m_materials[0]->m_kLST = 0.1;
psb->m_cfg.kMT = 0.05;
psb->m_cfg.piterations = 5;
psb->randomizeConstraints();
psb->scale(btVector3(6,6,6));
psb->setTotalMass(100,true);
psb->setPose(true,true);
psb->setPose(false,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
}
@@ -844,7 +854,7 @@ static void Init_TorusMatch(SoftDemo* pdemo)
psb->transform(btTransform(m,btVector3(0,4,0)));
psb->scale(btVector3(2,2,2));
psb->setTotalMass(50,true);
psb->setPose(true,true);
psb->setPose(false,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
}
@@ -866,7 +876,371 @@ static void Init_Cutting1(SoftDemo* pdemo)
pdemo->m_cutting=true;
}
unsigned current_demo=0;
//
// Clusters
//
//
static void Ctor_Gear(SoftDemo* pdemo,const btVector3& pos,btScalar speed)
{
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(pos);
btCompoundShape* shape=new btCompoundShape();
#if 1
shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btBoxShape(btVector3(5,1,6)));
shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(5,1,6)));
#else
shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btCylinderShapeZ(btVector3(5,1,7)));
shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(4,1,8)));
#endif
btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,shape);
body->setFriction(1);
btDynamicsWorld* world=pdemo->getDynamicsWorld();
btHingeConstraint* hinge=new btHingeConstraint(*body,btTransform::getIdentity());
if(speed!=0) hinge->enableAngularMotor(true,speed,3);
world->addConstraint(hinge);
}
//
static btSoftBody* Ctor_ClusterBunny(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,&gIndicesBunny[0][0],BUNNY_NUM_TRIANGLES);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_kLST = 1;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.piterations = 2;
psb->m_cfg.kDF = 1;
psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+
btSoftBody::fCollision::CL_RS;
psb->randomizeConstraints();
btMatrix3x3 m;
m.setEulerZYX(a.x(),a.y(),a.z());
psb->transform(btTransform(m,x));
psb->scale(btVector3(8,8,8));
psb->setTotalMass(150,true);
psb->generateClusters(1);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
}
//
static btSoftBody* Ctor_ClusterTorus(SoftDemo* pdemo,const btVector3& x,const btVector3& a,const btVector3& s=btVector3(2,2,2))
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,&gIndices[0][0],NUM_TRIANGLES);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_kLST = 1;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.piterations = 2;
psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+
btSoftBody::fCollision::CL_RS;
psb->randomizeConstraints();
psb->scale(s);
psb->rotate(btQuaternion(a[0],a[1],a[2]));
psb->translate(x);
psb->setTotalMass(50,true);
psb->generateClusters(64);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
}
//
static struct MotorControl : btSoftBody::AJoint::IControl
{
MotorControl()
{
goal=0;
maxtorque=0;
}
btScalar Speed(btSoftBody::AJoint*,btScalar current)
{
return(current+btMin(maxtorque,btMax(-maxtorque,goal-current)));
}
btScalar goal;
btScalar maxtorque;
} motorcontrol;
//
struct SteerControl : btSoftBody::AJoint::IControl
{
SteerControl(btScalar s)
{
angle=0;
sign=s;
}
void Prepare(btSoftBody::AJoint* joint)
{
joint->m_refs[0][0]=btCos(angle*sign);
joint->m_refs[0][2]=btSin(angle*sign);
}
btScalar Speed(btSoftBody::AJoint* joint,btScalar current)
{
return(motorcontrol.Speed(joint,current));
}
btScalar angle;
btScalar sign;
};
static SteerControl steercontrol_f(+1);
static SteerControl steercontrol_r(-1);
//
static void Init_ClusterDeform(SoftDemo* pdemo)
{
btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
psb->generateClusters(8);
psb->m_cfg.kDF=1;
}
//
static void Init_ClusterCollide1(SoftDemo* pdemo)
{
const btScalar s=8;
btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s),
btVector3(+s,0,-s),
btVector3(-s,0,+s),
btVector3(+s,0,+s),
31,31,
1+2+4+8,true);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_kLST = 0.4;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->m_cfg.kDF = 1;
psb->m_cfg.kSRHR_CL = 1;
psb->m_cfg.kSR_SPLT_CL = 0;
psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+
btSoftBody::fCollision::CL_RS;
psb->generateBendingConstraints(2,pm);
psb->setTotalMass(50);
psb->generateClusters(64);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
Ctor_RbUpStack(pdemo,10);
}
//
static void Init_ClusterCollide2(SoftDemo* pdemo)
{
struct Functor
{
static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,
&gIndices[0][0],
NUM_TRIANGLES);
psb->generateBendingConstraints(2);
psb->m_cfg.piterations=2;
psb->m_cfg.kSSHR_CL =1;
psb->m_cfg.kSS_SPLT_CL =0;
psb->m_cfg.collisions= btSoftBody::fCollision::CL_SS+
btSoftBody::fCollision::CL_RS;
psb->randomizeConstraints();
btMatrix3x3 m;
m.setEulerZYX(a.x(),a.y(),a.z());
psb->transform(btTransform(m,x));
psb->scale(btVector3(2,2,2));
psb->setTotalMass(50,true);
psb->generateClusters(16);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
}
};
for(int i=0;i<3;++i)
{
Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0));
}
}
//
static void Init_ClusterSocket(SoftDemo* pdemo)
{
btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
btRigidBody* prb=Ctor_BigPlate(pdemo,50,8);
psb->m_cfg.kDF=1;
btSoftBody::LJoint::Specs lj;
lj.position = btVector3(0,5,0);
psb->appendLinearJoint(lj,prb);
}
//
static void Init_ClusterHinge(SoftDemo* pdemo)
{
btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
btRigidBody* prb=Ctor_BigPlate(pdemo,50,8);
psb->m_cfg.kDF=1;
btSoftBody::AJoint::Specs aj;
aj.axis = btVector3(0,0,1);
psb->appendAngularJoint(aj,prb);
}
//
static void Init_ClusterCombine(SoftDemo* pdemo)
{
const btVector3 sz(2,4,2);
btSoftBody* psb0=Ctor_ClusterTorus(pdemo,btVector3(0,8,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz);
btSoftBody* psb1=Ctor_ClusterTorus(pdemo,btVector3(0,8,10),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz);
btSoftBody* psbs[]={psb0,psb1};
for(int j=0;j<2;++j)
{
psbs[j]->m_cfg.kDF=1;
psbs[j]->m_cfg.kDP=0;
psbs[j]->m_cfg.piterations=1;
psbs[j]->m_clusters[0]->m_matching = 0.05;
psbs[j]->m_clusters[0]->m_ndamping = 0.05;
}
btSoftBody::AJoint::Specs aj;
aj.axis = btVector3(0,0,1);
aj.icontrol = &motorcontrol;
psb0->appendAngularJoint(aj,psb1);
btSoftBody::LJoint::Specs lj;
lj.position = btVector3(0,8,5);
psb0->appendLinearJoint(lj,psb1);
}
//
static void Init_ClusterCar(SoftDemo* pdemo)
{
const btVector3 origin(100,80,0);
const btQuaternion orientation(-SIMD_PI/2,0,0);
const btScalar widthf=8;
const btScalar widthr=9;
const btScalar length=8;
const btScalar height=4;
const btVector3 wheels[]= {
btVector3(+widthf,-height,+length), // Front left
btVector3(-widthf,-height,+length), // Front right
btVector3(+widthr,-height,-length), // Rear left
btVector3(-widthr,-height,-length), // Rear right
};
btSoftBody* pa=Ctor_ClusterBunny(pdemo,btVector3(0,0,0),btVector3(0,0,0));
btSoftBody* pfl=Ctor_ClusterTorus(pdemo,wheels[0],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2));
btSoftBody* pfr=Ctor_ClusterTorus(pdemo,wheels[1],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2));
btSoftBody* prl=Ctor_ClusterTorus(pdemo,wheels[2],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2));
btSoftBody* prr=Ctor_ClusterTorus(pdemo,wheels[3],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2));
pfl->m_cfg.kDF =
pfr->m_cfg.kDF =
prl->m_cfg.kDF =
prr->m_cfg.kDF = 1;
btSoftBody::LJoint::Specs lspecs;
lspecs.cfm = 1;
lspecs.erp = 1;
lspecs.position = btVector3(0,0,0);
lspecs.position=wheels[0];pa->appendLinearJoint(lspecs,pfl);
lspecs.position=wheels[1];pa->appendLinearJoint(lspecs,pfr);
lspecs.position=wheels[2];pa->appendLinearJoint(lspecs,prl);
lspecs.position=wheels[3];pa->appendLinearJoint(lspecs,prr);
btSoftBody::AJoint::Specs aspecs;
aspecs.cfm = 1;
aspecs.erp = 1;
aspecs.axis = btVector3(1,0,0);
aspecs.icontrol = &steercontrol_f;
pa->appendAngularJoint(aspecs,pfl);
pa->appendAngularJoint(aspecs,pfr);
aspecs.icontrol = &motorcontrol;
pa->appendAngularJoint(aspecs,prl);
pa->appendAngularJoint(aspecs,prr);
pa->rotate(orientation);
pfl->rotate(orientation);
pfr->rotate(orientation);
prl->rotate(orientation);
prr->rotate(orientation);
pa->translate(origin);
pfl->translate(origin);
pfr->translate(origin);
prl->translate(origin);
prr->translate(origin);
pfl->m_cfg.piterations =
pfr->m_cfg.piterations =
prl->m_cfg.piterations =
prr->m_cfg.piterations = 1;
pfl->m_clusters[0]->m_matching =
pfr->m_clusters[0]->m_matching =
prl->m_clusters[0]->m_matching =
prr->m_clusters[0]->m_matching = 0.05;
pfl->m_clusters[0]->m_ndamping =
pfr->m_clusters[0]->m_ndamping =
prl->m_clusters[0]->m_ndamping =
prr->m_clusters[0]->m_ndamping = 0.05;
Ctor_LinearStair(pdemo,btVector3(0,-8,0),btVector3(3,2,40),0,20);
Ctor_RbUpStack(pdemo,50);
pdemo->m_autocam=true;
}
//
static void Init_ClusterRobot(SoftDemo* pdemo)
{
struct Functor
{
static btSoftBody* CreateBall(SoftDemo* pdemo,const btVector3& pos)
{
btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,pos,btVector3(1,1,1)*3,512);
psb->m_materials[0]->m_kLST = 0.45;
psb->m_cfg.kVC = 20;
psb->setTotalMass(50,true);
psb->setPose(true,false);
psb->generateClusters(1);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
}
};
const btVector3 base=btVector3(0,25,8);
btSoftBody* psb0=Functor::CreateBall(pdemo,base+btVector3(-8,0,0));
btSoftBody* psb1=Functor::CreateBall(pdemo,base+btVector3(+8,0,0));
btSoftBody* psb2=Functor::CreateBall(pdemo,base+btVector3(0,0,+8*btSqrt(2)));
const btVector3 ctr=(psb0->clusterCom(0)+psb1->clusterCom(0)+psb2->clusterCom(0))/3;
btCylinderShape* pshp=new btCylinderShape(btVector3(8,1,8));
btRigidBody* prb=pdemo->localCreateRigidBody(50,btTransform(btQuaternion(0,0,0),ctr+btVector3(0,5,0)),pshp);
btSoftBody::LJoint::Specs ls;
ls.erp=0.5f;
ls.position=psb0->clusterCom(0);psb0->appendLinearJoint(ls,prb);
ls.position=psb1->clusterCom(0);psb1->appendLinearJoint(ls,prb);
ls.position=psb2->clusterCom(0);psb2->appendLinearJoint(ls,prb);
btBoxShape* pbox=new btBoxShape(btVector3(20,1,40));
btRigidBody* pgrn=pdemo->localCreateRigidBody(0,btTransform(btQuaternion(0,-SIMD_HALF_PI/2,0),btVector3(0,0,0)),pbox);
pdemo->m_autocam=true;
}
//
static void Init_ClusterStackSoft(SoftDemo* pdemo)
{
for(int i=0;i<10;++i)
{
btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+8.25*i,0),btVector3(0,0,0));
psb->m_cfg.kDF=1;
}
}
//
static void Init_ClusterStackMixed(SoftDemo* pdemo)
{
for(int i=0;i<10;++i)
{
if((i+1)&1)
{
Ctor_BigPlate(pdemo,50,-9+4.25*i);
}
else
{
btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+4.25*i,0),btVector3(0,0,0));
psb->m_cfg.kDF=1;
}
}
}
unsigned current_demo=18;
void SoftDemo::clientResetScene()
{
@@ -880,6 +1254,12 @@ void SoftDemo::clientResetScene()
{
delete body->getMotionState();
}
while(m_dynamicsWorld->getNumConstraints())
{
btTypedConstraint* pc=m_dynamicsWorld->getConstraint(0);
m_dynamicsWorld->removeConstraint(pc);
delete pc;
}
btSoftBody* softBody = btSoftBody::upcast(obj);
if (softBody)
{
@@ -913,10 +1293,19 @@ void SoftDemo::clientResetScene()
Init_Bunny,
Init_BunnyMatch,
Init_Cutting1,
Init_ClusterDeform,
Init_ClusterCollide1,
Init_ClusterCollide2,
Init_ClusterSocket,
Init_ClusterHinge,
Init_ClusterCombine,
Init_ClusterCar,
Init_ClusterRobot,
Init_ClusterStackSoft,
Init_ClusterStackMixed,
};
current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0]));
m_softBodyWorldInfo.air_density = (btScalar)1.2;
m_softBodyWorldInfo.water_density = 0;
m_softBodyWorldInfo.water_offset = 0;
@@ -953,7 +1342,7 @@ void SoftDemo::renderme()
}
ps/=nps;
if(m_autocam)
m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01;
m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.05;
else
m_cameraTargetPosition=btVector3(0,0,0);
/* Anm */
@@ -1047,6 +1436,7 @@ void SoftDemo::renderme()
idraw->drawTriangle(o-x*s-y*s,o+x*s-y*s,o+x*s+y*s,c,a);
idraw->drawTriangle(o-x*s-y*s,o+x*s+y*s,o-x*s+y*s,c,a);
}
//
DemoApplication::renderme();
}
@@ -1055,10 +1445,15 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y)
{
switch(key)
{
case 'n': motorcontrol.maxtorque=10;motorcontrol.goal+=1;break;
case 'm': motorcontrol.maxtorque=10;motorcontrol.goal-=1;break;
case '4': steercontrol_f.angle+=0.1;steercontrol_r.angle+=0.1;break;
case '6': steercontrol_f.angle-=0.1;steercontrol_r.angle-=0.1;break;
case ']': ++current_demo;clientResetScene();break;
case '[': --current_demo;clientResetScene();break;
case ',': m_raycast=!m_raycast;break;
case ';': m_autocam=!m_autocam;break;
case 'c': getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()^fDrawFlags::Clusters);break;
case '`':
{
btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray();
@@ -1367,3 +1762,4 @@ void SoftDemo::exitPhysics()

2698
src/BulletSoftBody/btSoftBody.cpp
View File

File diff suppressed because it is too large Load Diff

329
src/BulletSoftBody/btSoftBody.h
View File

@@ -91,10 +91,12 @@ public:
///fCollision
struct fCollision { enum _ {
RVSmask = 0x000f, ///Rigid versus soft mask
SDF_RS = 0x0001, ///SDF base rigid vs soft
SDF_RS = 0x0001, ///SDF based rigid vs soft
CL_RS = 0x0002, ///Cluster vs convex rigid vs soft
SVSmask = 0x00f0, ///Rigid versus soft mask
VF_SS = 0x0010, ///Vertex vs face soft vs soft handling
CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling
/* presets */
Default = SDF_RS,
END
@@ -192,7 +194,7 @@ public:
btVector3 m_n; // Normal
btScalar m_im; // 1/mass
btScalar m_area; // Area
btDbvtNode* m_leaf; // Leaf data
btDbvtNode* m_leaf; // Leaf data
int m_battach:1; // Attached
};
/* Link */
@@ -212,12 +214,12 @@ public:
Node* m_n[3]; // Node pointers
btVector3 m_normal; // Normal
btScalar m_ra; // Rest area
btDbvtNode* m_leaf; // Leaf data
btDbvtNode* m_leaf; // Leaf data
};
/* RContact */
struct RContact
{
sCti m_cti; // Contact infos
btSoftBody::sCti m_cti; // Contact infos
Node* m_node; // Owner node
btMatrix3x3 m_c0; // Impulse matrix
btVector3 m_c1; // Relative anchor
@@ -268,6 +270,224 @@ public:
btMatrix3x3 m_scl; // Scale
btMatrix3x3 m_aqq; // Base scaling
};
/* Cluster */
struct Cluster
{
btAlignedObjectArray<Node*> m_nodes;
tScalarArray m_masses;
tVector3Array m_framerefs;
btTransform m_framexform;
btScalar m_idmass;
btScalar m_imass;
btMatrix3x3 m_locii;
btMatrix3x3 m_invwi;
btVector3 m_com;
btVector3 m_vimpulses[2];
btVector3 m_dimpulses[2];
int m_nvimpulses;
int m_ndimpulses;
btVector3 m_lv;
btVector3 m_av;
btDbvtNode* m_leaf;
btScalar m_ndamping;
btScalar m_ldamping;
btScalar m_adamping;
btScalar m_matching;
bool m_collide;
Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
};
/* Impulse */
struct Impulse
{
btVector3 m_velocity;
btVector3 m_drift;
int m_asVelocity:1;
int m_asDrift:1;
Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {}
Impulse operator -() const
{
Impulse i=*this;
i.m_velocity=-i.m_velocity;
i.m_drift=-i.m_drift;
return(i);
}
Impulse operator*(btScalar x) const
{
Impulse i=*this;
i.m_velocity*=x;
i.m_drift*=x;
return(i);
}
};
/* Body */
struct Body
{
btSoftBody::Cluster* m_soft;
btRigidBody* m_rigid;
Body() : m_soft(0),m_rigid(0) {}
Body(btSoftBody::Cluster* p) : m_soft(p),m_rigid(0) {}
Body(btRigidBody* p) : m_soft(0),m_rigid(p) {}
void activate() const
{
if(m_rigid) m_rigid->activate();
}
const btMatrix3x3& invWorldInertia() const
{
static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0);
if(m_rigid) return(m_rigid->getInvInertiaTensorWorld());
if(m_soft) return(m_soft->m_invwi);
return(iwi);
}
btScalar invMass() const
{
if(m_rigid) return(m_rigid->getInvMass());
if(m_soft) return(m_soft->m_imass);
return(0);
}
const btTransform& xform() const
{
static const btTransform identity=btTransform::getIdentity();
if(m_rigid) return(m_rigid->getInterpolationWorldTransform());
if(m_soft) return(m_soft->m_framexform);
return(identity);
}
btVector3 linearVelocity() const
{
if(m_rigid) return(m_rigid->getLinearVelocity());
if(m_soft) return(m_soft->m_lv);
return(btVector3(0,0,0));
}
btVector3 angularVelocity(const btVector3& rpos) const
{
if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos));
if(m_soft) return(cross(m_soft->m_av,rpos));
return(btVector3(0,0,0));
}
btVector3 angularVelocity() const
{
if(m_rigid) return(m_rigid->getAngularVelocity());
if(m_soft) return(m_soft->m_av);
return(btVector3(0,0,0));
}
btVector3 velocity(const btVector3& rpos) const
{
return(linearVelocity()+angularVelocity(rpos));
}
void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const
{
if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse);
}
void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const
{
if(m_rigid) m_rigid->applyImpulse(impulse,rpos);
if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse);
}
void applyImpulse(const Impulse& impulse,const btVector3& rpos) const
{
if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos);
if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos);
}
void applyVAImpulse(const btVector3& impulse) const
{
if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse);
}
void applyDAImpulse(const btVector3& impulse) const
{
if(m_rigid) m_rigid->applyTorqueImpulse(impulse);
if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse);
}
void applyAImpulse(const Impulse& impulse) const
{
if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity);
if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift);
}
void applyDCImpulse(const btVector3& impulse) const
{
if(m_rigid) m_rigid->applyCentralImpulse(impulse);
if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse);
}
};
/* Joint */
struct Joint
{
struct eType { enum _ {
Linear,
Angular,
Contact,
};};
struct Specs
{
Specs() : erp(1),cfm(1),split(1) {}
btScalar erp;
btScalar cfm;
btScalar split;
};
Body m_bodies[2];
btVector3 m_refs[2];
btScalar m_cfm;
btScalar m_erp;
btScalar m_split;
btVector3 m_drift;
btVector3 m_sdrift;
btMatrix3x3 m_massmatrix;
bool m_delete;
virtual ~Joint() {}
Joint() : m_delete(false) {}
virtual void Prepare(btScalar dt,int iterations);
virtual void Solve(btScalar dt,btScalar sor)=0;
virtual void Terminate(btScalar dt)=0;
virtual eType::_ Type() const=0;
};
/* LJoint */
struct LJoint : Joint
{
struct Specs : Joint::Specs
{
btVector3 position;
};
btVector3 m_rpos[2];
void Prepare(btScalar dt,int iterations);
void Solve(btScalar dt,btScalar sor);
void Terminate(btScalar dt);
eType::_ Type() const { return(eType::Linear); }
};
/* AJoint */
struct AJoint : Joint
{
struct IControl
{
virtual void Prepare(AJoint*) {}
virtual btScalar Speed(AJoint*,btScalar current) { return(current); }
static IControl* Default() { static IControl def;return(&def); }
};
struct Specs : Joint::Specs
{
Specs() : icontrol(IControl::Default()) {}
btVector3 axis;
IControl* icontrol;
};
btVector3 m_axis[2];
IControl* m_icontrol;
void Prepare(btScalar dt,int iterations);
void Solve(btScalar dt,btScalar sor);
void Terminate(btScalar dt);
eType::_ Type() const { return(eType::Angular); }
};
/* CJoint */
struct CJoint : Joint
{
int m_life;
int m_maxlife;
btVector3 m_rpos[2];
btVector3 m_normal;
btScalar m_friction;
void Prepare(btScalar dt,int iterations);
void Solve(btScalar dt,btScalar sor);
void Terminate(btScalar dt);
eType::_ Type() const { return(eType::Contact); }
};
/* Config */
struct Config
{
@@ -284,11 +504,18 @@ public:
btScalar kKHR; // Kinetic contacts hardness [0,1]
btScalar kSHR; // Soft contacts hardness [0,1]
btScalar kAHR; // Anchors hardness [0,1]
btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only)
btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only)
btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only)
btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only)
btScalar maxvolume; // Maximum volume ratio for pose
btScalar timescale; // Time scale
int viterations; // Velocities solver iterations
int piterations; // Positions solver iterations
int diterations; // Drift solver iterations
int citerations; // Cluster solver iterations
int collisions; // Collisions flags
tVSolverArray m_vsequence; // Velocity solvers sequence
tPSolverArray m_psequence; // Position solvers sequence
@@ -302,12 +529,32 @@ public:
btScalar velmrg; // velocity margin
btScalar radmrg; // radial margin
btScalar updmrg; // Update margin
};
};
/* RayCaster */
struct RayCaster : btDbvt::ICollide
{
btVector3 o;
btVector3 d;
btScalar mint;
btSoftBody::Face* face;
int tests;
RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt);
void Process(const btDbvtNode* leaf);
static inline btScalar rayTriangle(const btVector3& org,
const btVector3& dir,
const btVector3& a,
const btVector3& b,
const btVector3& c,
btScalar maxt=SIMD_INFINITY);
};
//
// Typedef's
//
typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar);
typedef void (*vsolver_t)(btSoftBody*,btScalar);
typedef btAlignedObjectArray<Cluster*> tClusterArray;
typedef btAlignedObjectArray<Note> tNoteArray;
typedef btAlignedObjectArray<Node> tNodeArray;
typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
@@ -317,7 +564,8 @@ public:
typedef btAlignedObjectArray<RContact> tRContactArray;
typedef btAlignedObjectArray<SContact> tSContactArray;
typedef btAlignedObjectArray<Material*> tMaterialArray;
typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
typedef btAlignedObjectArray<Joint*> tJointArray;
typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
//
// Fields
@@ -327,7 +575,7 @@ public:
SolverState m_sst; // Solver state
Pose m_pose; // Pose
void* m_tag; // User data
btSoftBodyWorldInfo* m_worldInfo; //
btSoftBodyWorldInfo* m_worldInfo; // World info
tNoteArray m_notes; // Notes
tNodeArray m_nodes; // Nodes
tLinkArray m_links; // Links
@@ -335,18 +583,22 @@ public:
tAnchorArray m_anchors; // Anchors
tRContactArray m_rcontacts; // Rigid contacts
tSContactArray m_scontacts; // Soft contacts
tJointArray m_joints; // Joints
tMaterialArray m_materials; // Materials
btScalar m_timeacc; // Time accumulator
btVector3 m_bounds[2]; // Spatial bounds
bool m_bUpdateRtCst; // Update runtime constants
btDbvt m_ndbvt; // Nodes tree
btDbvt m_fdbvt; // Faces tree
btDbvt m_cdbvt; // Clusters tree
tClusterArray m_clusters; // Clusters
//
// Api
//
/* ctor */
btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count,
btSoftBody( btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count,
const btVector3* x,
const btScalar* m);
/* dtor */
@@ -400,6 +652,14 @@ public:
/* Append anchor */
void appendAnchor( int node,
btRigidBody* body);
/* Append linear joint */
void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1);
void appendLinearJoint(const LJoint::Specs& specs,Body body=Body());
void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body);
/* Append linear joint */
void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1);
void appendAngularJoint(const AJoint::Specs& specs,Body body=Body());
void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body);
/* Add force (or gravity) to the entire body */
void addForce( const btVector3& force);
/* Add force (or gravity) to a node of the body */
@@ -435,11 +695,28 @@ public:
bool bframe);
/* Return the volume */
btScalar getVolume() const;
/* Cluster count */
int clusterCount() const;
/* Cluster center of mass */
static btVector3 clusterCom(const Cluster* cluster);
btVector3 clusterCom(int cluster) const;
/* Cluster velocity at rpos */
static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos);
/* Cluster impulse */
static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse);
static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse);
static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse);
static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse);
static void clusterAImpulse(Cluster* cluster,const Impulse& impulse);
static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse);
/* Generate bending constraints based on distance in the adjency graph */
int generateBendingConstraints( int distance,
Material* mat=0);
/* Randomize constraints to reduce solver bias */
void randomizeConstraints();
/* Generate clusters (K-mean) */
int generateClusters(int k,int maxiterations=8192);
/* Refine */
void refine(ImplicitFn* ifn,btScalar accurary,bool cut);
/* CutLink */
@@ -460,6 +737,8 @@ public:
void staticSolve(int iterations);
/* solveCommonConstraints */
static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations);
/* solveClusters */
static void solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies);
/* integrateMotion */
void integrateMotion();
/* defaultCollisionHandlers */
@@ -483,12 +762,44 @@ public:
return 0;
}
//
// ::btCollisionObject
//
virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const
{
aabbMin = m_bounds[0];
aabbMax = m_bounds[1];
}
//
// Private
//
void pointersToIndices();
void indicesToPointers(const int* map=0);
int rayCast(const btVector3& org,const btVector3& dir,
btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const;
void initializeFaceTree();
btVector3 evaluateCom() const;
bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const;
void updateNormals();
void updateBounds();
void updatePose();
void updateConstants();
void initializeClusters();
void updateClusters();
void cleanupClusters();
void prepareClusters(int iterations);
void solveClusters(btScalar sor);
void applyClusters(bool drift);
void dampClusters();
void applyForces();
static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti);
static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti);
static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti);
static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti);
static void VSolve_Links(btSoftBody* psb,btScalar kst);
static psolver_t getSolver(ePSolver::_ solver);
static vsolver_t getSolver(eVSolver::_ solver);
};

185
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
///btSoftBodyHelpers.cpp by Nathanael Presson
#include "btSoftBody.h"
#include "btSoftBodyInternals.h"
#include <stdio.h>
#include <string.h>
#include "btSoftBodyHelpers.h"
@@ -78,6 +78,49 @@ if(node)
}
//
template <typename T>
static inline T sum(const btAlignedObjectArray<T>& items)
{
T v;
if(items.size())
{
v=items[0];
for(int i=1,ni=items.size();i<ni;++i)
{
v+=items[i];
}
}
return(v);
}
//
template <typename T,typename Q>
static inline void add(btAlignedObjectArray<T>& items,const Q& value)
{
for(int i=0,ni=items.size();i<ni;++i)
{
items[i]+=value;
}
}
//
template <typename T,typename Q>
static inline void mul(btAlignedObjectArray<T>& items,const Q& value)
{
for(int i=0,ni=items.size();i<ni;++i)
{
items[i]*=value;
}
}
//
template <typename T>
static inline T average(const btAlignedObjectArray<T>& items)
{
const btScalar n=(btScalar)(items.size()>0?items.size():1);
return(sum(items)/n);
}
//
static inline btScalar tetravolume(const btVector3& x0,
const btVector3& x1,
@@ -175,9 +218,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
/* Anchors */
if(0!=(drawflags&fDrawFlags::Anchors))
{
int i;
for(i=0;i<psb->m_anchors.size();++i)
for(int i=0;i<psb->m_anchors.size();++i)
{
const btSoftBody::Anchor& a=psb->m_anchors[i];
const btVector3 q=a.m_body->getWorldTransform()*a.m_local;
@@ -185,7 +226,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
drawVertex(idraw,q,0.25,btVector3(0,1,0));
idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));
}
for(i=0;i<psb->m_nodes.size();++i)
for(int i=0;i<psb->m_nodes.size();++i)
{
const btSoftBody::Node& n=psb->m_nodes[i];
if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
@@ -207,20 +248,66 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};
const btVector3 c=(x[0]+x[1]+x[2])/3;
/*idraw->drawLine((x[0]-c)*scl+c,(x[1]-c)*scl+c,col);
idraw->drawLine((x[1]-c)*scl+c,(x[2]-c)*scl+c,col);
idraw->drawLine((x[2]-c)*scl+c,(x[0]-c)*scl+c,col);*/
idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
col,alp);
/*idraw->drawTriangle((x[0]-c)*scl+c,
(x[1]-c)*scl+c,
(x[2]-c)*scl+c,
f.m_n[0]->m_n,f.m_n[1]->m_n,f.m_n[2]->m_n,
col,alp);*/
}
}
/* Clusters */
if(0!=(drawflags&fDrawFlags::Clusters))
{
srand(1806);
for(int i=0;i<psb->m_clusters.size();++i)
{
if(psb->m_clusters[i]->m_collide)
{
btVector3 color( rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX,
rand()/(btScalar)RAND_MAX);
color=color.normalized()*0.75;
btAlignedObjectArray<btVector3> vertices;
vertices.resize(psb->m_clusters[i]->m_nodes.size());
for(int j=0,nj=vertices.size();j<nj;++j)
{
vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;
}
HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);
HullResult hres;
HullLibrary hlib;
hdsc.mMaxVertices=vertices.size();
hlib.CreateConvexHull(hdsc,hres);
const btVector3 center=average(hres.m_OutputVertices);
add(hres.m_OutputVertices,-center);
mul(hres.m_OutputVertices,(btScalar)1);
add(hres.m_OutputVertices,center);
for(int j=0;j<(int)hres.mNumFaces;++j)
{
const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};
idraw->drawTriangle(hres.m_OutputVertices[idx[0]],
hres.m_OutputVertices[idx[1]],
hres.m_OutputVertices[idx[2]],
color,1);
}
hlib.ReleaseResult(hres);
}
/* Velocities */
#if 0
for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)
{
const btSoftBody::Cluster& c=psb->m_clusters[i];
const btVector3 r=c.m_nodes[j]->m_x-c.m_com;
const btVector3 v=c.m_lv+cross(c.m_av,r);
idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));
}
#endif
/* Frame */
btSoftBody::Cluster& c=*psb->m_clusters[i];
idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));
idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));
idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));
}
}
/* Notes */
if(0!=(drawflags&fDrawFlags::Notes))
{
@@ -235,6 +322,46 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
idraw->draw3dText(p,n.m_text);
}
}
/* Node tree */
if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);
/* Face tree */
if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);
/* Cluster tree */
if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);
/* Joints */
if(0!=(drawflags&fDrawFlags::Joints))
{
for(int i=0;i<psb->m_joints.size();++i)
{
const btSoftBody::Joint* pj=psb->m_joints[i];
switch(pj->Type())
{
case btSoftBody::Joint::eType::Linear:
{
const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;
const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];
idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));
idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));
drawVertex(idraw,a0,0.25,btVector3(1,1,0));
drawVertex(idraw,a1,0.25,btVector3(0,1,1));
}
break;
case btSoftBody::Joint::eType::Angular:
{
const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;
const btVector3 o0=pj->m_bodies[0].xform().getOrigin();
const btVector3 o1=pj->m_bodies[1].xform().getOrigin();
const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];
const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];
idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));
idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));
idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));
idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));
}
}
}
}
}
//
@@ -281,6 +408,15 @@ void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,
drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);
}
//
void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb,
btIDebugDraw* idraw,
int mindepth,
int maxdepth)
{
drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);
}
//
void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw)
@@ -315,9 +451,7 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor
const int r=res+2;
btVector3* x=new btVector3[r];
btScalar* m=new btScalar[r];
int i;
for(i=0;i<r;++i)
for(int i=0;i<r;++i)
{
const btScalar t=i/(btScalar)(r-1);
x[i]=lerp(from,to,t);
@@ -329,7 +463,7 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBody::btSoftBodyWorldInfo& wor
delete[] x;
delete[] m;
/* Create links */
for(i=1;i<r;++i)
for(int i=1;i<r;++i)
{
psb->appendLink(i-1,i);
}
@@ -355,9 +489,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor
const int tot=rx*ry;
btVector3* x=new btVector3[tot];
btScalar* m=new btScalar[tot];
int iy;
for(iy=0;iy<ry;++iy)
for(int iy=0;iy<ry;++iy)
{
const btScalar ty=iy/(btScalar)(ry-1);
const btVector3 py0=lerp(corner00,corner01,ty);
@@ -377,7 +509,7 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBody::btSoftBodyWorldInfo& wor
delete[] x;
delete[] m;
/* Create links and faces */
for(iy=0;iy<ry;++iy)
for(int iy=0;iy<ry;++iy)
{
for(int ix=0;ix<rx;++ix)
{
@@ -452,10 +584,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
int ntriangles)
{
int maxidx=0;
int i,j;
int ni;
for( i=0,ni=ntriangles*3;i<ni;++i)
for(int i=0,ni=ntriangles*3;i<ni;++i)
{
maxidx=btMax(triangles[i],maxidx);
}
@@ -464,12 +593,12 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
btAlignedObjectArray<btVector3> vtx;
chks.resize(maxidx*maxidx,false);
vtx.resize(maxidx);
for( i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
for(int i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)
{
vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);
}
btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);
for(i=0,ni=ntriangles*3;i<ni;i+=3)
for(int i=0,ni=ntriangles*3;i<ni;i+=3)
{
const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};
#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))
@@ -513,4 +642,4 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
hlib.ReleaseResult(hres);
psb->randomizeConstraints();
return(psb);
}
}

12
src/BulletSoftBody/btSoftBodyHelpers.h
View File

@@ -32,8 +32,13 @@ struct fDrawFlags { enum _ {
Contacts = 0x0020,
Anchors = 0x0040,
Notes = 0x0080,
Clusters = 0x0100,
NodeTree = 0x0200,
FaceTree = 0x0400,
ClusterTree = 0x0800,
Joints = 0x1000,
/* presets */
Std = Links+Faces+Tetras+Anchors+Notes,
Std = Links+Faces+Tetras+Anchors+Notes+Joints,
StdTetra = Std-Faces+Tetras,
};};
@@ -59,6 +64,11 @@ struct btSoftBodyHelpers
btIDebugDraw* idraw,
int mindepth=0,
int maxdepth=-1);
/* Draw cluster tree */
static void DrawClusterTree(btSoftBody* psb,
btIDebugDraw* idraw,
int mindepth=0,
int maxdepth=-1);
/* Draw rigid frame */
static void DrawFrame( btSoftBody* psb,
btIDebugDraw* idraw);

891
src/BulletSoftBody/btSoftBodyInternals.h Normal file
View File

@@ -0,0 +1,891 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btSoftBody implementation by Nathanael Presson
#ifndef _BT_SOFT_BODY_INTERNALS_H
#define _BT_SOFT_BODY_INTERNALS_H
#include "btSoftBody.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
//
// btSymMatrix
//
template <typename T>
struct btSymMatrix
{
btSymMatrix() : dim(0) {}
btSymMatrix(int n,const T& init=T()) { resize(n,init); }
void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); }
int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
T& operator()(int c,int r) { return(store[index(c,r)]); }
const T& operator()(int c,int r) const { return(store[index(c,r)]); }
btAlignedObjectArray<T> store;
int dim;
};
//
// btSoftBodyCollisionShape
//
class btSoftBodyCollisionShape : public btConcaveShape
{
public:
btSoftBody* m_body;
btSoftBodyCollisionShape(btSoftBody* backptr)
{
m_body=backptr;
}
virtual ~btSoftBodyCollisionShape()
{
}
void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
{
//not yet
btAssert(0);
}
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
/* t should be identity, but better be safe than...fast? */
const btVector3 mins=m_body->m_bounds[0];
const btVector3 maxs=m_body->m_bounds[1];
const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()),
t*btVector3(maxs.x(),mins.y(),mins.z()),
t*btVector3(maxs.x(),maxs.y(),mins.z()),
t*btVector3(mins.x(),maxs.y(),mins.z()),
t*btVector3(mins.x(),mins.y(),maxs.z()),
t*btVector3(maxs.x(),mins.y(),maxs.z()),
t*btVector3(maxs.x(),maxs.y(),maxs.z()),
t*btVector3(mins.x(),maxs.y(),maxs.z())};
aabbMin=aabbMax=crns[0];
for(int i=1;i<8;++i)
{
aabbMin.setMin(crns[i]);
aabbMax.setMax(crns[i]);
}
}
virtual int getShapeType() const
{
return SOFTBODY_SHAPE_PROXYTYPE;
}
virtual void setLocalScaling(const btVector3& /*scaling*/)
{
///na
btAssert(0);
}
virtual const btVector3& getLocalScaling() const
{
static const btVector3 dummy(1,1,1);
return dummy;
}
virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
{
///not yet
btAssert(0);
}
virtual const char* getName()const
{
return "SoftBody";
}
};
//
// btSoftClusterCollisionShape
//
class btSoftClusterCollisionShape : public btConvexInternalShape
{
public:
const btSoftBody::Cluster* m_cluster;
btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); }
virtual btVector3 localGetSupportingVertex(const btVector3& vec) const
{
btSoftBody::Node* const * n=&m_cluster->m_nodes[0];
btScalar d=dot(vec,n[0]->m_x);
int j=0;
for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
{
const btScalar k=dot(vec,n[i]->m_x);
if(k>d) { d=k;j=i; }
}
return(n[j]->m_x);
}
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
return(localGetSupportingVertex(vec));
}
//notice that the vectors should be unit length
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{}
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
{}
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{}
virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; }
//debugging
virtual const char* getName()const {return "SOFTCLUSTER";}
virtual void setMargin(btScalar margin)
{
btConvexInternalShape::setMargin(margin);
}
virtual btScalar getMargin() const
{
return getMargin();
}
};
//
// Inline's
//
//
template <typename T>
static inline void ZeroInitialize(T& value)
{
static const T zerodummy;
value=zerodummy;
}
//
template <typename T>
static inline bool CompLess(const T& a,const T& b)
{ return(a<b); }
//
template <typename T>
static inline bool CompGreater(const T& a,const T& b)
{ return(a>b); }
//
template <typename T>
static inline T Lerp(const T& a,const T& b,btScalar t)
{ return(a+(b-a)*t); }
//
template <typename T>
static inline T InvLerp(const T& a,const T& b,btScalar t)
{ return((b+a*t-b*t)/(a*b)); }
//
static inline btMatrix3x3 Lerp( const btMatrix3x3& a,
const btMatrix3x3& b,
btScalar t)
{
btMatrix3x3 r;
r[0]=Lerp(a[0],b[0],t);
r[1]=Lerp(a[1],b[1],t);
r[2]=Lerp(a[2],b[2],t);
return(r);
}
//
static inline btVector3 Clamp(const btVector3& v,btScalar maxlength)
{
const btScalar sql=v.length2();
if(sql>(maxlength*maxlength))
return((v*maxlength)/btSqrt(sql));
else
return(v);
}
//
template <typename T>
static inline T Clamp(const T& x,const T& l,const T& h)
{ return(x<l?l:x>h?h:x); }
//
template <typename T>
static inline T Sq(const T& x)
{ return(x*x); }
//
template <typename T>
static inline T Cube(const T& x)
{ return(x*x*x); }
//
template <typename T>
static inline T Sign(const T& x)
{ return((T)(x<0?-1:+1)); }
//
template <typename T>
static inline bool SameSign(const T& x,const T& y)
{ return((x*y)>0); }
//
static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y)
{
const btVector3 d=x-y;
return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2]));
}
//
static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s)
{
const btScalar xx=a.x()*a.x();
const btScalar yy=a.y()*a.y();
const btScalar zz=a.z()*a.z();
const btScalar xy=a.x()*a.y();
const btScalar yz=a.y()*a.z();
const btScalar zx=a.z()*a.x();
btMatrix3x3 m;
m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx);
m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz);
m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s);
return(m);
}
//
static inline btMatrix3x3 Cross(const btVector3& v)
{
btMatrix3x3 m;
m[0]=btVector3(0,-v.z(),+v.y());
m[1]=btVector3(+v.z(),0,-v.x());
m[2]=btVector3(-v.y(),+v.x(),0);
return(m);
}
//
static inline btMatrix3x3 Diagonal(btScalar x)
{
btMatrix3x3 m;
m[0]=btVector3(x,0,0);
m[1]=btVector3(0,x,0);
m[2]=btVector3(0,0,x);
return(m);
}
//
static inline btMatrix3x3 Add(const btMatrix3x3& a,
const btMatrix3x3& b)
{
btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]+b[i];
return(r);
}
//
static inline btMatrix3x3 Sub(const btMatrix3x3& a,
const btMatrix3x3& b)
{
btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]-b[i];
return(r);
}
//
static inline btMatrix3x3 Mul(const btMatrix3x3& a,
btScalar b)
{
btMatrix3x3 r;
for(int i=0;i<3;++i) r[i]=a[i]*b;
return(r);
}
//
static inline void Orthogonalize(btMatrix3x3& m)
{
m[2]=cross(m[0],m[1]).normalized();
m[1]=cross(m[2],m[0]).normalized();
m[0]=cross(m[1],m[2]).normalized();
}
//
static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r)
{
const btMatrix3x3 cr=Cross(r);
return(Sub(Diagonal(im),cr*iwi*cr));
}
//
static inline btMatrix3x3 ImpulseMatrix( btScalar dt,
btScalar ima,
btScalar imb,
const btMatrix3x3& iwi,
const btVector3& r)
{
return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse());
}
//
static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra,
btScalar imb,const btMatrix3x3& iib,const btVector3& rb)
{
return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse());
}
//
static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia,
const btMatrix3x3& iib)
{
return(Add(iia,iib).inverse());
}
//
static inline btVector3 ProjectOnAxis( const btVector3& v,
const btVector3& a)
{
return(a*dot(v,a));
}
//
static inline btVector3 ProjectOnPlane( const btVector3& v,
const btVector3& a)
{
return(v-ProjectOnAxis(v,a));
}
//
static inline void ProjectOrigin( const btVector3& a,
const btVector3& b,
btVector3& prj,
btScalar& sqd)
{
const btVector3 d=b-a;
const btScalar m2=d.length2();
if(m2>SIMD_EPSILON)
{
const btScalar t=Clamp<btScalar>(-dot(a,d)/m2,0,1);
const btVector3 p=a+d*t;
const btScalar l2=p.length2();
if(l2<sqd)
{
prj=p;
sqd=l2;
}
}
}
//
static inline void ProjectOrigin( const btVector3& a,
const btVector3& b,
const btVector3& c,
btVector3& prj,
btScalar& sqd)
{
const btVector3& q=cross(b-a,c-a);
const btScalar m2=q.length2();
if(m2>SIMD_EPSILON)
{
const btVector3 n=q/btSqrt(m2);
const btScalar k=dot(a,n);
const btScalar k2=k*k;
if(k2<sqd)
{
const btVector3 p=n*k;
if( (dot(cross(a-p,b-p),q)>0)&&
(dot(cross(b-p,c-p),q)>0)&&
(dot(cross(c-p,a-p),q)>0))
{
prj=p;
sqd=k2;
}
else
{
ProjectOrigin(a,b,prj,sqd);
ProjectOrigin(b,c,prj,sqd);
ProjectOrigin(c,a,prj,sqd);
}
}
}
}
//
template <typename T>
static inline T BaryEval( const T& a,
const T& b,
const T& c,
const btVector3& coord)
{
return(a*coord.x()+b*coord.y()+c*coord.z());
}
//
static inline btVector3 BaryCoord( const btVector3& a,
const btVector3& b,
const btVector3& c,
const btVector3& p)
{
const btScalar w[]={ cross(a-p,b-p).length(),
cross(b-p,c-p).length(),
cross(c-p,a-p).length()};
const btScalar isum=1/(w[0]+w[1]+w[2]);
return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum));
}
//
static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn,
const btVector3& a,
const btVector3& b,
const btScalar accuracy,
const int maxiterations=256)
{
btScalar span[2]={0,1};
btScalar values[2]={fn->Eval(a),fn->Eval(b)};
if(values[0]>values[1])
{
btSwap(span[0],span[1]);
btSwap(values[0],values[1]);
}
if(values[0]>-accuracy) return(-1);
if(values[1]<+accuracy) return(-1);
for(int i=0;i<maxiterations;++i)
{
const btScalar t=Lerp(span[0],span[1],values[0]/(values[0]-values[1]));
const btScalar v=fn->Eval(Lerp(a,b,t));
if((t<=0)||(t>=1)) break;
if(btFabs(v)<accuracy) return(t);
if(v<0)
{ span[0]=t;values[0]=v; }
else
{ span[1]=t;values[1]=v; }
}
return(-1);
}
//
static inline btVector3 NormalizeAny(const btVector3& v)
{
const btScalar l=v.length();
if(l>SIMD_EPSILON)
return(v/l);
else
return(btVector3(0,0,0));
}
//
static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f,
btScalar margin)
{
const btVector3* pts[]={ &f.m_n[0]->m_x,
&f.m_n[1]->m_x,
&f.m_n[2]->m_x};
btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3);
vol.Expand(btVector3(margin,margin,margin));
return(vol);
}
//
static inline btVector3 CenterOf( const btSoftBody::Face& f)
{
return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3);
}
//
static inline btScalar AreaOf( const btVector3& x0,
const btVector3& x1,
const btVector3& x2)
{
const btVector3 a=x1-x0;
const btVector3 b=x2-x0;
const btVector3 cr=cross(a,b);
const btScalar area=cr.length();
return(area);
}
//
static inline btScalar VolumeOf( const btVector3& x0,
const btVector3& x1,
const btVector3& x2,
const btVector3& x3)
{
const btVector3 a=x1-x0;
const btVector3 b=x2-x0;
const btVector3 c=x3-x0;
return(dot(a,cross(b,c)));
}
//
static void EvaluateMedium( const btSoftBody::btSoftBodyWorldInfo* wfi,
const btVector3& x,
btSoftBody::sMedium& medium)
{
medium.m_velocity = btVector3(0,0,0);
medium.m_pressure = 0;
medium.m_density = wfi->air_density;
if(wfi->water_density>0)
{
const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset);
if(depth>0)
{
medium.m_density = wfi->water_density;
medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length();
}
}
}
//
static inline void ApplyClampedForce( btSoftBody::Node& n,
const btVector3& f,
btScalar dt)
{
const btScalar dtim=dt*n.m_im;
if((f*dtim).length2()>n.m_v.length2())
{/* Clamp */
n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim;
}
else
{/* Apply */
n.m_f+=f;
}
}
//
static inline int MatchEdge( const btSoftBody::Node* a,
const btSoftBody::Node* b,
const btSoftBody::Node* ma,
const btSoftBody::Node* mb)
{
if((a==ma)&&(b==mb)) return(0);
if((a==mb)&&(b==ma)) return(1);
return(-1);
}
//
// btEigen : Extract eigen system,
// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html
// outputs are NOT sorted.
//
struct btEigen
{
static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0)
{
static const int maxiterations=16;
static const btScalar accuracy=(btScalar)0.0001;
btMatrix3x3& v=*vectors;
int iterations=0;
vectors->setIdentity();
do {
int p=0,q=1;
if(btFabs(a[p][q])<btFabs(a[0][2])) { p=0;q=2; }
if(btFabs(a[p][q])<btFabs(a[1][2])) { p=1;q=2; }
if(btFabs(a[p][q])>accuracy)
{
const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]);
const btScalar z=btFabs(w);
const btScalar t=w/(z*(btSqrt(1+w*w)+z));
if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */
{
const btScalar c=1/btSqrt(t*t+1);
const btScalar s=c*t;
mulPQ(a,c,s,p,q);
mulTPQ(a,c,s,p,q);
mulPQ(v,c,s,p,q);
} else break;
} else break;
} while((++iterations)<maxiterations);
if(values)
{
*values=btVector3(a[0][0],a[1][1],a[2][2]);
}
return(iterations);
}
private:
static inline void mulTPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
{
const btScalar m[2][3]={ {a[p][0],a[p][1],a[p][2]},
{a[q][0],a[q][1],a[q][2]}};
for(int i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
for(int i=0;i<3;++i) a[q][i]=c*m[1][i]+s*m[0][i];
}
static inline void mulPQ(btMatrix3x3& a,btScalar c,btScalar s,int p,int q)
{
const btScalar m[2][3]={ {a[0][p],a[1][p],a[2][p]},
{a[0][q],a[1][q],a[2][q]}};
for(int i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
for(int i=0;i<3;++i) a[i][q]=c*m[1][i]+s*m[0][i];
}
};
//
// Polar decomposition,
// "Computing the Polar Decomposition with Applications", Nicholas J. Higham, 1986.
//
static inline int PolarDecompose( const btMatrix3x3& m,btMatrix3x3& q,btMatrix3x3& s)
{
static const btScalar half=(btScalar)0.5;
static const btScalar accuracy=(btScalar)0.0001;
static const int maxiterations=16;
int i=0;
btScalar det=0;
q = Mul(m,1/btVector3(m[0][0],m[1][1],m[2][2]).length());
det = q.determinant();
if(!btFuzzyZero(det))
{
for(;i<maxiterations;++i)
{
q=Mul(Add(q,Mul(q.adjoint(),1/det).transpose()),half);
const btScalar ndet=q.determinant();
if(Sq(ndet-det)>accuracy) det=ndet; else break;
}
/* Final orthogonalization */
Orthogonalize(q);
/* Compute 'S' */
s=q.transpose()*m;
}
else
{
q.setIdentity();
s.setIdentity();
}
return(i);
}
//
// btSoftColliders
//
struct btSoftColliders
{
//
// ClusterBase
//
struct ClusterBase : btDbvt::ICollide
{
btScalar erp;
btScalar idt;
btScalar margin;
btScalar friction;
btScalar threshold;
ClusterBase()
{
erp =(btScalar)1;
idt =0;
margin =0;
friction =0;
threshold =(btScalar)0;
}
bool SolveContact( const btGjkEpaSolver2::sResults& res,
btSoftBody::Body ba,btSoftBody::Body bb,
btSoftBody::CJoint& joint)
{
if(res.distance<margin)
{
const btVector3 ra=res.witnesses[0]-ba.xform().getOrigin();
const btVector3 rb=res.witnesses[1]-bb.xform().getOrigin();
const btVector3 va=ba.velocity(ra);
const btVector3 vb=bb.velocity(rb);
const btVector3 vrel=va-vb;
const btScalar rvac=dot(vrel,res.normal);
const btScalar depth=res.distance-margin;
const btVector3 iv=res.normal*rvac;
const btVector3 fv=vrel-iv;
joint.m_bodies[0] = ba;
joint.m_bodies[1] = bb;
joint.m_refs[0] = ra*ba.xform().getBasis();
joint.m_refs[1] = rb*bb.xform().getBasis();
joint.m_rpos[0] = ra;
joint.m_rpos[1] = rb;
joint.m_cfm = 1;
joint.m_erp = 1;
joint.m_life = 0;
joint.m_maxlife = 0;
joint.m_split = 1;
joint.m_drift = depth*res.normal;
joint.m_normal = res.normal;
joint.m_delete = false;
joint.m_friction = fv.length2()<(-rvac*friction)?1:friction;
joint.m_massmatrix = ImpulseMatrix( ba.invMass(),ba.invWorldInertia(),joint.m_rpos[0],
bb.invMass(),bb.invWorldInertia(),joint.m_rpos[1]);
return(true);
}
return(false);
}
};
//
// CollideCL_RS
//
struct CollideCL_RS : ClusterBase
{
btSoftBody* psb;
btRigidBody* prb;
void Process(const btDbvtNode* leaf)
{
btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data;
btSoftClusterCollisionShape cshape(cluster);
const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape();
btGjkEpaSolver2::sResults res;
if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
rshape,prb->getInterpolationWorldTransform(),
btVector3(1,0,0),res))
{
btSoftBody::CJoint joint;
if(SolveContact(res,cluster,prb,joint))
{
btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
*pj=joint;psb->m_joints.push_back(pj);
if(prb->isStaticOrKinematicObject())
{
pj->m_erp *= psb->m_cfg.kSKHR_CL;
pj->m_split *= psb->m_cfg.kSK_SPLT_CL;
}
else
{
pj->m_erp *= psb->m_cfg.kSRHR_CL;
pj->m_split *= psb->m_cfg.kSR_SPLT_CL;
}
}
}
}
void Process(btSoftBody* ps,btRigidBody* pr)
{
psb = ps;
prb = pr;
idt = ps->m_sst.isdt;
margin = ps->getCollisionShape()->getMargin()+
pr->getCollisionShape()->getMargin();
friction = btMin(psb->m_cfg.kDF,prb->getFriction());
btVector3 mins;
btVector3 maxs;
btDbvtVolume volume;
pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs);
volume=btDbvtVolume::FromMM(mins,maxs);
volume.Expand(btVector3(1,1,1)*margin);
btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this);
}
};
//
// CollideCL_SS
//
struct CollideCL_SS : ClusterBase
{
btSoftBody* bodies[2];
void Process(const btDbvtNode* la,const btDbvtNode* lb)
{
btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data;
btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data;
btSoftClusterCollisionShape csa(cla);
btSoftClusterCollisionShape csb(clb);
btGjkEpaSolver2::sResults res;
if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(),
&csb,btTransform::getIdentity(),
cla->m_com-clb->m_com,res))
{
btSoftBody::CJoint joint;
if(SolveContact(res,cla,clb,joint))
{
btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
*pj=joint;bodies[0]->m_joints.push_back(pj);
pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
}
}
}
void Process(btSoftBody* psa,btSoftBody* psb)
{
idt = psa->m_sst.isdt;
margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF);
bodies[0] = psa;
bodies[1] = psb;
btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this);
}
};
//
// CollideSDF_RS
//
struct CollideSDF_RS : btDbvt::ICollide
{
void Process(const btDbvtNode* leaf)
{
btSoftBody::Node* node=(btSoftBody::Node*)leaf->data;
DoNode(*node);
}
void DoNode(btSoftBody::Node& n) const
{
const btScalar m=n.m_im>0?dynmargin:stamargin;
btSoftBody::RContact c;
if( (!n.m_battach)&&
psb->checkContact(prb,n.m_x,m,c.m_cti))
{
const btScalar ima=n.m_im;
const btScalar imb=prb->getInvMass();
const btScalar ms=ima+imb;
if(ms>0)
{
const btTransform& wtr=prb->getInterpolationWorldTransform();
const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld();
const btVector3 ra=n.m_x-wtr.getOrigin();
const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt;
const btVector3 vb=n.m_x-n.m_q;
const btVector3 vr=vb-va;
const btScalar dn=dot(vr,c.m_cti.m_normal);
const btVector3 fv=vr-c.m_cti.m_normal*dn;
const btScalar fc=psb->m_cfg.kDF*prb->getFriction();
c.m_node = &n;
c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra);
c.m_c1 = ra;
c.m_c2 = ima*psb->m_sst.sdt;
c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc;
c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
psb->m_rcontacts.push_back(c);
prb->activate();
}
}
}
btSoftBody* psb;
btRigidBody* prb;
btScalar dynmargin;
btScalar stamargin;
};
//
// CollideVF_SS
//
struct CollideVF_SS : btDbvt::ICollide
{
void Process(const btDbvtNode* lnode,
const btDbvtNode* lface)
{
btSoftBody::Node* node=(btSoftBody::Node*)lnode->data;
btSoftBody::Face* face=(btSoftBody::Face*)lface->data;
btVector3 o=node->m_x;
btVector3 p;
btScalar d=SIMD_INFINITY;
ProjectOrigin( face->m_n[0]->m_x-o,
face->m_n[1]->m_x-o,
face->m_n[2]->m_x-o,
p,d);
const btScalar m=mrg+(o-node->m_q).length()*2;
if(d<(m*m))
{
const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]};
const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o);
const btScalar ma=node->m_im;
btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w);
if( (n[0]->m_im<=0)||
(n[1]->m_im<=0)||
(n[2]->m_im<=0))
{
mb=0;
}
const btScalar ms=ma+mb;
if(ms>0)
{
btSoftBody::SContact c;
c.m_normal = p/-btSqrt(d);
c.m_margin = m;
c.m_node = node;
c.m_face = face;
c.m_weights = w;
c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF);
c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR;
c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR;
psb[0]->m_scontacts.push_back(c);
}
}
}
btSoftBody* psb[2];
btScalar mrg;
};
};
#endif //_BT_SOFT_BODY_INTERNALS_H

23
src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
View File

@@ -24,7 +24,10 @@ subject to the following restrictions:
btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
{
m_drawFlags = fDrawFlags::Std;
m_drawNodeTree = true;
m_drawFaceTree = false;
m_drawClusterTree = false;
}
btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld()
@@ -70,7 +73,12 @@ void btSoftRigidDynamicsWorld::updateSoftBodies()
void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints()
{
BT_PROFILE("solveSoftConstraints");
if(m_softBodies.size())
{
btSoftBody::solveClusters(m_softBodies);
}
for(int i=0;i<m_softBodies.size();++i)
{
btSoftBody* psb=(btSoftBody*)m_softBodies[i];
@@ -106,12 +114,13 @@ void btSoftRigidDynamicsWorld::debugDrawWorld()
{
btSoftBody* psb=(btSoftBody*)this->m_softBodies[i];
btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Nodes+fDrawFlags::Std);
btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
//btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
}
}
}
}
}
}

7
src/BulletSoftBody/btSoftRigidDynamicsWorld.h
View File

@@ -25,6 +25,10 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
{
btSoftBodyArray m_softBodies;
int m_drawFlags;
bool m_drawNodeTree;
bool m_drawFaceTree;
bool m_drawClusterTree;
protected:
@@ -48,6 +52,9 @@ public:
void addSoftBody(btSoftBody* body);
void removeSoftBody(btSoftBody* body);
int getDrawFlags() const { return(m_drawFlags); }
void setDrawFlags(int f) { m_drawFlags=f; }
btSoftBodyArray& getSoftBodyArray()