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

Final 2.68 from Nathanael Presson, mainly soft body related. Added raycast support for soft bodies (press comma-key in the soft body demos to toggle ray cast tests)

Browse Source
This commit is contained in:
erwin.coumans
2008-04-14 06:24:56 +00:00
parent 912b4ccd29
commit be2490e4fb
11 changed files with 472 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
Demos/AllBulletDemos/Main.cpp
View File

@@ -41,7 +41,7 @@ namespace
int iterationCount = 10;
int width = 640;
int height = 480;
int framePeriod = 16;
int framePeriod = 16;//todo: test if this value should be 0
int mainWindow;
GLUI *glui;
float hz;
@@ -66,11 +66,8 @@ void Resize(int w, int h)
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
double ratio = (double)tw / (double)th;
if (demo)
demo->reshape(w, h);
demo->reshape(tw, th);
}
DemoApplication* CreatDemo(btDemoEntry* entry)

131
Demos/SoftDemo/SoftDemo.cpp
View File

@@ -106,7 +106,7 @@ void SoftDemo::clientMoveAndDisplay()
if (m_dynamicsWorld)
{
#define FIXED_STEP 0
//#define FIXED_STEP
#ifdef FIXED_STEP
m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0);
@@ -340,8 +340,6 @@ static void Init_RopeAttach(SoftDemo* pdemo)
static btSoftBody* CtorRope(SoftDemo* pdemo,const btVector3& p)
{
btSoftBody* psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,p,p+btVector3(10,0,0),8,1);
psb->m_cfg.kDF = 0;
psb->m_cfg.kDP = 0.001;
psb->setTotalMass(50);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
return(psb);
@@ -391,7 +389,7 @@ static void Init_Impact(SoftDemo* pdemo)
0,
1);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
psb->m_cfg.kCHR=0.5;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,20,0));
@@ -428,36 +426,6 @@ static void Init_Collide(SoftDemo* pdemo)
{
Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0));
}
#if 0
const btScalar s=2;
const btScalar s2=s*1.01;
{
const btScalar h=0;
const btVector3 p[]={ btVector3(-s,h,-s),btVector3(+s,h,-s),
btVector3(+s,h,+s),btVector3(-s,h,+s)};
btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,p,0);
psb->appendLink(0,1,1,btSoftBody::eLType::Structural);
psb->appendLink(1,2,1,btSoftBody::eLType::Structural);
psb->appendLink(2,3,1,btSoftBody::eLType::Structural);
psb->appendLink(3,0,1,btSoftBody::eLType::Structural);
psb->appendLink(0,2,1,btSoftBody::eLType::Structural);
psb->appendFace(0,1,2);
psb->appendFace(2,3,0);
/*psb->setMass(0,0);
psb->setMass(1,0);
psb->setMass(2,0);
psb->setMass(3,0);*/
psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
}
{
btVector3 p[]={btVector3(0.5,5,-0.5)};
btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,1,p,0);
//psb->setMass(0,0);
psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
}
#endif
}
//
@@ -489,7 +457,7 @@ static void Init_Collide2(SoftDemo* pdemo)
};
for(int i=0;i<3;++i)
{
Functor::Create(pdemo,btVector3(0,-1+4*i,0),btVector3(0,SIMD_PI/2*(i&1),0));
Functor::Create(pdemo,btVector3(0,-1+5*i,0),btVector3(0,SIMD_PI/2*(i&1),0));
}
}
@@ -519,7 +487,7 @@ static void Init_Collide3(SoftDemo* pdemo)
btVector3(+s,0,-s)+o,
btVector3(-s,0,+s)+o,
btVector3(+s,0,+s)+o,
15,15,0,true);
7,7,0,true);
psb->generateBendingConstraints(2,0.5);
psb->m_cfg.kLST = 0.4;
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
@@ -595,7 +563,7 @@ static void Init_Pressure(SoftDemo* pdemo)
512);
psb->m_cfg.kLST = 0.1;
psb->m_cfg.kDF = 1;
psb->m_cfg.kDP = 0.008; // fun factor...
psb->m_cfg.kDP = 0.001; // fun factor...
psb->m_cfg.kPR = 2500;
psb->setTotalMass(30,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
@@ -769,7 +737,7 @@ static void Init_TorusMatch(SoftDemo* pdemo)
}
unsigned current_demo=14;
unsigned current_demo=0;
void SoftDemo::clientResetScene()
{
@@ -827,13 +795,12 @@ void SoftDemo::clientResetScene()
m_autocam = false;
m_showtrees = 0;
m_raycast = false;
demofncs[current_demo](this);
}
void SoftDemo::renderme()
{
DemoApplication::renderme();
btIDebugDraw* idraw=m_dynamicsWorld->getDebugDrawer();
m_dynamicsWorld->debugDrawWorld();
@@ -851,16 +818,87 @@ void SoftDemo::renderme()
{
ps+=psb->getNodes()[i].m_x;
}
btSoftBodyHelpers::DrawFrame(psb,idraw);
btSoftBodyHelpers::Draw(psb,idraw,fDrawFlags::Std);
if(m_showtrees&1)
btSoftBodyHelpers::DrawNodeTree(psb,idraw);
}
ps/=nps;
if(m_autocam)
m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.01;
else
m_cameraTargetPosition=btVector3(0,0,0);
/* Anm */
if(!isIdle())
m_animtime=::GetTickCount()/1000.;
/* Ray cast */
if(m_raycast)
{
/* Prepare rays */
const int res=64;
const btScalar fres=res-1;
const btScalar size=8;
const btScalar dist=10;
btTransform trs;
trs.setOrigin(ps);
const btScalar angle=m_animtime*0.2;
trs.setRotation(btQuaternion(angle,SIMD_PI/4,0));
const btVector3 dir=trs.getBasis()*btVector3(0,-1,0);
trs.setOrigin(ps-dir*dist);
btAlignedObjectArray<btVector3> origins;
btAlignedObjectArray<btScalar> times;
origins.resize(res*res);
times.resize(res*res,SIMD_INFINITY);
for(int y=0;y<res;++y)
{
for(int x=0;x<res;++x)
{
const int idx=y*res+x;
origins[idx]=trs*btVector3(-size+size*2*x/fres,dist,-size+size*2*y/fres);
}
}
/* Cast rays */
{
m_clock.reset();
const btVector3* org=&origins[0];
btScalar* mint=&times[0];
btSoftBody** psbs=&sbs[0];
btSoftBody::sRayCast results;
for(int i=0,ni=origins.size(),nb=sbs.size();i<ni;++i)
{
for(int ib=0;ib<nb;++ib)
{
if(psbs[ib]->rayCast(*org,dir,results,*mint))
{
*mint=results.time;
}
}
++org;++mint;
}
long ms=btMax<long>(m_clock.getTimeMilliseconds(),1);
long rayperseconds=(1000*(origins.size()*sbs.size()))/ms;
printf("%d ms (%d rays/s)\r\n",ms,rayperseconds);
}
/* Draw rays */
const btVector3 c[]={ origins[0],
origins[res-1],
origins[res*(res-1)],
origins[res*(res-1)+res-1]};
idraw->drawLine(c[0],c[1],btVector3(0,0,0));
idraw->drawLine(c[1],c[3],btVector3(0,0,0));
idraw->drawLine(c[3],c[2],btVector3(0,0,0));
idraw->drawLine(c[2],c[0],btVector3(0,0,0));
for(int i=0,ni=origins.size();i<ni;++i)
{
const btScalar tim=times[i];
const btVector3& org=origins[i];
if(tim<SIMD_INFINITY)
{
idraw->drawLine(org,org+dir*tim,btVector3(1,0,0));
}
else
{
idraw->drawLine(org,org-dir*0.1,btVector3(0,0,0));
}
}
#undef RES
}
/* Water level */
static const btVector3 axis[]={btVector3(1,0,0),
btVector3(0,1,0),
@@ -877,6 +915,8 @@ 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();
}
void SoftDemo::keyboardCallback(unsigned char key, int x, int y)
@@ -885,8 +925,8 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y)
{
case ']': ++current_demo;clientResetScene();break;
case '[': --current_demo;clientResetScene();break;
case ',': m_raycast=!m_raycast;break;
case ';': m_autocam=!m_autocam;break;
case ',': m_showtrees+=1;break;
default: DemoApplication::keyboardCallback(key,x,y);
}
}
@@ -1010,3 +1050,4 @@ void SoftDemo::exitPhysics()

5
Demos/SoftDemo/SoftDemo.h
View File

@@ -52,7 +52,9 @@ public:
bool m_autocam;
int m_showtrees;
bool m_raycast;
btScalar m_animtime;
btClock m_clock;
//keep the collision shapes, for deletion/cleanup
@@ -154,3 +156,4 @@ MACRO_SOFT_DEMO(16)//Init_BunnyMatch
#endif //CCD_PHYSICS_DEMO_H

57
src/BulletSoftBody/btDbvt.cpp
View File

@@ -1,10 +1,5 @@
#include "btDbvt.h"
#include <stdio.h>
namespace btdbvt_internals
{
//
typedef btAlignedObjectArray<btDbvt::Node*> tNodeArray;
@@ -23,7 +18,6 @@ Merge(a,b,res);
return(res);
}
// volume+edge lengths
static inline btScalar size(const btDbvt::Aabb& a)
{
@@ -266,14 +260,20 @@ if(leafs.size()>1)
tNodeArray sets[2];
int bestaxis=-1;
int bestmidp=leafs.size();
sets[0].reserve(leafs.size());
sets[1].reserve(leafs.size());
int splitcount[3][2]={0,0,0,0,0,0};
for(int i=0;i<leafs.size();++i)
{
const btVector3 x=leafs[i]->box.Center()-org;
for(int j=0;j<3;++j)
{
++splitcount[j][dot(x,axis[j])>0?1:0];
}
}
for(int i=0;i<3;++i)
{
split(leafs,sets[0],sets[1],org,axis[i]);
if((sets[0].size()>0)&&(sets[1].size()>0))
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
{
const int midp=abs(sets[0].size()-sets[1].size());
const int midp=abs(splitcount[i][0]-splitcount[i][1]);
if(midp<bestmidp)
{
bestaxis=i;
@@ -283,12 +283,14 @@ if(leafs.size()>1)
}
if(bestaxis>=0)
{
sets[0].reserve(splitcount[bestaxis][0]);
sets[1].reserve(splitcount[bestaxis][1]);
split(leafs,sets[0],sets[1],org,axis[bestaxis]);
}
else
{
sets[0].resize(0);
sets[1].resize(0);
sets[0].reserve(leafs.size()/2+1);
sets[1].reserve(leafs.size()/2);
for(int i=0,ni=leafs.size();i<ni;++i)
{
sets[i&1].push_back(leafs[i]);
@@ -332,11 +334,6 @@ if(parent)
return(node);
}
}
using namespace btdbvt_internals;
//
// Api
//
@@ -438,14 +435,12 @@ removeleaf(this,leaf);
deletenode(this,leaf);
}
//
void btDbvt::collide(btDbvt* tree,
ICollide* icollide) const
{
if(tree->m_root&&m_root)
{
btAlignedObjectArray<sStkElm> stack;
stack.reserve(128);
stack.push_back(sStkElm(m_root,tree->m_root));
@@ -524,9 +519,23 @@ if(m_root)
}
//
void btDbvt::collide(const btVector3& org,
const btVector3& dir,
ICollide* icollide) const
void btDbvt::collide(ICollide* icollide) const
{
/* not implemented */
if(m_root)
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(64);
stack.push_back(m_root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(icollide->Descent(n))
{
if(n->isinternal())
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
else
{ icollide->Process(n); }
}
} while(stack.size()>0);
}
}

48
src/BulletSoftBody/btDbvt.h
View File

@@ -45,6 +45,8 @@ struct btDbvt
inline bool Contain(const Aabb& a) const;
inline friend bool Intersect( const Aabb& a,
const Aabb& b);
inline friend bool Intersect( const Aabb& a,
const btVector3& b);
inline friend btScalar Proximity( const Aabb& a,
const Aabb& b);
inline friend void Merge( const Aabb& a,
@@ -66,8 +68,7 @@ struct btDbvt
void* data;
};
};
// Interfaces
/* Stack element */
struct sStkElm
{
const Node* a;
@@ -75,11 +76,14 @@ struct btDbvt
sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {}
};
// Interfaces
/* ICollide */
struct ICollide
{
virtual void Process(const Node* leaf0,const Node* leaf1) {};
virtual void Process(const Node* leaf) {};
virtual void Process(const Node*,const Node*) {}
virtual void Process(const Node*) {}
virtual bool Descent(const Node*) { return(false); }
};
// Fields
@@ -90,6 +94,7 @@ struct btDbvt
btDbvt();
~btDbvt();
void clear();
bool empty() const { return(0==m_root); }
int leafCount() const;
void optimizeBottomUp();
void optimizeTopDown(int bu_treshold=128);
@@ -104,6 +109,29 @@ struct btDbvt
void collide(const btVector3& org,
const btVector3& dir,
ICollide* icollide) const;
void collide(ICollide* icollide) const;
// Generics
template <typename T/* must implement ICollide*/>
void collideGeneric(T& policy) const
{
if(m_root)
{
btAlignedObjectArray<const Node*> stack;
stack.reserve(64);
stack.push_back(m_root);
do {
const Node* n=stack[stack.size()-1];
stack.pop_back();
if(policy.Descent(n))
{
if(n->isinternal())
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
else
{ policy.Process(n); }
}
} while(stack.size()>0);
}
}
//
private:
btDbvt(const btDbvt&) {}
@@ -198,6 +226,18 @@ return( (a.mi.x()<=b.mx.x())&&
(a.mx.z()>=b.mi.z()));
}
//
inline bool Intersect( const btDbvt::Aabb& a,
const btVector3& b)
{
return( (b.x()>=a.mi.x())&&
(b.y()>=a.mi.y())&&
(b.z()>=a.mi.z())&&
(b.x()<=a.mx.x())&&
(b.y()<=a.mx.y())&&
(b.z()<=a.mx.z()));
}
//
inline btScalar Proximity( const btDbvt::Aabb& a,
const btDbvt::Aabb& b)

270
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -15,7 +15,10 @@ subject to the following restrictions:
///btSoftBody implementation by Nathanael Presson
#include "btSoftBody.h"
#if 1
#include <stdio.h>
#define DOTRACE
#endif
#include <string.h>
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
@@ -67,7 +70,7 @@ public:
{
return SOFTBODY_SHAPE_PROXYTYPE;
}
virtual void setLocalScaling(const btVector3& scaling)
virtual void setLocalScaling(const btVector3& /*scaling*/)
{
///na
btAssert(0);
@@ -77,7 +80,7 @@ public:
static const btVector3 dummy(1,1,1);
return dummy;
}
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const
virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
{
///not yet
btAssert(0);
@@ -99,18 +102,18 @@ btSoftBodyCollisionShape::~btSoftBodyCollisionShape()
}
void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
{
//not yet
btAssert(0);
}
//
// Helpers
//
//
#ifdef DOTRACE
static inline void Trace(const btMatrix3x3& m,const char* name)
{
printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z());
@@ -118,6 +121,9 @@ static inline void Trace(const btMatrix3x3& m,const char* name)
printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z());
printf("\r\n");
}
#else
static inline void Trace(const btMatrix3x3&,const char*) {}
#endif
//
template <typename T>
@@ -144,6 +150,10 @@ 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)); }
//
@@ -348,6 +358,7 @@ static inline btVector3 NormalizeAny(const btVector3& v)
}
//
#if 0
static void PointersToIndices(btSoftBody* psb)
{
#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
@@ -365,8 +376,10 @@ static void PointersToIndices(btSoftBody* psb)
}
#undef PTR2IDX
}
#endif
//
#if 0
static void IndicesToPointers(btSoftBody* psb)
{
#define IDX2PTR(_p_,_b_) ((_b_)+(((char*)_p_)-(char*)0))
@@ -384,6 +397,7 @@ static void IndicesToPointers(btSoftBody* psb)
}
#undef IDX2PTR
}
#endif
//
static inline btDbvt::Aabb BoxOf( const btSoftBody::Face& f,
@@ -431,7 +445,7 @@ static inline btScalar RayTriangle(const btVector3& org,
{
static const btScalar ceps=-SIMD_EPSILON*10;
static const btScalar teps=SIMD_EPSILON*10;
const btVector3 n=cross(b-a,c-a).normalized();
const btVector3 n=cross(b-a,c-a);
const btScalar d=dot(a,n);
const btScalar den=dot(dir,n);
if(!btFuzzyZero(den))
@@ -461,10 +475,52 @@ static int RaycastInternal(const btSoftBody* psb,
const btVector3& org,
const btVector3& dir,
btScalar& mint,
int& face,
bool bcountonly)
{
struct RayCaster : btDbvt::ICollide
{
btVector3 o;
btVector3 d;
btVector3 nd;
btScalar mint;
btSoftBody::Face* face;
int tests;
RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
{
o = org;
d = dir;
nd = dir.normalized();
mint = mxt;
face = 0;
tests = 0;
}
void Process(const btDbvt::Node* leaf)
{
btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
const btScalar t=RayTriangle( o,d,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if((t>0)&&(t<mint))
{
mint=t;
face=&f;
}
++tests;
}
bool Descent(const btDbvt::Node* node)
{
const btVector3 ctr=node->box.Center()-o;
const btScalar sqr=node->box.Lengths().length2()/4;
const btScalar prj=dot(ctr,nd);
return((ctr-(nd*prj)).length2()<=sqr);
}
};
int cnt=0;
mint=SIMD_INFINITY;
if(bcountonly||psb->m_fdbvt.empty())
{/* Full search */
for(int i=0,ni=psb->getFaces().size();i<ni;++i)
{
const btSoftBody::Face& f=psb->getFaces()[i];
@@ -475,12 +531,35 @@ static int RaycastInternal(const btSoftBody* psb,
mint);
if(t>0)
{
++cnt;if(!bcountonly) mint=t;
++cnt;if(!bcountonly) { face=i;mint=t; }
}
}
}
else
{/* Use dbvt */
RayCaster collider(org,dir,mint);
psb->m_fdbvt.collideGeneric(collider);
if(collider.face)
{
mint=collider.mint;
face=(int)(collider.face-&psb->getFaces()[0]);
cnt=1;
}
}
return(cnt);
}
//
static void InitializeFaceTree(btSoftBody* psb)
{
psb->m_fdbvt.clear();
for(int i=0;i<psb->getFaces().size();++i)
{
btSoftBody::Face& f=psb->getFaces()[i];
f.m_leaf=psb->m_fdbvt.insert(BoxOf(f,0),&f);
}
psb->m_fdbvt.optimizeTopDown();
}
//
static btVector3 EvaluateCom(btSoftBody* psb)
@@ -519,13 +598,13 @@ static void EvaluateMedium( const btSoftBody::btSoftBodyWorldInfo* wfi,
static bool CheckContact( btSoftBody* psb,
btRigidBody* prb,
const btVector3& x,
btScalar margin,
btSoftBody::sCti& cti)
{
btVector3 nrm;
btCollisionShape* shp=prb->getCollisionShape();
btAssert(shp->isConvex());
btConvexShape* csh=static_cast<btConvexShape*>(shp);
const btScalar margin=psb->getCollisionShape()->getMargin();
const btTransform& wtr=prb->getInterpolationWorldTransform();
btScalar dst=psb->m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
csh,
@@ -693,6 +772,7 @@ static inline void ApplyClampedForce( btSoftBody::Node& n,
//
static void ApplyForces(btSoftBody* psb,btScalar dt)
{
BT_PROFILE("SoftBody applyForces");
const btScalar kLF=psb->m_cfg.kLF;
const btScalar kDG=psb->m_cfg.kDG;
const btScalar kPR=psb->m_cfg.kPR;
@@ -832,7 +912,6 @@ static void PSolve_Anchors(btSoftBody* psb,btScalar sdt)
//
static void PSolve_RContacts(btSoftBody* psb,btScalar sdt)
{
const btScalar kCHR=psb->m_cfg.kCHR;
for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
{
const btSoftBody::RContact& c=psb->m_rcontacts[i];
@@ -845,7 +924,7 @@ static void PSolve_RContacts(btSoftBody* psb,btScalar sdt)
{
const btScalar dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset;
const btVector3 fv=vr-cti.m_normal*dn;
const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*kCHR));
const btVector3 impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4));
c.m_node->m_x-=impulse*c.m_c2;
c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
}
@@ -912,9 +991,8 @@ static void PSolve_Links(btSoftBody* psb,btScalar w)
btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
:m_worldInfo(worldInfo)
{
m_internalType=CO_SOFT_BODY;
/* Init */
m_internalType = CO_SOFT_BODY;
m_cfg.aeromodel = eAeroModel::V_Point;
m_cfg.kDG = 0;
m_cfg.kLF = 0;
@@ -1254,6 +1332,81 @@ void btSoftBody::setPose(bool bvolume,bool bframe)
UpdateConstants(this);
}
//
void btSoftBody::setDistanceField(int res)
{
struct SignChecker : btDbvt::ICollide
{
btVector3 m_x;
btVector3 m_axe;
int m_count;
SignChecker(const btVector3& x,
const btVector3& a,
btDbvt& tree) : m_x(x),m_axe(a),m_count(0)
{
tree.collide(this);
}
void Process(const btDbvt::Node* n)
{
const Face& f=*(const Face*)n->data;
if(RayTriangle( m_x,
m_axe,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x)>0)
{
++m_count;
}
}
bool Descent(const btDbvt::Node* n)
{
return(dot(m_x-n->box.Maxs()*m_axe,m_axe)<=0);
}
};
btDbvt dbvt;
int ires[3];
btScalar fres[3];
const btScalar margin=getCollisionShape()->getMargin();
const btVector3 lengths=m_bounds[1]-m_bounds[0];
const btScalar smallest=btMin(btMin(lengths.x(),lengths.y()),lengths.z());
const btScalar ratio=smallest/res;
ires[0]=btMax(2,(int)(lengths.x()/ratio));
ires[1]=btMax(2,(int)(lengths.y()/ratio));
ires[2]=btMax(2,(int)(lengths.z()/ratio));
fres[0]=(btScalar)ires[0]-1;
fres[1]=(btScalar)ires[1]-1;
fres[2]=(btScalar)ires[2]-1;
for(int i=0;i<getFaces().size();++i)
{
dbvt.insert(BoxOf(getFaces()[i],margin),&getFaces()[i]);
}
dbvt.optimizeTopDown(256);
m_dfld.pts.clear();
for(int z=0;z<ires[2];++z)
{
const btScalar fz=Lerp(m_bounds[0].z(),m_bounds[1].z(),z/fres[2]);
for(int y=0;y<ires[1];++y)
{
const btScalar fy=Lerp(m_bounds[0].y(),m_bounds[1].y(),y/fres[1]);
for(int x=0;x<ires[0];++x)
{
const btScalar fx=Lerp(m_bounds[0].x(),m_bounds[1].x(),x/fres[0]);
const btVector3 p(fx,fy,fz);
const SignChecker scx(p,btVector3(1,0,0),dbvt);
const SignChecker scy(p,btVector3(0,1,0),dbvt);
const SignChecker scz(p,btVector3(0,0,1),dbvt);
const int parity= ((scx.m_count&1)?1:0)+
((scy.m_count&1)?1:0)+
((scz.m_count&1)?1:0);
if(parity>1)
{
m_dfld.pts.push_back(p-m_pose.m_com);
}
}
}
}
}
//
btScalar btSoftBody::getVolume() const
{
@@ -1348,14 +1501,21 @@ void btSoftBody::randomizeConstraints()
}
//
btScalar btSoftBody::raycast(const btVector3& org,
const btVector3& dir) const
bool btSoftBody::rayCast(const btVector3& org,
const btVector3& dir,
sRayCast& results,
btScalar maxtime)
{
btScalar mint=0;
if(RaycastInternal(this,org,dir,mint,false))
return(mint);
else
return(-1);
if(getFaces().size())
{
if(m_fdbvt.empty()) InitializeFaceTree(this);
results.time = maxtime;
results.face = -1;
return(RaycastInternal( this,org,dir,
results.time,
results.face,false)!=0);
}
return(false);
}
//
@@ -1369,24 +1529,20 @@ void btSoftBody::predictMotion(btScalar dt)
m_fdbvt.clear();
if(m_cfg.collisions&fCollision::VF_SS)
{
for(int i=0;i<getFaces().size();++i)
{
Face& f=getFaces()[i];
f.m_leaf=m_fdbvt.insert(BoxOf(f,0),&f);
}
InitializeFaceTree(this);
}
}
/* Prepare */
m_sst.iit = 1/(btScalar)m_cfg.iterations;
m_sst.sdt = dt*m_cfg.timescale;
m_sst.isdt = 1/m_sst.sdt;
m_sst.velmrg = m_sst.sdt*2;
m_sst.velmrg = m_sst.sdt*3;
m_sst.radmrg = getCollisionShape()->getMargin();
m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
/* Forces */
addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
ApplyForces(this,m_sst.sdt);
/* Integrate */
const btScalar margin=getCollisionShape()->getMargin();
const btScalar updmrg=margin*(btScalar)1.0;
for(int i=0,ni=getNodes().size();i<ni;++i)
{
Node& n=getNodes()[i];
@@ -1394,12 +1550,13 @@ void btSoftBody::predictMotion(btScalar dt)
n.m_v += n.m_f*n.m_im*m_sst.sdt;
n.m_x += n.m_v*m_sst.sdt;
m_ndbvt.update( n.m_leaf,
btDbvt::Aabb::FromCR(n.m_x,margin),
n.m_v*m_sst.velmrg,updmrg);
btDbvt::Aabb::FromCR(n.m_x,m_sst.radmrg),
n.m_v*m_sst.velmrg,
m_sst.updmrg);
}
UpdateBounds(this);
/* Faces */
if(m_fdbvt.m_root)
if(!m_fdbvt.empty())
{
for(int i=0;i<getFaces().size();++i)
{
@@ -1407,7 +1564,10 @@ void btSoftBody::predictMotion(btScalar dt)
const btVector3 v=( f.m_n[0]->m_v+
f.m_n[1]->m_v+
f.m_n[2]->m_v)/3;
m_fdbvt.update(f.m_leaf,BoxOf(f,margin),v*m_sst.velmrg,updmrg);
m_fdbvt.update( f.m_leaf,
BoxOf(f,m_sst.radmrg),
v*m_sst.velmrg,
m_sst.updmrg);
}
}
/* Pose */
@@ -1459,18 +1619,9 @@ void btSoftBody::solveConstraints()
}
//
void btSoftBody::solveCommonConstraints(btSoftBody** bodies,int count,int iterations)
void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
{
/// placeholder
#if 0
for(int isolve=0;isolve<iterations;++isolve)
{
for(int ibody=0;ibody<count;++ibody)
{
PSolve_SContacts(bodies[ibody]);
}
}
#endif
}
//
@@ -1504,9 +1655,10 @@ switch(m_cfg.collisions&fCollision::RVSmask)
}
void DoNode(Node& n) const
{
const btScalar m=n.m_im>0?dynmargin:stamargin;
RContact c;
if( (!n.m_battach)&&
CheckContact(psb,prb,n.m_x,c.m_cti))
CheckContact(psb,prb,n.m_x,m,c.m_cti))
{
const btScalar ima=n.m_im;
const btScalar imb=prb->getInvMass();
@@ -1527,6 +1679,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
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 = psb->m_cfg.kCHR*(btScalar)(prb->isStaticOrKinematicObject()?0.5:1);
psb->m_rcontacts.push_back(c);
prb->activate();
}
@@ -1534,8 +1687,14 @@ switch(m_cfg.collisions&fCollision::RVSmask)
}
btSoftBody* psb;
btRigidBody* prb;
btScalar dynmargin;
btScalar stamargin;
} docollide;
const btScalar margin=getCollisionShape()->getMargin();
btRigidBody* prb=btRigidBody::upcast(pco);
const btTransform wtr=prb->getInterpolationWorldTransform();
const btTransform ctr=prb->getWorldTransform();
const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
const btScalar basemargin=getCollisionShape()->getMargin();
btVector3 mins;
btVector3 maxs;
btDbvt::Aabb aabb;
@@ -1543,9 +1702,11 @@ switch(m_cfg.collisions&fCollision::RVSmask)
mins,
maxs);
aabb=btDbvt::Aabb::FromMM(mins,maxs);
aabb.Expand(btVector3(margin,margin,margin));
docollide.psb=this;
docollide.prb=btRigidBody::upcast(pco);
aabb.Expand(btVector3(basemargin,basemargin,basemargin));
docollide.psb = this;
docollide.prb = prb;
docollide.dynmargin = basemargin+timemargin;
docollide.stamargin = basemargin;
m_ndbvt.collide(aabb,&docollide);
}
break;
@@ -1577,15 +1738,16 @@ switch(cf&fCollision::SVSmask)
const btScalar m=mrg+(o-node->m_q).length()*2;
if(d<(m*m))
{
const btVector3 w=BaryCoord(face->m_n[0]->m_x,
face->m_n[1]->m_x,
face->m_n[2]->m_x,
p+o);
const 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;
const btScalar mb=BaryEval(face->m_n[0]->m_im,
face->m_n[1]->m_im,
face->m_n[2]->m_im,
w);
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)
{

57
src/BulletSoftBody/btSoftBody.h
View File

@@ -69,6 +69,17 @@ public:
Default = SDF_RS,
};};
//
// API Types
//
/* sRayCast */
struct sRayCast
{
int face; /// face
btScalar time; /// time of impact (rayorg+raydir*time)
};
//
// Internal types
//
@@ -150,6 +161,7 @@ public:
btVector3 m_c1; // Relative anchor
btScalar m_c2; // ima*dt
btScalar m_c3; // Friction
btScalar m_c4; // Hardness
};
/* SContact */
struct SContact
@@ -185,6 +197,11 @@ public:
btMatrix3x3 m_scl; // Scale
btMatrix3x3 m_aqq; // Base scaling
};
/* DFld */
struct DFld
{
btAlignedObjectArray<btVector3> pts;
};
/* Config */
struct Config
{
@@ -213,6 +230,8 @@ public:
btScalar sdt; // dt*timescale
btScalar isdt; // 1/sdt
btScalar velmrg; // velocity margin
btScalar radmrg; // radial margin
btScalar updmrg; // Update margin
};
//
@@ -220,6 +239,7 @@ public:
//
typedef btAlignedObjectArray<Node> tNodeArray;
typedef btAlignedObjectArray<btDbvt::Node*> tLeafArray;
typedef btAlignedObjectArray<Link> tLinkArray;
typedef btAlignedObjectArray<Face> tFaceArray;
typedef btAlignedObjectArray<Anchor> tAnchorArray;
@@ -234,6 +254,7 @@ public:
Config m_cfg; // Configuration
SolverState m_sst; // Solver state
Pose m_pose; // Pose
DFld m_dfld; // Distance field
void* m_tag; // User data
btSoftBodyWorldInfo* m_worldInfo; //
tAnchorArray m_anchors; // Anchors
@@ -311,6 +332,8 @@ public:
/* Set current state as pose */
void setPose( bool bvolume,
bool bframe);
/* Set current state as distance field */
void setDistanceField(int nominalresolution);
/* Return the volume */
btScalar getVolume() const;
/* Generate bending constraints based on distance in the adjency graph */
@@ -319,8 +342,10 @@ public:
/* Randomize constraints to reduce solver bias */
void randomizeConstraints();
/* Ray casting */
btScalar raycast(const btVector3& org,
const btVector3& dir) const;
bool rayCast(const btVector3& org,
const btVector3& dir,
sRayCast& results,
btScalar maxtime=SIMD_INFINITY);
/* predictMotion */
void predictMotion(btScalar dt);
/* solveConstraints */
@@ -333,8 +358,21 @@ public:
void defaultCollisionHandler(btCollisionObject* pco);
void defaultCollisionHandler(btSoftBody* psb);
///to keep collision detection and dynamics separate we don't store a rigidbody pointer
///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
//
// Accessor's and cast.
//
tNodeArray& getNodes();
const tNodeArray& getNodes() const;
tLinkArray& getLinks();
const tLinkArray& getLinks() const;
tFaceArray& getFaces();
const tFaceArray& getFaces() const;
//
// Cast
//
static const btSoftBody* upcast(const btCollisionObject* colObj)
{
if (colObj->getInternalType()==CO_SOFT_BODY)
@@ -348,17 +386,6 @@ public:
return 0;
}
//
// ...
//
tNodeArray& getNodes();
const tNodeArray& getNodes() const;
tLinkArray& getLinks();
const tLinkArray& getLinks() const;
tFaceArray& getFaces();
const tFaceArray& getFaces() const;
};

15
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -79,6 +79,7 @@ if(node)
}
//
#if 0
static btVector3 stresscolor(btScalar stress)
{
static const btVector3 spectrum[]= { btVector3(1,0,1),
@@ -95,6 +96,7 @@ static btVector3 stresscolor(btScalar stress)
const btScalar frc=stress-sel;
return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
}
#endif
//
void btSoftBodyHelpers::Draw( btSoftBody* psb,
@@ -103,7 +105,6 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb,
{
const btScalar scl=(btScalar)0.1;
const btScalar nscl=scl*5;
const btScalar alpha=(btScalar)0.5;
const btVector3 scolor=btVector3(0,0,0);
const btVector3 bcolor=btVector3(1,1,0);
const btVector3 ncolor=btVector3(1,1,1);
@@ -208,7 +209,7 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,
btIDebugDraw* idraw,
bool masses,
bool areas,
bool stress)
bool /*stress*/)
{
for(int i=0;i<psb->getNodes().size();++i)
{
@@ -266,9 +267,12 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,
for(int i=0;i<psb->m_pose.m_pos.size();++i)
{
const btVector3 x=com+trs*psb->m_pose.m_pos[i];
idraw->drawLine(x-btVector3(1,0,0)*nscl,x+btVector3(1,0,0)*nscl,btVector3(1,0,1));
idraw->drawLine(x-btVector3(0,1,0)*nscl,x+btVector3(0,1,0)*nscl,btVector3(1,0,1));
idraw->drawLine(x-btVector3(0,0,1)*nscl,x+btVector3(0,0,1)*nscl,btVector3(1,0,1));
drawVertex(idraw,x,nscl,btVector3(1,0,1));
}
for(int i=0;i<psb->m_dfld.pts.size();++i)
{
const btVector3 x=com+trs*psb->m_dfld.pts[i];
drawVertex(idraw,x,nscl*(btScalar)0.5,btVector3(0,0,1));
}
}
}
@@ -483,3 +487,4 @@ btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
return(psb);
}

21
src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp
View File

@@ -24,30 +24,12 @@ subject to the following restrictions:
//#include <stdio.h>
btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped)
: btCollisionAlgorithm(ci),
//m_ownManifold(false),
//m_manifoldPtr(mf),
m_isSwapped(isSwapped)
{
//m_softBody = m_isSwapped? (btSoftBody*)col1 : (btSoftBody*)col0;
//m_rigidCollisionObject = m_isSwapped? col0 : col1;
//quick fix, add overlapping rigidbody to softbody, so it can be handled within btSoftBody::Step method
//m_softBody->m_overlappingRigidBodies.push_back(m_rigidCollisionObject);
///store the contacts straight into the btSoftBody for now?
/*if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
{
m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
m_ownManifold = true;
}
*/
}
@@ -94,3 +76,4 @@ btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject*
}

19
src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
View File

@@ -46,7 +46,6 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
{
btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
///solve soft bodies constraints
@@ -100,24 +99,10 @@ void btSoftRigidDynamicsWorld::debugDrawWorld()
{
btDiscreteDynamicsWorld::debugDrawWorld();
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
if (getDebugDrawer())
{
int i;
for ( i=0;i<this->m_softBodies.size();i++)
{
btSoftBody* psb=(btSoftBody*)this->m_softBodies[i];
btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
btSoftBodyHelpers::Draw(psb,m_debugDrawer,fDrawFlags::Std+fDrawFlags::Nodes);
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
//btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
}
}
if (getDebugDrawer())
{
for (int i=0;i<this->m_softBodies.size();i++)
{
btSoftBody* psb=(btSoftBody*)this->m_softBodies[i];
btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
@@ -125,7 +110,7 @@ void btSoftRigidDynamicsWorld::debugDrawWorld()
if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
{
btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
}
//btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
}
}
}

65
src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp
View File

@@ -21,84 +21,25 @@ subject to the following restrictions:
#define USE_PERSISTENT_CONTACTS 1
btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/)
: btCollisionAlgorithm(ci)
//m_ownManifold(false),
//m_manifoldPtr(mf)
{
/*m_softBody0 = (btSoftBody*) obj0;
m_softBody1 = (btSoftBody*) obj1;
m_softBody0->m_overlappingSoftBodies.push_back(m_softBody1);
m_softBody1->m_overlappingSoftBodies.push_back(m_softBody0);*/
/*if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
{
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
m_ownManifold = true;
}
*/
}
btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm()
{
//m_softBody0->m_overlappingSoftBodies.remove(m_softBody1);
//m_softBody1->m_overlappingSoftBodies.remove(m_softBody0);
//this gets called when the overlap stops.
//here is where contacts (manifolds) should be removed
/*
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
*/
}
void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
{
btSoftBody* soft0 = (btSoftBody*)body0;
btSoftBody* soft1 = (btSoftBody*)body1;
soft0->defaultCollisionHandler(soft1);
/*
btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
#ifndef USE_PERSISTENT_CONTACTS
m_manifoldPtr->clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform();
btBoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#ifdef USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
*/
}
btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
{
//not yet
return 1.f;