btSoftBodyInternals.h

Go to the documentation of this file.

/*
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.
*/

#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"
#include <string.h> //for memset
//
// btSymMatrix
//
template <typename T>
struct btSymMatrix
{
    btSymMatrix() : dim(0)                  {}
    btSymMatrix(int n,const T& init=T())    { resize(n,init); }
    void                    resize(int n,const T& init=T())         { dim=n;store.resize((n*(n+1))/2,init); }
    int                     index(int c,int r) const                { if(c>r) btSwap(c,r);btAssert(r<dim);return((r*(r+1))/2+c); }
    T&                      operator()(int c,int r)                 { return(store[index(c,r)]); }
    const T&                operator()(int c,int r) const           { return(store[index(c,r)]); }
    btAlignedObjectArray<T> store;
    int                     dim;
};  

//
// btSoftBodyCollisionShape
//
class btSoftBodyCollisionShape : public btConcaveShape
{
public:
    btSoftBody*                     m_body;

    btSoftBodyCollisionShape(btSoftBody* backptr)
    {
        m_shapeType = SOFTBODY_SHAPE_PROXYTYPE;
        m_body=backptr;
    }

    virtual ~btSoftBodyCollisionShape()
    {

    }

    void    processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const
    {
        //not yet
        btAssert(0);
    }

    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*/)
    {       
    }
    virtual const btVector3& getLocalScaling() const
    {
        static const btVector3 dummy(1,1,1);
        return dummy;
    }
    virtual void    calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const
    {
        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=btDot(vec,n[0]->m_x);
        int                                             j=0;
        for(int i=1,ni=m_cluster->m_nodes.size();i<ni;++i)
        {
            const btScalar  k=btDot(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)
{
    memset(&value,0,sizeof(T));
}
//
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]=btCross(m[0],m[1]).normalized();
    m[1]=btCross(m[2],m[0]).normalized();
    m[0]=btCross(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*btDot(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>(-btDot(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=btCross(b-a,c-a);
    const btScalar      m2=q.length2();
    if(m2>SIMD_EPSILON)
    {
        const btVector3 n=q/btSqrt(m2);
        const btScalar  k=btDot(a,n);
        const btScalar  k2=k*k;
        if(k2<sqd)
        {
            const btVector3 p=n*k;
            if( (btDot(btCross(a-p,b-p),q)>0)&&
                (btDot(btCross(b-p,c-p),q)>0)&&
                (btDot(btCross(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[]={   btCross(a-p,b-p).length(),
        btCross(b-p,c-p).length(),
        btCross(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=btCross(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(btDot(a,btCross(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=-(btDot(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            m_margin;
        btScalar            friction;
        btScalar            threshold;
        ClusterBase()
        {
            erp         =(btScalar)1;
            idt         =0;
            m_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<m_margin)
            {
                btVector3 norm = res.normal;
                norm.normalize();//is it necessary?

                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=btDot(vrel,norm);
                 btScalar       depth=res.distance-m_margin;
                
//              printf("depth=%f\n",depth);
                const btVector3     iv=norm*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*norm;

                joint.m_normal      =   norm;
//              printf("normal=%f,%f,%f\n",res.normal.getX(),res.normal.getY(),res.normal.getZ());
                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();

            if(m_colObj->isStaticOrKinematicObject() && cluster->m_containsAnchor)
                return;

            btGjkEpaSolver2::sResults   res;        
            if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(),
                rshape,m_colObj->getWorldTransform(),
                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;
            m_margin        =   m_colObj->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin();
            friction    =   btMin(psb->m_cfg.kDF,m_colObj->getFriction());
            btVector3           mins;
            btVector3           maxs;

            ATTRIBUTE_ALIGNED16(btDbvtVolume)       volume;
            colOb->getCollisionShape()->getAabb(colOb->getWorldTransform(),mins,maxs);
            volume=btDbvtVolume::FromMM(mins,maxs);
            volume.Expand(btVector3(1,1,1)*m_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;
            //m_margin      =   (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2;
            m_margin        =   (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin());
            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->getWorldTransform() : 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=btDot(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