Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-28 04:56:08 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
d9e7058ff28fe080053c26b9874ded3abc1821f4
bullet3/src/BulletSoftBody/btSoftBody.cpp
erwin.coumans d9e7058ff2 Softbody improvements, thanks to Nathanael
2008-05-17 12:39:16 +00:00

2371 lines
57 KiB
C++
Raw Blame History

/*
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
#include "btSoftBody.h"
#include "LinearMath/btQuickprof.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.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); }
T& operator()(int c,int r)
{
if(c>r) btSwap(c,r);
btAssert(r<dim);
return(store[(r*(r+1))/2+c]);
}
const T& operator()(int c,int r) const
{
if(c>r) btSwap(c,r);
btAssert(r<dim);
return(store[(r*(r+1))/2+c]);
}
btAlignedObjectArray<T> store;
int dim;
};
//
// Collision shape
//
///btSoftBodyCollisionShape is work-in-progress collision shape for softbodies
class btSoftBodyCollisionShape : public btConcaveShape
{
public:
btSoftBody* m_body;
btSoftBodyCollisionShape(btSoftBody* backptr);
virtual ~btSoftBodyCollisionShape();
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
///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";
}
};
btSoftBodyCollisionShape::btSoftBodyCollisionShape(btSoftBody* backptr)
{
m_body=backptr;
}
btSoftBodyCollisionShape::~btSoftBodyCollisionShape()
{
}
void btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
{
//not yet
btAssert(0);
}
//
// Helpers
//
//
//
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);
}
//
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 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 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 void PolarDecompose( const btMatrix3x3& m,
btMatrix3x3& q,
btMatrix3x3& s)
{
static const btScalar half=(btScalar)0.5;
static const btScalar accuracy=(btScalar)0.00001;
static const int maxiterations=64;
btScalar det=m.determinant();
if(!btFuzzyZero(det))
{
q=m;
for(int i=0;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 */
q[0]=q[0].normalized();
q[2]=cross(q[0],q[1]).normalized();
q[1]=cross(q[2],q[0]).normalized();
/* Compute 'S' */
s=q.transpose()*m;
}
else
{
q.setIdentity();
s.setIdentity();
}
}
//
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 void PointersToIndices(btSoftBody* psb)
{
#define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
btSoftBody::Node* base=&psb->m_nodes[0];
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
if(psb->m_nodes[i].m_leaf)
{
psb->m_nodes[i].m_leaf->data=*(void**)&i;
}
}
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
psb->m_links[i].m_n[0]=PTR2IDX(psb->m_links[i].m_n[0],base);
psb->m_links[i].m_n[1]=PTR2IDX(psb->m_links[i].m_n[1],base);
}
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
psb->m_faces[i].m_n[0]=PTR2IDX(psb->m_faces[i].m_n[0],base);
psb->m_faces[i].m_n[1]=PTR2IDX(psb->m_faces[i].m_n[1],base);
psb->m_faces[i].m_n[2]=PTR2IDX(psb->m_faces[i].m_n[2],base);
if(psb->m_faces[i].m_leaf)
{
psb->m_faces[i].m_leaf->data=*(void**)&i;
}
}
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
psb->m_anchors[i].m_node=PTR2IDX(psb->m_anchors[i].m_node,base);
}
for(int i=0,ni=psb->m_notes.size();i<ni;++i)
{
for(int j=0;j<psb->m_notes[i].m_rank;++j)
{
psb->m_notes[i].m_nodes[j]=PTR2IDX(psb->m_notes[i].m_nodes[j],base);
}
}
#undef PTR2IDX
}
//
static void IndicesToPointers(btSoftBody* psb,const int* map=0)
{
#define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
(&(_b_)[(((char*)_p_)-(char*)0)])
btSoftBody::Node* base=&psb->m_nodes[0];
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
if(psb->m_nodes[i].m_leaf)
{
psb->m_nodes[i].m_leaf->data=&psb->m_nodes[i];
}
}
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
psb->m_links[i].m_n[0]=IDX2PTR(psb->m_links[i].m_n[0],base);
psb->m_links[i].m_n[1]=IDX2PTR(psb->m_links[i].m_n[1],base);
}
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
psb->m_faces[i].m_n[0]=IDX2PTR(psb->m_faces[i].m_n[0],base);
psb->m_faces[i].m_n[1]=IDX2PTR(psb->m_faces[i].m_n[1],base);
psb->m_faces[i].m_n[2]=IDX2PTR(psb->m_faces[i].m_n[2],base);
if(psb->m_faces[i].m_leaf)
{
psb->m_faces[i].m_leaf->data=&psb->m_faces[i];
}
}
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
psb->m_anchors[i].m_node=IDX2PTR(psb->m_anchors[i].m_node,base);
}
for(int i=0,ni=psb->m_notes.size();i<ni;++i)
{
for(int j=0;j<psb->m_notes[i].m_rank;++j)
{
psb->m_notes[i].m_nodes[j]=IDX2PTR(psb->m_notes[i].m_nodes[j],base);
}
}
#undef IDX2PTR
}
//
static inline btDbvt::Volume 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};
btDbvt::Volume vol=btDbvt::Volume::FromPoints(pts,3);
vol.Expand(btVector3(margin,margin,margin));
return(vol);
}
//
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 inline btScalar RayTriangle(const btVector3& org,
const btVector3& dir,
const btVector3& a,
const btVector3& b,
const btVector3& c,
btScalar maxt=SIMD_INFINITY)
{
static const btScalar ceps=-SIMD_EPSILON*10;
static const btScalar teps=SIMD_EPSILON*10;
const btVector3 n=cross(b-a,c-a);
const btScalar d=dot(a,n);
const btScalar den=dot(dir,n);
if(!btFuzzyZero(den))
{
const btScalar num=dot(org,n)-d;
const btScalar t=-num/den;
if((t>teps)&&(t<maxt))
{
const btVector3 hit=org+dir*t;
if( (dot(n,cross(a-hit,b-hit))>ceps) &&
(dot(n,cross(b-hit,c-hit))>ceps) &&
(dot(n,cross(c-hit,a-hit))>ceps))
{
return(t);
}
}
}
return(-1);
}
//
// Private implementation
//
struct RayCaster : btDbvt::ICollide
{
btVector3 o;
btVector3 d;
btScalar mint;
btSoftBody::Face* face;
int tests;
RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
{
o = org;
d = dir;
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;
}
};
//
static int RaycastInternal(const btSoftBody* psb,
const btVector3& org,
const btVector3& dir,
btScalar& mint,
btSoftBody::eFeature::_& feature,
int& index,
bool bcountonly)
{
int cnt=0;
if(bcountonly||psb->m_fdbvt.empty())
{/* Full search */
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
const btSoftBody::Face& f=psb->m_faces[i];
const btScalar t=RayTriangle( org,dir,
f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
mint);
if(t>0)
{
++cnt;
if(!bcountonly)
{
feature=btSoftBody::eFeature::Face;
index=i;
mint=t;
}
}
}
}
else
{/* Use dbvt */
RayCaster collider(org,dir,mint);
btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider);
if(collider.face)
{
mint=collider.mint;
feature=btSoftBody::eFeature::Face;
index=(int)(collider.face-&psb->m_faces[0]);
cnt=1;
}
}
return(cnt);
}
//
static void InitializeFaceTree(btSoftBody* psb)
{
psb->m_fdbvt.clear();
for(int i=0;i<psb->m_faces.size();++i)
{
btSoftBody::Face& f=psb->m_faces[i];
f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f);
}
}
//
static btVector3 EvaluateCom(btSoftBody* psb)
{
btVector3 com(0,0,0);
if(psb->m_pose.m_bframe)
{
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
com+=psb->m_nodes[i].m_x*psb->m_pose.m_wgh[i];
}
}
return(com);
}
//
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 bool CheckContact( btSoftBody* psb,
btRigidBody* prb,
const btVector3& x,
btScalar margin,
btSoftBody::sCti& cti)
{
btVector3 nrm;
btCollisionShape* shp=prb->getCollisionShape();
const btTransform& wtr=prb->getInterpolationWorldTransform();
btScalar dst=psb->m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
shp,
nrm,
margin);
if(dst<0)
{
cti.m_body = prb;
cti.m_normal = wtr.getBasis()*nrm;
cti.m_offset = -dot( cti.m_normal,
x-cti.m_normal*dst);
return(true);
}
return(false);
}
//
static void UpdateNormals(btSoftBody* psb)
{
const btVector3 zv(0,0,0);
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
psb->m_nodes[i].m_n=zv;
}
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=psb->m_faces[i];
const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x,
f.m_n[2]->m_x-f.m_n[0]->m_x);
f.m_normal=n.normalized();
f.m_n[0]->m_n+=n;
f.m_n[1]->m_n+=n;
f.m_n[2]->m_n+=n;
}
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
psb->m_nodes[i].m_n.normalize();
}
}
//
static void UpdateBounds(btSoftBody* psb)
{
if(psb->m_ndbvt.m_root)
{
const btVector3& mins=psb->m_ndbvt.m_root->volume.Mins();
const btVector3& maxs=psb->m_ndbvt.m_root->volume.Maxs();
const btScalar csm=psb->getCollisionShape()->getMargin();
const btVector3 mrg=btVector3( csm,
csm,
csm)*1; // ??? to investigate...
psb->m_bounds[0]=mins-mrg;
psb->m_bounds[1]=maxs+mrg;
if(0!=psb->getBroadphaseHandle())
{
psb->m_worldInfo->m_broadphase->setAabb(psb->getBroadphaseHandle(),
psb->m_bounds[0],
psb->m_bounds[1],
psb->m_worldInfo->m_dispatcher);
}
}
else
{
psb->m_bounds[0]=
psb->m_bounds[1]=btVector3(0,0,0);
}
}
//
static void UpdatePose(btSoftBody* psb)
{
if(psb->m_pose.m_bframe)
{
btSoftBody::Pose& pose=psb->m_pose;
const btVector3 com=EvaluateCom(psb);
/* Com */
pose.m_com = com;
/* Rotation */
btMatrix3x3 Apq;
const btScalar eps=1/(btScalar)(100*psb->m_nodes.size());
Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
const btVector3 a=pose.m_wgh[i]*(psb->m_nodes[i].m_x-com);
const btVector3& b=pose.m_pos[i];
Apq[0]+=a.x()*b;
Apq[1]+=a.y()*b;
Apq[2]+=a.z()*b;
}
btMatrix3x3 r,s;
PolarDecompose(Apq,r,s);
pose.m_rot=r;
pose.m_scl=pose.m_aqq*r.transpose()*Apq;
if(psb->m_cfg.maxvolume>1)
{
const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
1,psb->m_cfg.maxvolume);
pose.m_scl=Mul(pose.m_scl,idet);
}
}
}
//
static void UpdateConstants(btSoftBody* psb)
{
/* Links */
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
btSoftBody::Link& l=psb->m_links[i];
btSoftBody::Material& m=*l.m_material;
l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
l.m_c1 = l.m_rl*l.m_rl;
}
/* Faces */
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=psb->m_faces[i];
f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
}
/* Area's */
btAlignedObjectArray<int> counts;
counts.resize(psb->m_nodes.size(),0);
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
psb->m_nodes[i].m_area = 0;
}
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=psb->m_faces[i];
for(int j=0;j<3;++j)
{
const int index=(int)(f.m_n[j]-&psb->m_nodes[0]);
counts[index]++;
f.m_n[j]->m_area+=btFabs(f.m_ra);
}
}
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
if(counts[i]>0)
psb->m_nodes[i].m_area/=(btScalar)counts[i];
else
psb->m_nodes[i].m_area=0;
}
}
//
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 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;
const btScalar kVC=psb->m_cfg.kVC;
const bool as_lift=kLF>0;
const bool as_drag=kDG>0;
const bool as_pressure=kPR!=0;
const bool as_volume=kVC>0;
const bool as_aero= as_lift ||
as_drag ;
const bool as_vaero= as_aero &&
(psb->m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
const bool as_faero= as_aero &&
(psb->m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
const bool use_medium= as_aero;
const bool use_volume= as_pressure ||
as_volume ;
btScalar volume=0;
btScalar ivolumetp=0;
btScalar dvolumetv=0;
btSoftBody::sMedium medium;
if(use_volume)
{
volume = psb->getVolume();
ivolumetp = 1/btFabs(volume)*kPR;
dvolumetv = (psb->m_pose.m_volume-volume)*kVC;
}
/* Per vertex forces */
for(int i=0,ni=psb->m_nodes.size();i<ni;++i)
{
btSoftBody::Node& n=psb->m_nodes[i];
if(n.m_im>0)
{
if(use_medium)
{
EvaluateMedium(psb->m_worldInfo,n.m_x,medium);
/* Aerodynamics */
if(as_vaero)
{
const btVector3 rel_v=n.m_v-medium.m_velocity;
const btScalar rel_v2=rel_v.length2();
if(rel_v2>SIMD_EPSILON)
{
btVector3 nrm=n.m_n;
/* Setup normal */
switch(psb->m_cfg.aeromodel)
{
case btSoftBody::eAeroModel::V_Point:
nrm=NormalizeAny(rel_v);break;
case btSoftBody::eAeroModel::V_TwoSided:
nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
}
const btScalar dvn=dot(rel_v,nrm);
/* Compute forces */
if(dvn>0)
{
btVector3 force(0,0,0);
const btScalar c0 = n.m_area*dvn*rel_v2/2;
const btScalar c1 = c0*medium.m_density;
force += nrm*(-c1*kLF);
force += rel_v.normalized()*(-c1*kDG);
ApplyClampedForce(n,force,dt);
}
}
}
}
/* Pressure */
if(as_pressure)
{
n.m_f += n.m_n*(n.m_area*ivolumetp);
}
/* Volume */
if(as_volume)
{
n.m_f += n.m_n*(n.m_area*dvolumetv);
}
}
}
/* Per face forces */
for(int i=0,ni=psb->m_faces.size();i<ni;++i)
{
btSoftBody::Face& f=psb->m_faces[i];
if(as_faero)
{
const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
EvaluateMedium(psb->m_worldInfo,x,medium);
const btVector3 rel_v=v-medium.m_velocity;
const btScalar rel_v2=rel_v.length2();
if(rel_v2>SIMD_EPSILON)
{
btVector3 nrm=f.m_normal;
/* Setup normal */
switch(psb->m_cfg.aeromodel)
{
case btSoftBody::eAeroModel::F_TwoSided:
nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break;
}
const btScalar dvn=dot(rel_v,nrm);
/* Compute forces */
if(dvn>0)
{
btVector3 force(0,0,0);
const btScalar c0 = f.m_ra*dvn*rel_v2;
const btScalar c1 = c0*medium.m_density;
force += nrm*(-c1*kLF);
force += rel_v.normalized()*(-c1*kDG);
force /= 3;
for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
}
}
}
}
}
//
static void PSolve_Anchors(btSoftBody* psb,btScalar kst)
{
const btScalar kAHR=psb->m_cfg.kAHR*kst;
const btScalar dt=psb->m_sst.sdt;
for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
{
const btSoftBody::Anchor& a=psb->m_anchors[i];
const btTransform& t=a.m_body->getInterpolationWorldTransform();
btSoftBody::Node& n=*a.m_node;
const btVector3 wa=t*a.m_local;
const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
const btVector3 vb=n.m_x-n.m_q;
const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
const btVector3 impulse=a.m_c0*vr;
n.m_x+=impulse*a.m_c2;
a.m_body->applyImpulse(-impulse,a.m_c1);
}
}
//
static void PSolve_RContacts(btSoftBody* psb,btScalar kst)
{
const btScalar dt=psb->m_sst.sdt;
const btScalar mrg=psb->getCollisionShape()->getMargin();
for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
{
const btSoftBody::RContact& c=psb->m_rcontacts[i];
const btSoftBody::sCti& cti=c.m_cti;
const btVector3 va=cti.m_body->getVelocityInLocalPoint(c.m_c1)*dt;
const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
const btVector3 vr=vb-va;
const btScalar dn=dot(vr,cti.m_normal);
if(dn<=SIMD_EPSILON)
{
const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
const btVector3 fv=vr-cti.m_normal*dn;
const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
c.m_node->m_x-=impulse*c.m_c2;
c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
}
}
}
//
static void PSolve_SContacts(btSoftBody* psb,btScalar)
{
for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
{
const btSoftBody::SContact& c=psb->m_scontacts[i];
const btVector3& nr=c.m_normal;
btSoftBody::Node& n=*c.m_node;
btSoftBody::Face& f=*c.m_face;
const btVector3 p=BaryEval( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x,
c.m_weights);
const btVector3 q=BaryEval( f.m_n[0]->m_q,
f.m_n[1]->m_q,
f.m_n[2]->m_q,
c.m_weights);
const btVector3 vr=(n.m_x-n.m_q)-(p-q);
btVector3 corr(0,0,0);
if(dot(vr,nr)<0)
{
const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p));
corr+=c.m_normal*j;
}
corr -= ProjectOnPlane(vr,nr)*c.m_friction;
n.m_x += corr*c.m_cfm[0];
f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
}
}
//
static void PSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
btSoftBody::Link& l=psb->m_links[i];
if(l.m_c0>0)
{
btSoftBody::Node& a=*l.m_n[0];
btSoftBody::Node& b=*l.m_n[1];
const btVector3 del=b.m_x-a.m_x;
const btScalar len=del.length2();
const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
a.m_x-=del*(k*a.m_im);
b.m_x+=del*(k*b.m_im);
}
}
}
//
static void VSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
{
btSoftBody::Link& l=psb->m_links[i];
btSoftBody::Node** n=l.m_n;
const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
}
}
//
static void (* const VSolvers[])(btSoftBody*,btScalar)= {
VSolve_Links,
};
//
static void (* const PSolvers[])(btSoftBody*,btScalar)= {
PSolve_Links,
PSolve_Anchors,
PSolve_RContacts,
PSolve_SContacts,
};
//
// btSoftBody
//
//
btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
:m_worldInfo(worldInfo)
{
/* Init */
m_internalType = CO_SOFT_BODY;
m_cfg.aeromodel = eAeroModel::V_Point;
m_cfg.kVCF = 1;
m_cfg.kDG = 0;
m_cfg.kLF = 0;
m_cfg.kDP = 0;
m_cfg.kPR = 0;
m_cfg.kVC = 0;
m_cfg.kDF = (btScalar)0.2;
m_cfg.kMT = 0;
m_cfg.kCHR = (btScalar)1.0;
m_cfg.kKHR = (btScalar)0.1;
m_cfg.kSHR = (btScalar)1.0;
m_cfg.kAHR = (btScalar)0.7;
m_cfg.maxvolume = (btScalar)1;
m_cfg.timescale = 1;
m_cfg.viterations = 0;
m_cfg.piterations = 1;
m_cfg.diterations = 0;
m_cfg.collisions = fCollision::Default;
m_pose.m_bvolume = false;
m_pose.m_bframe = false;
m_pose.m_volume = 0;
m_pose.m_com = btVector3(0,0,0);
m_pose.m_rot.setIdentity();
m_pose.m_scl.setIdentity();
m_tag = 0;
m_timeacc = 0;
m_bUpdateRtCst = true;
m_bounds[0] = btVector3(0,0,0);
m_bounds[1] = btVector3(0,0,0);
m_worldTransform.setIdentity();
setSolver(eSolverPresets::Positions);
/* Default material */
Material* pm=appendMaterial();
pm->m_kLST = 1;
pm->m_kAST = 1;
pm->m_kVST = 1;
pm->m_flags = fMaterial::Default;
/* Collision shape */
///for now, create a collision shape internally
setCollisionShape(new btSoftBodyCollisionShape(this));
m_collisionShape->setMargin(0.25);
/* Nodes */
const btScalar margin=getCollisionShape()->getMargin();
m_nodes.resize(node_count);
for(int i=0,ni=node_count;i<ni;++i)
{
Node& n=m_nodes[i];
ZeroInitialize(n);
n.m_x = x?*x++:btVector3(0,0,0);
n.m_q = n.m_x;
n.m_im = m?*m++:1;
n.m_im = n.m_im>0?1/n.m_im:0;
n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n);
n.m_material= pm;
}
UpdateBounds(this);
}
//
btSoftBody::~btSoftBody()
{
//for now, delete the internal shape
delete m_collisionShape;
for(int i=0;i<m_materials.size();++i) btAlignedFree(m_materials[i]);
}
//
bool btSoftBody::checkLink(int node0,int node1) const
{
return(checkLink(&m_nodes[node0],&m_nodes[node1]));
}
//
bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
{
const Node* n[]={node0,node1};
for(int i=0,ni=m_links.size();i<ni;++i)
{
const Link& l=m_links[i];
if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
(l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
{
return(true);
}
}
return(false);
}
//
bool btSoftBody::checkFace(int node0,int node1,int node2) const
{
const Node* n[]={ &m_nodes[node0],
&m_nodes[node1],
&m_nodes[node2]};
for(int i=0,ni=m_faces.size();i<ni;++i)
{
const Face& f=m_faces[i];
int c=0;
for(int j=0;j<3;++j)
{
if( (f.m_n[j]==n[0])||
(f.m_n[j]==n[1])||
(f.m_n[j]==n[2])) c|=1<<j; else break;
}
if(c==7) return(true);
}
return(false);
}
//
btSoftBody::Material* btSoftBody::appendMaterial()
{
Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
if(m_materials.size()>0)
*pm=*m_materials[0];
else
ZeroInitialize(*pm);
m_materials.push_back(pm);
return(pm);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
const btVector4& c,
Node* n0,
Node* n1,
Node* n2,
Node* n3)
{
Note n;
ZeroInitialize(n);
n.m_rank = 0;
n.m_text = text;
n.m_offset = o;
n.m_coords[0] = c.x();
n.m_coords[1] = c.y();
n.m_coords[2] = c.z();
n.m_coords[3] = c.w();
n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
m_notes.push_back(n);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Node* feature)
{
appendNote(text,o,btVector4(1,0,0,0),feature);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Link* feature)
{
static const btScalar w=1/(btScalar)2;
appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
feature->m_n[1]);
}
//
void btSoftBody::appendNote( const char* text,
const btVector3& o,
Face* feature)
{
static const btScalar w=1/(btScalar)3;
appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
feature->m_n[1],
feature->m_n[2]);
}
//
void btSoftBody::appendNode( const btVector3& x,btScalar m)
{
if(m_nodes.capacity()==m_nodes.size())
{
PointersToIndices(this);
m_nodes.reserve(m_nodes.size()*2+1);
IndicesToPointers(this);
}
const btScalar margin=getCollisionShape()->getMargin();
m_nodes.push_back(Node());
Node& n=m_nodes[m_nodes.size()-1];
ZeroInitialize(n);
n.m_x = x;
n.m_q = n.m_x;
n.m_im = m>0?1/m:0;
n.m_material = m_materials[0];
n.m_leaf = m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n);
}
//
void btSoftBody::appendLink(int model,Material* mat)
{
Link l;
if(model>=0)
l=m_links[model];
else
{ ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
m_links.push_back(l);
}
//
void btSoftBody::appendLink( int node0,
int node1,
Material* mat,
bool bcheckexist)
{
appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
}
//
void btSoftBody::appendLink( Node* node0,
Node* node1,
Material* mat,
bool bcheckexist)
{
if((!bcheckexist)||(!checkLink(node0,node1)))
{
appendLink(-1,mat);
Link& l=m_links[m_links.size()-1];
l.m_n[0] = node0;
l.m_n[1] = node1;
l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
m_bUpdateRtCst=true;
}
}
//
void btSoftBody::appendFace(int model,Material* mat)
{
Face f;
if(model>=0)
{ f=m_faces[model]; }
else
{ ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
m_faces.push_back(f);
}
//
void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
{
appendFace(-1,mat);
Face& f=m_faces[m_faces.size()-1];
f.m_n[0] = &m_nodes[node0];
f.m_n[1] = &m_nodes[node1];
f.m_n[2] = &m_nodes[node2];
f.m_ra = AreaOf( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x);
m_bUpdateRtCst=true;
}
//
void btSoftBody::appendAnchor(int node,btRigidBody* body)
{
Anchor a;
a.m_node = &m_nodes[node];
a.m_body = body;
a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
a.m_node->m_battach = 1;
m_anchors.push_back(a);
}
//
void btSoftBody::addForce(const btVector3& force)
{
for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
}
//
void btSoftBody::addForce(const btVector3& force,int node)
{
Node& n=m_nodes[node];
if(n.m_im>0)
{
n.m_f += force;
}
}
//
void btSoftBody::addVelocity(const btVector3& velocity)
{
for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
}
//
void btSoftBody::addVelocity(const btVector3& velocity,int node)
{
Node& n=m_nodes[node];
if(n.m_im>0)
{
n.m_v += velocity;
}
}
//
void btSoftBody::setMass(int node,btScalar mass)
{
m_nodes[node].m_im=mass>0?1/mass:0;
m_bUpdateRtCst=true;
}
//
btScalar btSoftBody::getMass(int node) const
{
return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
}
//
btScalar btSoftBody::getTotalMass() const
{
btScalar mass=0;
for(int i=0;i<m_nodes.size();++i)
{
mass+=getMass(i);
}
return(mass);
}
//
void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
{
if(fromfaces)
{
for(int i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im=0;
}
for(int i=0;i<m_faces.size();++i)
{
const Face& f=m_faces[i];
const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
f.m_n[1]->m_x,
f.m_n[2]->m_x);
for(int j=0;j<3;++j)
{
f.m_n[j]->m_im+=twicearea;
}
}
for(int i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im=1/m_nodes[i].m_im;
}
}
const btScalar tm=getTotalMass();
const btScalar itm=1/tm;
for(int i=0;i<m_nodes.size();++i)
{
m_nodes[i].m_im/=itm*mass;
}
m_bUpdateRtCst=true;
}
//
void btSoftBody::setTotalDensity(btScalar density)
{
setTotalMass(getVolume()*density,true);
}
//
void btSoftBody::transform(const btTransform& trs)
{
const btScalar margin=getCollisionShape()->getMargin();
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x=trs*n.m_x;
n.m_q=trs*n.m_q;
n.m_n=trs.getBasis()*n.m_n;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
}
UpdateNormals(this);
UpdateBounds(this);
UpdateConstants(this);
}
//
void btSoftBody::translate(const btVector3& trs)
{
btTransform t;
t.setIdentity();
t.setOrigin(trs);
transform(t);
}
//
void btSoftBody::rotate( const btQuaternion& rot)
{
btTransform t;
t.setIdentity();
t.setRotation(rot);
transform(t);
}
//
void btSoftBody::scale(const btVector3& scl)
{
const btScalar margin=getCollisionShape()->getMargin();
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x*=scl;
n.m_q*=scl;
m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
}
UpdateNormals(this);
UpdateBounds(this);
UpdateConstants(this);
}
//
void btSoftBody::setPose(bool bvolume,bool bframe)
{
m_pose.m_bvolume = bvolume;
m_pose.m_bframe = bframe;
/* Weights */
const btScalar omass=getTotalMass();
const btScalar kmass=omass*m_nodes.size()*1000;
btScalar tmass=omass;
m_pose.m_wgh.resize(m_nodes.size());
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
if(m_nodes[i].m_im<=0) tmass+=kmass;
}
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
m_pose.m_wgh[i]= n.m_im>0 ?
1/(m_nodes[i].m_im*tmass) :
kmass/tmass;
}
/* Pos */
const btVector3 com=EvaluateCom(this);
m_pose.m_pos.resize(m_nodes.size());
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
m_pose.m_pos[i]=m_nodes[i].m_x-com;
}
m_pose.m_volume = bvolume?getVolume():0;
m_pose.m_com = com;
m_pose.m_rot.setIdentity();
m_pose.m_scl.setIdentity();
/* Aqq */
m_pose.m_aqq[0] =
m_pose.m_aqq[1] =
m_pose.m_aqq[2] = btVector3(0,0,0);
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
const btVector3& q=m_pose.m_pos[i];
const btVector3 mq=m_pose.m_wgh[i]*q;
m_pose.m_aqq[0]+=mq.x()*q;
m_pose.m_aqq[1]+=mq.y()*q;
m_pose.m_aqq[2]+=mq.z()*q;
}
m_pose.m_aqq=m_pose.m_aqq.inverse();
UpdateConstants(this);
}
//
btScalar btSoftBody::getVolume() const
{
btScalar vol=0;
if(m_nodes.size()>0)
{
const btVector3 org=m_nodes[0].m_x;
for(int i=0,ni=m_faces.size();i<ni;++i)
{
const Face& f=m_faces[i];
vol+=dot(f.m_n[0]->m_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
}
vol/=(btScalar)6;
}
return(vol);
}
//
int btSoftBody::generateBendingConstraints(int distance,Material* mat)
{
if(distance>1)
{
/* Build graph */
const int n=m_nodes.size();
const unsigned inf=(~(unsigned)0)>>1;
unsigned* adj=new unsigned[n*n];
#define IDX(_x_,_y_) ((_y_)*n+(_x_))
for(int j=0;j<n;++j)
{
for(int i=0;i<n;++i)
{
if(i!=j) adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
else
adj[IDX(i,j)]=adj[IDX(j,i)]=0;
}
}
for(int i=0;i<m_links.size();++i)
{
const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
adj[IDX(ia,ib)]=1;
adj[IDX(ib,ia)]=1;
}
for(int k=0;k<n;++k)
{
for(int j=0;j<n;++j)
{
for(int i=j+1;i<n;++i)
{
const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
if(adj[IDX(i,j)]>sum)
{
adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
}
}
}
}
/* Build links */
int nlinks=0;
for(int j=0;j<n;++j)
{
for(int i=j+1;i<n;++i)
{
if(adj[IDX(i,j)]==(unsigned)distance)
{
appendLink(i,j,mat);
m_links[m_links.size()-1].m_bbending=1;
++nlinks;
}
}
}
delete[] adj;
return(nlinks);
}
return(0);
}
//
void btSoftBody::randomizeConstraints()
{
unsigned long seed=243703;
#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
for(int i=0,ni=m_links.size();i<ni;++i)
{
btSwap(m_links[i],m_links[NEXTRAND%ni]);
}
for(int i=0,ni=m_faces.size();i<ni;++i)
{
btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
}
#undef NEXTRAND
}
//
void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
{
const Node* nbase(&m_nodes[0]);
int ncount(m_nodes.size());
btSymMatrix<int> edges(ncount,-2);
int newnodes=0;
/* Filter out */
for(int i=0;i<m_links.size();++i)
{
Link& l=m_links[i];
if(l.m_bbending)
{
if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
{
btSwap(m_links[i],m_links[m_links.size()-1]);
m_links.pop_back();--i;
}
}
}
/* Fill edges */
for(int i=0;i<m_links.size();++i)
{
Link& l=m_links[i];
edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
}
for(int i=0;i<m_faces.size();++i)
{
Face& f=m_faces[i];
edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
}
/* Intersect */
for(int i=0;i<ncount;++i)
{
for(int j=i+1;j<ncount;++j)
{
if(edges(i,j)==-1)
{
Node& a=m_nodes[i];
Node& b=m_nodes[j];
const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
if(t>0)
{
const btVector3 x=Lerp(a.m_x,b.m_x,t);
const btVector3 v=Lerp(a.m_v,b.m_v,t);
btScalar m=0;
if(a.m_im>0)
{
if(b.m_im>0)
{
const btScalar ma=1/a.m_im;
const btScalar mb=1/b.m_im;
const btScalar mc=Lerp(ma,mb,t);
const btScalar f=(ma+mb)/(ma+mb+mc);
a.m_im=1/(ma*f);
b.m_im=1/(mb*f);
m=mc*f;
}
else
{ a.m_im/=0.5;m=1/a.m_im; }
}
else
{
if(b.m_im>0)
{ b.m_im/=0.5;m=1/b.m_im; }
else
m=0;
}
appendNode(x,m);
edges(i,j)=m_nodes.size()-1;
m_nodes[edges(i,j)].m_v=v;
++newnodes;
}
}
}
}
nbase=&m_nodes[0];
/* Refine links */
for(int i=0,ni=m_links.size();i<ni;++i)
{
Link& feat=m_links[i];
const int idx[]={ int(feat.m_n[0]-nbase),
int(feat.m_n[1]-nbase)};
if((idx[0]<ncount)&&(idx[1]<ncount))
{
const int ni=edges(idx[0],idx[1]);
if(ni>0)
{
appendLink(i);
Link* pft[]={ &m_links[i],
&m_links[m_links.size()-1]};
pft[0]->m_n[0]=&m_nodes[idx[0]];
pft[0]->m_n[1]=&m_nodes[ni];
pft[1]->m_n[0]=&m_nodes[ni];
pft[1]->m_n[1]=&m_nodes[idx[1]];
}
}
}
/* Refine faces */
for(int i=0;i<m_faces.size();++i)
{
const Face& feat=m_faces[i];
const int idx[]={ int(feat.m_n[0]-nbase),
int(feat.m_n[1]-nbase),
int(feat.m_n[2]-nbase)};
for(int j=2,k=0;k<3;j=k++)
{
if((idx[j]<ncount)&&(idx[k]<ncount))
{
const int ni=edges(idx[j],idx[k]);
if(ni>0)
{
appendFace(i);
const int l=(k+1)%3;
Face* pft[]={ &m_faces[i],
&m_faces[m_faces.size()-1]};
pft[0]->m_n[0]=&m_nodes[idx[l]];
pft[0]->m_n[1]=&m_nodes[idx[j]];
pft[0]->m_n[2]=&m_nodes[ni];
pft[1]->m_n[0]=&m_nodes[ni];
pft[1]->m_n[1]=&m_nodes[idx[k]];
pft[1]->m_n[2]=&m_nodes[idx[l]];
appendLink(ni,idx[l],pft[0]->m_material);
--i;break;
}
}
}
}
/* Cut */
if(cut)
{
btAlignedObjectArray<int> cnodes;
const int pcount=ncount;
ncount=m_nodes.size();
cnodes.resize(ncount,0);
/* Nodes */
for(int i=0;i<ncount;++i)
{
const btVector3 x=m_nodes[i].m_x;
if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
{
const btVector3 v=m_nodes[i].m_v;
btScalar m=getMass(i);
if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
appendNode(x,m);
cnodes[i]=m_nodes.size()-1;
m_nodes[cnodes[i]].m_v=v;
}
}
nbase=&m_nodes[0];
/* Links */
for(int i=0,ni=m_links.size();i<ni;++i)
{
const int id[]={ int(m_links[i].m_n[0]-nbase),
int(m_links[i].m_n[1]-nbase)};
int todetach=0;
if(cnodes[id[0]]&&cnodes[id[1]])
{
appendLink(i);
todetach=m_links.size()-1;
}
else
{
if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
(ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
todetach=i;
}
if(todetach)
{
Link& l=m_links[todetach];
for(int j=0;j<2;++j)
{
int cn=cnodes[int(l.m_n[j]-nbase)];
if(cn) l.m_n[j]=&m_nodes[cn];
}
}
}
/* Faces */
for(int i=0,ni=m_faces.size();i<ni;++i)
{
Node** n= m_faces[i].m_n;
if( (ifn->Eval(n[0]->m_x)<accurary)&&
(ifn->Eval(n[1]->m_x)<accurary)&&
(ifn->Eval(n[2]->m_x)<accurary))
{
for(int j=0;j<3;++j)
{
int cn=cnodes[int(n[j]-nbase)];
if(cn) n[j]=&m_nodes[cn];
}
}
}
/* Clean orphans */
int nnodes=m_nodes.size();
btAlignedObjectArray<int> ranks;
btAlignedObjectArray<int> todelete;
ranks.resize(nnodes,0);
for(int i=0,ni=m_links.size();i<ni;++i)
{
for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
}
for(int i=0,ni=m_faces.size();i<ni;++i)
{
for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
}
for(int i=0;i<m_links.size();++i)
{
const int id[]={ int(m_links[i].m_n[0]-nbase),
int(m_links[i].m_n[1]-nbase)};
const bool sg[]={ ranks[id[0]]==1,
ranks[id[1]]==1};
if(sg[0]||sg[1])
{
--ranks[id[0]];
--ranks[id[1]];
btSwap(m_links[i],m_links[m_links.size()-1]);
m_links.pop_back();--i;
}
}
#if 0
for(int i=nnodes-1;i>=0;--i)
{
if(!ranks[i]) todelete.push_back(i);
}
if(todelete.size())
{
btAlignedObjectArray<int>& map=ranks;
for(int i=0;i<nnodes;++i) map[i]=i;
PointersToIndices(this);
for(int i=0,ni=todelete.size();i<ni;++i)
{
int j=todelete[i];
int& a=map[j];
int& b=map[--nnodes];
m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
btSwap(m_nodes[a],m_nodes[b]);
j=a;a=b;b=j;
}
IndicesToPointers(this,&map[0]);
m_nodes.resize(nnodes);
}
#endif
}
m_bUpdateRtCst=true;
}
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);
}
//
bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
{
return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
}
//
bool btSoftBody::cutLink(int node0,int node1,btScalar position)
{
bool done=false;
const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
const btScalar m=1;
appendNode(x,m);
appendNode(x,m);
Node* pa=&m_nodes[node0];
Node* pb=&m_nodes[node1];
Node* pn[2]={ &m_nodes[m_nodes.size()-2],
&m_nodes[m_nodes.size()-1]};
pn[0]->m_v=v;
pn[1]->m_v=v;
for(int i=0,ni=m_links.size();i<ni;++i)
{
const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
if(mtch!=-1)
{
appendLink(i);
Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
pft[0]->m_n[1]=pn[mtch];
pft[1]->m_n[0]=pn[1-mtch];
done=true;
}
}
for(int i=0,ni=m_faces.size();i<ni;++i)
{
for(int k=2,l=0;l<3;k=l++)
{
const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
if(mtch!=-1)
{
appendFace(i);
Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
pft[0]->m_n[l]=pn[mtch];
pft[1]->m_n[k]=pn[1-mtch];
appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
}
}
}
if(!done)
{
m_ndbvt.remove(pn[0]->m_leaf);
m_ndbvt.remove(pn[1]->m_leaf);
m_nodes.pop_back();
m_nodes.pop_back();
}
return(done);
}
//
bool btSoftBody::rayCast(const btVector3& org,
const btVector3& dir,
sRayCast& results,
btScalar maxtime)
{
if(m_faces.size()&&m_fdbvt.empty()) InitializeFaceTree(this);
results.body = this;
results.time = maxtime;
results.feature = eFeature::None;
results.index = -1;
return(RaycastInternal( this,org,dir,
results.time,
results.feature,
results.index,false)!=0);
}
//
void btSoftBody::setSolver(eSolverPresets::_ preset)
{
m_cfg.m_vsequence.clear();
m_cfg.m_psequence.clear();
m_cfg.m_dsequence.clear();
switch(preset)
{
case eSolverPresets::Positions:
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_psequence.push_back(ePSolver::Linear);
break;
case eSolverPresets::Velocities:
m_cfg.m_vsequence.push_back(eVSolver::Linear);
m_cfg.m_psequence.push_back(ePSolver::Anchors);
m_cfg.m_psequence.push_back(ePSolver::RContacts);
m_cfg.m_psequence.push_back(ePSolver::SContacts);
m_cfg.m_dsequence.push_back(ePSolver::Linear);
break;
}
}
//
void btSoftBody::predictMotion(btScalar dt)
{
/* Update */
if(m_bUpdateRtCst)
{
m_bUpdateRtCst=false;
UpdateConstants(this);
m_fdbvt.clear();
if(m_cfg.collisions&fCollision::VF_SS)
{
InitializeFaceTree(this);
}
}
/* Prepare */
m_sst.sdt = dt*m_cfg.timescale;
m_sst.isdt = 1/m_sst.sdt;
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 */
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_q = n.m_x;
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::Volume::FromCR(n.m_x,m_sst.radmrg),
n.m_v*m_sst.velmrg,
m_sst.updmrg);
}
UpdateBounds(this);
/* Faces */
if(!m_fdbvt.empty())
{
for(int i=0;i<m_faces.size();++i)
{
Face& f=m_faces[i];
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,
VolumeOf(f,m_sst.radmrg),
v*m_sst.velmrg,
m_sst.updmrg);
}
}
/* Pose */
UpdatePose(this);
/* Match */
if(m_pose.m_bframe&&(m_cfg.kMT>0))
{
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
if(n.m_im>0)
{
const btVector3 x=m_pose.m_rot*m_pose.m_pos[i]+m_pose.m_com;
n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
}
}
}
/* Clear contacts */
m_rcontacts.resize(0);
m_scontacts.resize(0);
/* Optimize dbvt's */
m_ndbvt.optimizeIncremental(1);
m_fdbvt.optimizeIncremental(1);
}
//
void btSoftBody::solveConstraints()
{
/* Prepare links */
for(int i=0,ni=m_links.size();i<ni;++i)
{
Link& l=m_links[i];
l.m_c3 = l.m_n[1]->m_x-l.m_n[0]->m_x;
l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
}
/* Prepare anchors */
for(int i=0,ni=m_anchors.size();i<ni;++i)
{
Anchor& a=m_anchors[i];
const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
a.m_c0 = ImpulseMatrix( m_sst.sdt,
a.m_node->m_im,
a.m_body->getInvMass(),
a.m_body->getInvInertiaTensorWorld(),
ra);
a.m_c1 = ra;
a.m_c2 = m_sst.sdt*a.m_node->m_im;
a.m_body->activate();
}
/* Solve velocities */
if(m_cfg.viterations>0)
{
/* Solve */
for(int isolve=0;isolve<m_cfg.viterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
{
VSolvers[m_cfg.m_vsequence[iseq]](this,1);
}
}
/* Update */
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_x = n.m_q+n.m_v*m_sst.sdt;
}
}
/* Solve positions */
if(m_cfg.piterations>0)
{
for(int isolve=0;isolve<m_cfg.piterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
PSolvers[m_cfg.m_psequence[iseq]](this,1);
}
}
const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_v = (n.m_x-n.m_q)*vc;
n.m_f = btVector3(0,0,0);
}
}
/* Solve drift */
if(m_cfg.diterations>0)
{
const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_q = n.m_x;
}
for(int idrift=0;idrift<m_cfg.diterations;++idrift)
{
for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
{
PSolvers[m_cfg.m_dsequence[iseq]](this,1);
}
}
for(int i=0,ni=m_nodes.size();i<ni;++i)
{
Node& n=m_nodes[i];
n.m_v += (n.m_x-n.m_q)*vcf;
}
}
}
//
void btSoftBody::staticSolve(int iterations)
{
for(int isolve=0;isolve<iterations;++isolve)
{
for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
{
PSolvers[m_cfg.m_psequence[iseq]](this,1);
}
}
}
//
void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
{
/// placeholder
}
//
void btSoftBody::integrateMotion()
{
/* Update */
UpdateNormals(this);
}
//
void btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
{
switch(m_cfg.collisions&fCollision::RVSmask)
{
case fCollision::SDF_RS:
{
struct DoCollide : btDbvt::ICollide
{
void Process(const btDbvt::Node* leaf)
{
Node* node=(Node*)leaf->data;
DoNode(*node);
}
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,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;
} docollide;
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::Volume volume;
pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(),
mins,
maxs);
volume=btDbvt::Volume::FromMM(mins,maxs);
volume.Expand(btVector3(basemargin,basemargin,basemargin));
docollide.psb = this;
docollide.prb = prb;
docollide.dynmargin = basemargin+timemargin;
docollide.stamargin = basemargin;
btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
}
break;
}
}
//
void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
const int cf=m_cfg.collisions&psb->m_cfg.collisions;
switch(cf&fCollision::SVSmask)
{
case fCollision::VF_SS:
{
struct DoCollide : btDbvt::ICollide
{
void Process(const btDbvt::Node* lnode,
const btDbvt::Node* lface)
{
Node* node=(Node*)lnode->data;
Face* face=(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 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)
{
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;
} docollide;
/* common */
docollide.mrg= getCollisionShape()->getMargin()+
psb->getCollisionShape()->getMargin();
/* psb0 nodes vs psb1 faces */
docollide.psb[0]=this;
docollide.psb[1]=psb;
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
/* psb1 nodes vs psb0 faces */
docollide.psb[0]=psb;
docollide.psb[1]=this;
btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root,
docollide.psb[1]->m_fdbvt.m_root,
docollide);
}
break;
}
}
Reference in New Issue View Git Blame Copy Permalink