Added several updates for btSoftBody: convex cluster collision detection, new constraints, new demos (only enabled in SoftBodyDemo, todo for AllBulletDemos) etc.
Thanks a lot to Nathanael Presson for this update.
This commit is contained in:
erwin.coumans
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
@@ -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()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -303,11 +530,31 @@ public:
|
||||
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,6 +564,7 @@ public:
|
||||
typedef btAlignedObjectArray<RContact> tRContactArray;
|
||||
typedef btAlignedObjectArray<SContact> tSContactArray;
|
||||
typedef btAlignedObjectArray<Material*> tMaterialArray;
|
||||
typedef btAlignedObjectArray<Joint*> tJointArray;
|
||||
typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
|
||||
|
||||
//
|
||||
@@ -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);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -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_))
|
||||
|
||||
@@ -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
891
src/BulletSoftBody/btSoftBodyInternals.h
Normal 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
|
||||
@@ -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()
|
||||
@@ -71,6 +74,11 @@ 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,11 +114,12 @@ 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -25,6 +25,10 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld
|
||||
{
|
||||
|
||||
btSoftBodyArray m_softBodies;
|
||||
int m_drawFlags;
|
||||
bool m_drawNodeTree;
|
||||
bool m_drawFaceTree;
|
||||
bool m_drawClusterTree;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -49,6 +53,9 @@ public:
|
||||
|
||||
void removeSoftBody(btSoftBody* body);
|
||||
|
||||
int getDrawFlags() const { return(m_drawFlags); }
|
||||
void setDrawFlags(int f) { m_drawFlags=f; }
|
||||
|
||||
|
||||
btSoftBodyArray& getSoftBodyArray()
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user