Logo Search packages:      
Sourcecode: blender version File versions  Download package

btSoftBodyInternals.h

/*
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>
00032 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_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
            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 void      setLocalScaling(const btVector3& /*scaling*/)
      {           
            ///na
      }
      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 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]}};
            int i;

            for(i=0;i<3;++i) a[p][i]=c*m[0][i]-s*m[1][i];
            for(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]}};
            int i;

            for(i=0;i<3;++i) a[i][p]=c*m[0][i]-s*m[1][i];
            for(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;
            
            btCollisionObject*      m_colObj;
            void        Process(const btDbvtNode* leaf)
            {
                  btSoftBody::Cluster*          cluster=(btSoftBody::Cluster*)leaf->data;
                  btSoftClusterCollisionShape   cshape(cluster);
                  const btConvexShape*          rshape=(const btConvexShape*)m_colObj->getCollisionShape();
                  btGjkEpaSolver2::sResults     res;        
                  if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
                        rshape,m_colObj->getInterpolationWorldTransform(),
                        btVector3(1,0,0),res))
                  {
                        btSoftBody::CJoint      joint;
                        if(SolveContact(res,cluster,m_colObj,joint))//prb,joint))
                        {
                              btSoftBody::CJoint*     pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
                              *pj=joint;psb->m_joints.push_back(pj);
                              if(m_colObj->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,btCollisionObject* colOb)
            {
                  psb               =     ps;
                  m_colObj                =     colOb;
                  idt               =     ps->m_sst.isdt;
                  margin            =     m_colObj->getCollisionShape()->getMargin();
                  ///Bullet rigid body uses multiply instead of minimum to determine combined friction. Some customization would be useful.
                  friction    =     btMin(psb->m_cfg.kDF,m_colObj->getFriction());
                  btVector3               mins;
                  btVector3               maxs;

                  ATTRIBUTE_ALIGNED16(btDbvtVolume)         volume;
                  colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs);
                  volume=btDbvtVolume::FromMM(mins,maxs);
                  volume.Expand(btVector3(1,1,1)*margin);
                  ps->m_cdbvt.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;


                  bool connected=false;
                  if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
                  {
                        connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
                  }

                  if (!connected)
                  {
                        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;
                              }
                        }
                  } else
                  {
                        static int count=0;
                        count++;
                        //printf("count=%d\n",count);
                        
                  }
            }
            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;
                  psa->m_cdbvt.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(m_colObj1,n.m_x,m,c.m_cti))
                  {
                        const btScalar    ima=n.m_im;
                        const btScalar    imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f;
                        const btScalar    ms=ima+imb;
                        if(ms>0)
                        {
                              const btTransform&      wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform();
                              static const btMatrix3x3      iwiStatic(0,0,0,0,0,0,0,0,0);
                              const btMatrix3x3&      iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
                              const btVector3         ra=n.m_x-wtr.getOrigin();
                              const btVector3         va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0);
                              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*m_colObj1->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            =     m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR;
                              psb->m_rcontacts.push_back(c);
                              if (m_rigidBody)
                                    m_rigidBody->activate();
                        }
                  }
            }
            btSoftBody*       psb;
            btCollisionObject*      m_colObj1;
            btRigidBody*      m_rigidBody;
            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

Generated by  Doxygen 1.6.0   Back to index