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

btSoftBody.cpp

/*
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 "btSoftBodyInternals.h"

//
00020 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.kSRHR_CL          =     (btScalar)0.1;
      m_cfg.kSKHR_CL          =     (btScalar)1;
      m_cfg.kSSHR_CL          =     (btScalar)0.5;
      m_cfg.kSR_SPLT_CL =     (btScalar)0.5;
      m_cfg.kSK_SPLT_CL =     (btScalar)0.5;
      m_cfg.kSS_SPLT_CL =     (btScalar)0.5;
      m_cfg.maxvolume         =     (btScalar)1;
      m_cfg.timescale         =     1;
      m_cfg.viterations =     0;
      m_cfg.piterations =     1;    
      m_cfg.diterations =     0;
      m_cfg.citerations =     4;
      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
      m_collisionShape = 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(btDbvtVolume::FromCR(n.m_x,margin),&n);
            n.m_material=     pm;
      }
      updateBounds();   

      m_initialWorldTransform.setIdentity();
}

//
btSoftBody::~btSoftBody()
{
      //for now, delete the internal shape
      delete m_collisionShape;      
      int i;

      releaseClusters();
      for(i=0;i<m_materials.size();++i) 
            btAlignedFree(m_materials[i]);
      for(i=0;i<m_joints.size();++i) 
            btAlignedFree(m_joints[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();
      m_nodes.reserve(m_nodes.size()*2+1);
      indicesToPointers();
      }
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(btDbvtVolume::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)
{
      if (node0==node1)
            return;
      if (node1==node2)
            return;
      if (node2==node0)
            return;

      appendFace(-1,mat);
      Face& f=m_faces[m_faces.size()-1];
      btAssert(node0!=node1);
      btAssert(node1!=node2);
      btAssert(node2!=node0);
      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,bool disableCollisionWithBody=false)
{
      if (disableCollisionWithBody)
      {
            if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
            {
                  m_collisionDisabledObjects.push_back(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::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
{
LJoint*           pj    =     new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
pj->m_bodies[0]   =     body0;
pj->m_bodies[1]   =     body1;
pj->m_refs[0]     =     pj->m_bodies[0].xform().inverse()*specs.position;
pj->m_refs[1]     =     pj->m_bodies[1].xform().inverse()*specs.position;
pj->m_cfm         =     specs.cfm;
pj->m_erp         =     specs.erp;
pj->m_split       =     specs.split;
m_joints.push_back(pj);
}

//
void              btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
{
appendLinearJoint(specs,m_clusters[0],body);
}

//
void              btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
{
appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
}

//
void              btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
{
AJoint*           pj    =     new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
pj->m_bodies[0]   =     body0;
pj->m_bodies[1]   =     body1;
pj->m_refs[0]     =     pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
pj->m_refs[1]     =     pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
pj->m_cfm         =     specs.cfm;
pj->m_erp         =     specs.erp;
pj->m_split       =     specs.split;
pj->m_icontrol    =     specs.icontrol;
m_joints.push_back(pj);
}

//
void              btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
{
appendAngularJoint(specs,m_clusters[0],body);
}

//
void              btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
{
appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
}

//
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);
}

/* Set velocity for the entire body                                                       */ 
void                    btSoftBody::setVelocity(      const btVector3& velocity)
{
      for(int i=0,ni=m_nodes.size();i<ni;++i) 
      {
            Node& n=m_nodes[i];
            if(n.m_im>0)
            {
                  n.m_v =     velocity;
            }
      }
}


//
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)
{
      int i;

      if(fromfaces)
      {

            for(i=0;i<m_nodes.size();++i)
            {
                  m_nodes[i].m_im=0;
            }
            for(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( 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( 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,btDbvtVolume::FromCR(n.m_x,margin));
      }
      updateNormals();
      updateBounds();
      updateConstants();
      m_initialWorldTransform = trs;
}

//
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,btDbvtVolume::FromCR(n.m_x,margin));
      }
      updateNormals();
      updateBounds();
      updateConstants();
}

//
void              btSoftBody::setPose(bool bvolume,bool bframe)
{
      m_pose.m_bvolume  =     bvolume;
      m_pose.m_bframe         =     bframe;
      int i,ni;

      /* 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(i=0,ni=m_nodes.size();i<ni;++i)
      {
            if(m_nodes[i].m_im<=0) tmass+=kmass;
      }
      for( 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();
      m_pose.m_pos.resize(m_nodes.size());
      for( 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( 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();
}

//
btScalar          btSoftBody::getVolume() const
{
      btScalar    vol=0;
      if(m_nodes.size()>0)
      {
            int i,ni;

            const btVector3   org=m_nodes[0].m_x;
            for(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::clusterCount() const
{
return(m_clusters.size());
}

//
btVector3         btSoftBody::clusterCom(const Cluster* cluster)
{
btVector3         com(0,0,0);
for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
      {
      com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
      }
return(com*cluster->m_imass);
}

//
btVector3         btSoftBody::clusterCom(int cluster) const
{
return(clusterCom(m_clusters[cluster]));
}

//
btVector3         btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
{
return(cluster->m_lv+cross(cluster->m_av,rpos));
}

//
void              btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
{
const btVector3   li=cluster->m_imass*impulse;
const btVector3   ai=cluster->m_invwi*cross(rpos,impulse);
cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
cluster->m_nvimpulses++;
}

//
void              btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
{
const btVector3   li=cluster->m_imass*impulse;
const btVector3   ai=cluster->m_invwi*cross(rpos,impulse);
cluster->m_dimpulses[0]+=li;
cluster->m_dimpulses[1]+=ai;
cluster->m_ndimpulses++;
}

//
void              btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
{
if(impulse.m_asVelocity)      clusterVImpulse(cluster,rpos,impulse.m_velocity);
if(impulse.m_asDrift)         clusterDImpulse(cluster,rpos,impulse.m_drift);
}

//
void              btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
{
const btVector3   ai=cluster->m_invwi*impulse;
cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
cluster->m_nvimpulses++;
}

//
void              btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
{
const btVector3   ai=cluster->m_invwi*impulse;
cluster->m_dimpulses[1]+=ai;
cluster->m_ndimpulses++;
}

//
void              btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
{
if(impulse.m_asVelocity)      clusterVAImpulse(cluster,impulse.m_velocity);
if(impulse.m_asDrift)         clusterDAImpulse(cluster,impulse.m_drift);
}

//
void              btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
{
cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
cluster->m_ndimpulses++;
}

//
int                     btSoftBody::generateBendingConstraints(int distance,Material* mat)
{
      int i,j;

      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(j=0;j<n;++j)
            {
                  for(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( 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(j=0;j<n;++j)
                  {
                        for(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(j=0;j<n;++j)
            {
                  for(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)
int i,ni;

      for(i=0,ni=m_links.size();i<ni;++i)
      {
            btSwap(m_links[i],m_links[NEXTRAND%ni]);
      }
      for(i=0,ni=m_faces.size();i<ni;++i)
      {
            btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
      }
#undef NEXTRAND
}

//
void              btSoftBody::releaseCluster(int index)
{
Cluster*    c=m_clusters[index];
if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
c->~Cluster();
btAlignedFree(c);
m_clusters.remove(c);
}

//
void              btSoftBody::releaseClusters()
{
while(m_clusters.size()>0) releaseCluster(0);
}

//
int                     btSoftBody::generateClusters(int k,int maxiterations)
{
int i;
releaseClusters();
m_clusters.resize(btMin(k,m_nodes.size()));
for(i=0;i<m_clusters.size();++i)
      {
      m_clusters[i]                 =     new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
      m_clusters[i]->m_collide=     true;
      }
k=m_clusters.size();
if(k>0)
      {
      /* Initialize           */ 
      btAlignedObjectArray<btVector3>     centers;
      btVector3                                 cog(0,0,0);
      int                                             i;
      for(i=0;i<m_nodes.size();++i)
            {
            cog+=m_nodes[i].m_x;
            m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
            }
      cog/=(btScalar)m_nodes.size();
      centers.resize(k,cog);
      /* Iterate              */ 
      const btScalar    slope=16;
      bool              changed;
      int                     iterations=0;
      do    {
            const btScalar    w=2-btMin<btScalar>(1,iterations/slope);
            changed=false;
            iterations++;     
            int i;

            for(i=0;i<k;++i)
                  {
                  btVector3   c(0,0,0);
                  for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
                        {
                        c+=m_clusters[i]->m_nodes[j]->m_x;
                        }
                  if(m_clusters[i]->m_nodes.size())
                        {
                        c                 /=    (btScalar)m_clusters[i]->m_nodes.size();
                        c                 =     centers[i]+(c-centers[i])*w;
                        changed           |=    ((c-centers[i]).length2()>SIMD_EPSILON);
                        centers[i]  =     c;
                        m_clusters[i]->m_nodes.resize(0);
                        }                 
                  }
            for(i=0;i<m_nodes.size();++i)
                  {
                  const btVector3   nx=m_nodes[i].m_x;
                  int                     kbest=0;
                  btScalar          kdist=ClusterMetric(centers[0],nx);
                  for(int j=1;j<k;++j)
                        {
                        const btScalar    d=ClusterMetric(centers[j],nx);
                        if(d<kdist)
                              {
                              kbest=j;
                              kdist=d;
                              }
                        }
                  m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
                  }           
            } while(changed&&(iterations<maxiterations));
      /* Merge          */ 
      btAlignedObjectArray<int>     cids;
      cids.resize(m_nodes.size(),-1);
      for(i=0;i<m_clusters.size();++i)
            {
            for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
                  {
                  cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
                  }
            }
      for(i=0;i<m_faces.size();++i)
            {
            const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
                                          int(m_faces[i].m_n[1]-&m_nodes[0]),
                                          int(m_faces[i].m_n[2]-&m_nodes[0])};
            for(int j=0;j<3;++j)
                  {
                  const int cid=cids[idx[j]];
                  for(int q=1;q<3;++q)
                        {
                        const int kid=idx[(j+q)%3];
                        if(cids[kid]!=cid)
                              {
                              if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
                                    {
                                    m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
                                    }
                              }
                        }
                  }
            }
      /* Master         */ 
      if(m_clusters.size()>1)
            {
            Cluster*    pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
            pmaster->m_collide      =     false;
            pmaster->m_nodes.reserve(m_nodes.size());
            for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
            m_clusters.push_back(pmaster);
            btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
            }
      /* Terminate      */ 
      for(i=0;i<m_clusters.size();++i)
            {
            if(m_clusters[i]->m_nodes.size()==0)
                  {
                  releaseCluster(i--);
                  }
            }
            
      initializeClusters();
      updateClusters();
      return(m_clusters.size());
      }
return(0);
}

//
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;
int i,j,k,ni;

/* Filter out           */ 
for(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(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(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(i=0;i<ncount;++i)
      {
      for(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(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(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(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;
      int                                       i;
      ncount=m_nodes.size();
      cnodes.resize(ncount,0);
      /* Nodes          */ 
      for(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(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(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(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(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(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(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;
}

//
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;
int i,ni;
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(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(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();
results.body      =     this;
results.time      =     maxtime;
results.feature   =     eFeature::None;
results.index     =     -1;
return(rayCast(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)
{
      int i,ni;

      /* Update                     */ 
      if(m_bUpdateRtCst)
      {
            m_bUpdateRtCst=false;
            updateConstants();
            m_fdbvt.clear();
            if(m_cfg.collisions&fCollision::VF_SS)
                  {
                  initializeFaceTree();               
                  }
      }
            
      /* 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();
      /* Integrate                  */ 
      for(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;
            n.m_f =     btVector3(0,0,0);
      }
      /* Clusters                   */ 
      updateClusters();
      /* Bounds                     */ 
      updateBounds();   
      /* Nodes                      */ 
      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            Node& n=m_nodes[i];
            m_ndbvt.update(   n.m_leaf,
                                    btDbvtVolume::FromCR(n.m_x,m_sst.radmrg),
                                    n.m_v*m_sst.velmrg,
                                    m_sst.updmrg);
      }
      /* 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();
      /* Match                      */ 
      if(m_pose.m_bframe&&(m_cfg.kMT>0))
            {
            const btMatrix3x3 posetrs=m_pose.m_rot;
            for(int i=0,ni=m_nodes.size();i<ni;++i)
                  {
                  Node& n=m_nodes[i];
                  if(n.m_im>0)
                        {
                        const btVector3   x=posetrs*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);
      m_cdbvt.optimizeIncremental(1);
}

//
void              btSoftBody::solveConstraints()
{
/* Apply clusters       */ 
applyClusters(false);
/* Prepare links        */ 

int i,ni;

for(i=0,ni=m_links.size();i<ni;++i)
      {
      Link& l=m_links[i];
      l.m_c3            =     l.m_n[1]->m_q-l.m_n[0]->m_q;
      l.m_c2            =     1/(l.m_c3.length2()*l.m_c0);
      }
/* Prepare anchors            */ 
for(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)
                  {
                  getSolver(m_cfg.m_vsequence[iseq])(this,1);
                  }
            }
      /* Update               */ 
      for(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)
            {
            const btScalar ti=isolve/(btScalar)m_cfg.piterations;
            for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
                  {
                  getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
                  }
            }
      const btScalar    vc=m_sst.isdt*(1-m_cfg.kDP);
      for(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(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)
                  {
                  getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
                  }
            }
      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;
            }
      }
/* Apply clusters       */ 
dampClusters();
applyClusters(true);
}

//
void              btSoftBody::staticSolve(int iterations)
{
for(int isolve=0;isolve<iterations;++isolve)
      {
      for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
            {
            getSolver(m_cfg.m_psequence[iseq])(this,1,0);
            }
      }
}

//
01467 void              btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
{
/// placeholder
}

//
void              btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
{
const int   nb=bodies.size();
int               iterations=0;
int i;

for(i=0;i<nb;++i)
      {
      iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
      }
for(i=0;i<nb;++i)
      {
      bodies[i]->prepareClusters(iterations);
      }
for(i=0;i<iterations;++i)
      {
      const btScalar sor=1;
      for(int j=0;j<nb;++j)
            {
            bodies[j]->solveClusters(sor);
            }
      }
for(i=0;i<nb;++i)
      {
      bodies[i]->cleanupClusters();
      }
}

//
void              btSoftBody::integrateMotion()
{
      /* Update               */ 
      updateNormals();
}

//
                              btSoftBody::RayCaster::RayCaster(const btVector3& org,const btVector3& dir,btScalar mxt)
{
o           =     org;
d           =     dir;
mint  =     mxt;
face  =     0;
tests =     0;
}

//
void                    btSoftBody::RayCaster::Process(const btDbvtNode* 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;
}

//
btScalar                btSoftBody::RayCaster::rayTriangle( const btVector3& org,
                                                                                    const btVector3& dir,
                                                                                    const btVector3& a,
                                                                                    const btVector3& b,
                                                                                    const btVector3& c,
                                                                                    btScalar maxt)
{
      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);
}

//
void                    btSoftBody::pointersToIndices()
{
#define     PTR2IDX(_p_,_b_)  reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
      btSoftBody::Node* base=&m_nodes[0];
      int i,ni;

      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            if(m_nodes[i].m_leaf)
                  {
                  m_nodes[i].m_leaf->data=*(void**)&i;
                  }
      }
      for(i=0,ni=m_links.size();i<ni;++i)
      {
            m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
            m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
      }
      for(i=0,ni=m_faces.size();i<ni;++i)
      {
            m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
            m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
            m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
            if(m_faces[i].m_leaf)
                  {
                  m_faces[i].m_leaf->data=*(void**)&i;
                  }
      }
      for(i=0,ni=m_anchors.size();i<ni;++i)
            {
            m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
            }
      for(i=0,ni=m_notes.size();i<ni;++i)
            {
            for(int j=0;j<m_notes[i].m_rank;++j)
                  {
                  m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
                  }
            }
#undef      PTR2IDX
}

//
void                    btSoftBody::indicesToPointers(const int* map)
{
#define     IDX2PTR(_p_,_b_)  map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]):     \
                                                (&(_b_)[(((char*)_p_)-(char*)0)])
      btSoftBody::Node* base=&m_nodes[0];
      int i,ni;

      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            if(m_nodes[i].m_leaf)
                  {
                  m_nodes[i].m_leaf->data=&m_nodes[i];
                  }
      }
      for(i=0,ni=m_links.size();i<ni;++i)
      {
            m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
            m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
      }
      for(i=0,ni=m_faces.size();i<ni;++i)
      {
            m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
            m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
            m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
            if(m_faces[i].m_leaf)
                  {
                  m_faces[i].m_leaf->data=&m_faces[i];
                  }
      }
      for(i=0,ni=m_anchors.size();i<ni;++i)
            {
            m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
            }
      for(i=0,ni=m_notes.size();i<ni;++i)
            {
            for(int j=0;j<m_notes[i].m_rank;++j)
                  {
                  m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
                  }
            }
#undef      IDX2PTR
}

//
int                           btSoftBody::rayCast(const btVector3& org,const btVector3& dir,
                                                            btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
{
      int   cnt=0;
      if(bcountonly||m_fdbvt.empty())
            {/* Full search   */ 
            for(int i=0,ni=m_faces.size();i<ni;++i)
                  {
                  const btSoftBody::Face& f=m_faces[i];
                  const btScalar                t=RayCaster::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(m_fdbvt.m_root,org,dir,collider);
            if(collider.face)
                  {
                  mint=collider.mint;
                  feature=btSoftBody::eFeature::Face;
                  index=(int)(collider.face-&m_faces[0]);
                  cnt=1;
                  }
            }
      return(cnt);
}

//
void              btSoftBody::initializeFaceTree()
{
m_fdbvt.clear();
for(int i=0;i<m_faces.size();++i)
      {
      Face& f=m_faces[i];
      f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
      }
}

//
btVector3         btSoftBody::evaluateCom() const
{
btVector3   com(0,0,0);
if(m_pose.m_bframe)
      {
      for(int i=0,ni=m_nodes.size();i<ni;++i)
            {
            com+=m_nodes[i].m_x*m_pose.m_wgh[i];
            }
      }
      return(com);
}
      
//
bool                    btSoftBody::checkContact(     btRigidBody* prb,
                                                                        const btVector3& x,
                                                                        btScalar margin,
                                                                        btSoftBody::sCti& cti) const
{
      btVector3               nrm;
      btCollisionShape* shp=prb->getCollisionShape();
      const btTransform&      wtr=prb->getInterpolationWorldTransform();
      btScalar                dst=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);
}

//
void                          btSoftBody::updateNormals()
{
      const btVector3   zv(0,0,0);
      int i,ni;

      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            m_nodes[i].m_n=zv;
      }
      for(i=0,ni=m_faces.size();i<ni;++i)
      {
            btSoftBody::Face& f=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(i=0,ni=m_nodes.size();i<ni;++i)
      {
            btScalar len = m_nodes[i].m_n.length();
            if (len>SIMD_EPSILON)
                  m_nodes[i].m_n /= len;
      }
}

//
void                          btSoftBody::updateBounds()
{
      if(m_ndbvt.m_root)
            {
            const btVector3&  mins=m_ndbvt.m_root->volume.Mins();
            const btVector3&  maxs=m_ndbvt.m_root->volume.Maxs();
            const btScalar          csm=getCollisionShape()->getMargin();
            const btVector3         mrg=btVector3(    csm,
                                                                  csm,
                                                                  csm)*1; // ??? to investigate...
            m_bounds[0]=mins-mrg;
            m_bounds[1]=maxs+mrg;
                  if(0!=getBroadphaseHandle())
                  {                             
                        m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
                                                                              m_bounds[0],
                                                                              m_bounds[1],
                                                                              m_worldInfo->m_dispatcher);
                  }
            }
            else
            {
            m_bounds[0]=
            m_bounds[1]=btVector3(0,0,0);
            }           
}


//
void                          btSoftBody::updatePose()
{
      if(m_pose.m_bframe)
      {
            btSoftBody::Pose& pose=m_pose;
            const btVector3         com=evaluateCom();
            /* Com                  */ 
            pose.m_com  =     com;
            /* Rotation       */ 
            btMatrix3x3       Apq;
            const btScalar    eps=SIMD_EPSILON;
            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=m_nodes.size();i<ni;++i)
            {
                  const btVector3         a=pose.m_wgh[i]*(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(m_cfg.maxvolume>1)
                  {
                  const btScalar    idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
                                                                              1,m_cfg.maxvolume);
                  pose.m_scl=Mul(pose.m_scl,idet);
                  }
            
      }
}

//
void                    btSoftBody::updateConstants()
{
      int i,ni;

      /* Links          */ 
      for(i=0,ni=m_links.size();i<ni;++i)
      {
            Link&       l=m_links[i];
            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(i=0,ni=m_faces.size();i<ni;++i)
      {
            Face&       f=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(m_nodes.size(),0);
      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            m_nodes[i].m_area =     0;
      }
      for(i=0,ni=m_faces.size();i<ni;++i)
      {
            btSoftBody::Face& f=m_faces[i];
            for(int j=0;j<3;++j)
            {
                  const int index=(int)(f.m_n[j]-&m_nodes[0]);
                  counts[index]++;
                  f.m_n[j]->m_area+=btFabs(f.m_ra);
            }
      }
      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            if(counts[i]>0)
                  m_nodes[i].m_area/=(btScalar)counts[i];
            else
                  m_nodes[i].m_area=0;
      }
}

//
void                          btSoftBody::initializeClusters()
{
      int i;

for( i=0;i<m_clusters.size();++i)
      {
      Cluster&    c=*m_clusters[i];
      c.m_imass=0;
      c.m_masses.resize(c.m_nodes.size());
      for(int j=0;j<c.m_nodes.size();++j)
            {
            c.m_masses[j]     =     c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0;
            c.m_imass         +=    c.m_masses[j];
            }
      c.m_imass         =     1/c.m_imass;
      c.m_com                 =     btSoftBody::clusterCom(&c);
      c.m_lv                  =     btVector3(0,0,0);
      c.m_av                  =     btVector3(0,0,0);
      c.m_leaf          =     0;
      /* Inertia  */ 
      btMatrix3x3&      ii=c.m_locii;
      ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
      {
            int i,ni;

            for(i=0,ni=c.m_nodes.size();i<ni;++i)
            {
                  const btVector3   k=c.m_nodes[i]->m_x-c.m_com;
                  const btVector3   q=k*k;
                  const btScalar    m=c.m_masses[i];
                  ii[0][0]    +=    m*(q[1]+q[2]);
                  ii[1][1]    +=    m*(q[0]+q[2]);
                  ii[2][2]    +=    m*(q[0]+q[1]);
                  ii[0][1]    -=    m*k[0]*k[1];
                  ii[0][2]    -=    m*k[0]*k[2];
                  ii[1][2]    -=    m*k[1]*k[2];
            }
      }
      ii[1][0]=ii[0][1];
      ii[2][0]=ii[0][2];
      ii[2][1]=ii[1][2];
      ii=ii.inverse();
      /* Frame    */ 
      c.m_framexform.setIdentity();
      c.m_framexform.setOrigin(c.m_com);
      c.m_framerefs.resize(c.m_nodes.size());
      {
            int i;
            for(i=0;i<c.m_framerefs.size();++i)
                  {
                  c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
                  }
            }
      }
}

//
void                          btSoftBody::updateClusters()
{
BT_PROFILE("UpdateClusters");
int i;

for(i=0;i<m_clusters.size();++i)
      {
      btSoftBody::Cluster&    c=*m_clusters[i];
      const int                     n=c.m_nodes.size();
      const btScalar                invn=1/(btScalar)n;
      if(n)
            {
            /* Frame                      */ 
            const btScalar    eps=btScalar(0.0001);
            btMatrix3x3       m,r,s;
            m[0]=m[1]=m[2]=btVector3(0,0,0);
            m[0][0]=eps*1;
            m[1][1]=eps*2;
            m[2][2]=eps*3;
            c.m_com=clusterCom(&c);
            for(int i=0;i<c.m_nodes.size();++i)
                  {
                  const btVector3         a=c.m_nodes[i]->m_x-c.m_com;
                  const btVector3&  b=c.m_framerefs[i];
                  m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
                  }
            PolarDecompose(m,r,s);
            c.m_framexform.setOrigin(c.m_com);
            c.m_framexform.setBasis(r);         
            /* Inertia              */ 
            #if 1/* Constant  */ 
            c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
            #else
                  #if 0/* Sphere    */ 
                  const btScalar    rk=(2*c.m_extents.length2())/(5*c.m_imass);
                  const btVector3   inertia(rk,rk,rk);
                  const btVector3   iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
                                                btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
                                                btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
                  
                  c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
                  #else/* Actual    */          
                  c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
                  for(int i=0;i<n;++i)
                        {
                        const btVector3   k=c.m_nodes[i]->m_x-c.m_com;
                        const btVector3         q=k*k;
                        const btScalar          m=1/c.m_nodes[i]->m_im;
                        c.m_invwi[0][0]   +=    m*(q[1]+q[2]);
                        c.m_invwi[1][1]   +=    m*(q[0]+q[2]);
                        c.m_invwi[2][2]   +=    m*(q[0]+q[1]);
                        c.m_invwi[0][1]   -=    m*k[0]*k[1];
                        c.m_invwi[0][2]   -=    m*k[0]*k[2];
                        c.m_invwi[1][2]   -=    m*k[1]*k[2];
                        }
                  c.m_invwi[1][0]=c.m_invwi[0][1];
                  c.m_invwi[2][0]=c.m_invwi[0][2];
                  c.m_invwi[2][1]=c.m_invwi[1][2];
                  c.m_invwi=c.m_invwi.inverse();
                  #endif
            #endif
            /* Velocities                 */ 
            c.m_lv=btVector3(0,0,0);
            c.m_av=btVector3(0,0,0);
            {
                  int i;

                  for(i=0;i<n;++i)
                  {
                        const btVector3   v=c.m_nodes[i]->m_v*c.m_masses[i];
                        c.m_lv      +=    v;
                        c.m_av      +=    cross(c.m_nodes[i]->m_x-c.m_com,v);
                  }
            }
            c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
            c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
            c.m_vimpulses[0]  =
            c.m_vimpulses[1]  = btVector3(0,0,0);
            c.m_dimpulses[0]  =
            c.m_dimpulses[1]  = btVector3(0,0,0);
            c.m_nvimpulses          = 0;
            c.m_ndimpulses          = 0;
            /* Matching                   */ 
            if(c.m_matching>0)
                  {
                  for(int j=0;j<c.m_nodes.size();++j)
                        {
                        Node&             n=*c.m_nodes[j];
                        const btVector3   x=c.m_framexform*c.m_framerefs[j];
                        n.m_x=Lerp(n.m_x,x,c.m_matching);
                        }
                  }
            /* Dbvt                             */ 
            if(c.m_collide)
                  {
                  btVector3   mi=c.m_nodes[0]->m_x;
                  btVector3   mx=mi;
                  for(int j=1;j<n;++j)
                        {
                        mi.setMin(c.m_nodes[j]->m_x);
                        mx.setMax(c.m_nodes[j]->m_x);
                        }                 
                  const ATTRIBUTE_ALIGNED16(btDbvtVolume)   bounds=btDbvtVolume::FromMM(mi,mx);
                  if(c.m_leaf)
                        m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
                        else
                        c.m_leaf=m_cdbvt.insert(bounds,&c);
                  }
            }
      }
}

//
void                          btSoftBody::cleanupClusters()
{
for(int i=0;i<m_joints.size();++i)
      {
      m_joints[i]->Terminate(m_sst.sdt);
      if(m_joints[i]->m_delete)
            {
            btAlignedFree(m_joints[i]);
            m_joints.remove(m_joints[i--]);
            }     
      }
}

//
void                          btSoftBody::prepareClusters(int iterations)
{
for(int i=0;i<m_joints.size();++i)
      {
      m_joints[i]->Prepare(m_sst.sdt,iterations);
      }
}


//
void                          btSoftBody::solveClusters(btScalar sor)
{
for(int i=0,ni=m_joints.size();i<ni;++i)
      {
      m_joints[i]->Solve(m_sst.sdt,sor);
      }
}

//
void                          btSoftBody::applyClusters(bool drift)
{
BT_PROFILE("ApplyClusters");
const btScalar                            f0=m_sst.sdt;
const btScalar                            f1=f0/2;
btAlignedObjectArray<btVector3>     deltas;
btAlignedObjectArray<btScalar>      weights;
deltas.resize(m_nodes.size(),btVector3(0,0,0));
weights.resize(m_nodes.size(),0);
int i;

if(drift)
      {
      for(i=0;i<m_clusters.size();++i)
            {
            Cluster&    c=*m_clusters[i];
            if(c.m_ndimpulses)
                  {
                  c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
                  c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
                  }
            }
      }
for(i=0;i<m_clusters.size();++i)
      {
      Cluster&    c=*m_clusters[i]; 
      if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
            {
            const btVector3         v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
            const btVector3         w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
            for(int j=0;j<c.m_nodes.size();++j)
                  {
                  const int               idx=int(c.m_nodes[j]-&m_nodes[0]);
                  const btVector3&  x=c.m_nodes[j]->m_x;
                  const btScalar          q=c.m_masses[j];
                  deltas[idx]       +=    (v+cross(w,x-c.m_com))*q;
                  weights[idx]      +=    q;
                  }
            }
      }
      for(i=0;i<deltas.size();++i)
      {
            if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
      }
}

//
void                          btSoftBody::dampClusters()
{
      int i;

for(i=0;i<m_clusters.size();++i)
      {
      Cluster&    c=*m_clusters[i]; 
      if(c.m_ndamping>0)
            {
            for(int j=0;j<c.m_nodes.size();++j)
                  {
                  Node&             n=*c.m_nodes[j];
                  if(n.m_im>0)
                        {
                        const btVector3   vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
                        n.m_v +=    c.m_ndamping*(vx-n.m_v);
                        }
                  }
            }
      }
}

//
void                    btSoftBody::Joint::Prepare(btScalar dt,int)
{
m_bodies[0].activate();
m_bodies[1].activate();
}

//
void                    btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
{
static const btScalar   maxdrift=4;
Joint::Prepare(dt,iterations);
m_rpos[0]         =     m_bodies[0].xform()*m_refs[0];
m_rpos[1]         =     m_bodies[1].xform()*m_refs[1];
m_drift                 =     Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
m_rpos[0]         -=    m_bodies[0].xform().getOrigin();
m_rpos[1]         -=    m_bodies[1].xform().getOrigin();
m_massmatrix      =     ImpulseMatrix(    m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
                                                      m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
if(m_split>0)
      {
      m_sdrift    =     m_massmatrix*(m_drift*m_split);
      m_drift           *=    1-m_split;
      }
m_drift     /=(btScalar)iterations;
}

//
void                    btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
const btVector3         vr=va-vb;
btSoftBody::Impulse     impulse;
impulse.m_asVelocity    =     1;
impulse.m_velocity            =     m_massmatrix*(m_drift+vr*m_cfm)*sor;
m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
m_bodies[1].applyImpulse( impulse,m_rpos[1]);
}

//
void                    btSoftBody::LJoint::Terminate(btScalar dt)
{
if(m_split>0)
      {
      m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
      m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
      }
}

//
void                    btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
{
static const btScalar   maxdrift=SIMD_PI/16;
m_icontrol->Prepare(this);
Joint::Prepare(dt,iterations);
m_axis[0]   =     m_bodies[0].xform().getBasis()*m_refs[0];
m_axis[1]   =     m_bodies[1].xform().getBasis()*m_refs[1];
m_drift           =     NormalizeAny(cross(m_axis[1],m_axis[0]));
m_drift           *=    btMin(maxdrift,btAcos(Clamp<btScalar>(dot(m_axis[0],m_axis[1]),-1,+1)));
m_drift           *=    m_erp/dt;
m_massmatrix=     AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
if(m_split>0)
      {
      m_sdrift    =     m_massmatrix*(m_drift*m_split);
      m_drift           *=    1-m_split;
      }
m_drift     /=(btScalar)iterations;
}

//
void                    btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3         va=m_bodies[0].angularVelocity();
const btVector3         vb=m_bodies[1].angularVelocity();
const btVector3         vr=va-vb;
const btScalar          sp=dot(vr,m_axis[0]);
const btVector3         vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
btSoftBody::Impulse     impulse;
impulse.m_asVelocity    =     1;
impulse.m_velocity            =     m_massmatrix*(m_drift+vc*m_cfm)*sor;
m_bodies[0].applyAImpulse(-impulse);
m_bodies[1].applyAImpulse( impulse);
}

//
void                    btSoftBody::AJoint::Terminate(btScalar dt)
{
if(m_split>0)
      {
      m_bodies[0].applyDAImpulse(-m_sdrift);
      m_bodies[1].applyDAImpulse( m_sdrift);
      }
}

//
void                    btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
{
Joint::Prepare(dt,iterations);
const bool  dodrift=(m_life==0);
m_delete=(++m_life)>m_maxlife;
if(dodrift)
      {
      m_drift=m_drift*m_erp/dt;
      if(m_split>0)
            {
            m_sdrift    =     m_massmatrix*(m_drift*m_split);
            m_drift           *=    1-m_split;
            }
      m_drift/=(btScalar)iterations;
      }
      else
      {
      m_drift=m_sdrift=btVector3(0,0,0);
      }
}

//
void                    btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
{
const btVector3         va=m_bodies[0].velocity(m_rpos[0]);
const btVector3         vb=m_bodies[1].velocity(m_rpos[1]);
const btVector3         vrel=va-vb;
const btScalar          rvac=dot(vrel,m_normal);
btSoftBody::Impulse     impulse;
impulse.m_asVelocity    =     1;
impulse.m_velocity            =     m_drift;
if(rvac<0)
      {
      const btVector3   iv=m_normal*rvac;
      const btVector3   fv=vrel-iv;
      impulse.m_velocity      +=    iv+fv*m_friction;
      }
impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
m_bodies[1].applyImpulse( impulse,m_rpos[1]);
}

//
void                    btSoftBody::CJoint::Terminate(btScalar dt)
{
if(m_split>0)
      {
      m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
      m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
      }
}

//
void                    btSoftBody::applyForces()
{
      BT_PROFILE("SoftBody applyForces");
      const btScalar                            dt=m_sst.sdt;
      const btScalar                            kLF=m_cfg.kLF;
      const btScalar                            kDG=m_cfg.kDG;
      const btScalar                            kPR=m_cfg.kPR;
      const btScalar                            kVC=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           &&
                                                                        (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
      const bool                                as_faero=   as_aero           &&
                                                                        (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            =     getVolume();
            ivolumetp   =     1/btFabs(volume)*kPR;
            dvolumetv   =     (m_pose.m_volume-volume)*kVC;
      }
      /* Per vertex forces                */ 
      int i,ni;

      for(i=0,ni=m_nodes.size();i<ni;++i)
      {
            btSoftBody::Node& n=m_nodes[i];
            if(n.m_im>0)
            {
                  if(use_medium)
                  {
                        EvaluateMedium(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(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(i=0,ni=m_faces.size();i<ni;++i)
      {
            btSoftBody::Face& f=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(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(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);
                        }
                  }
            }
      }
}

//
void                    btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
{
      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 Anchor&           a=psb->m_anchors[i];
            const btTransform&      t=a.m_body->getInterpolationWorldTransform();
            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);
      }
}

//
02443 void                    btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
{
      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 RContact&         c=psb->m_rcontacts[i];
            ///skip object that don't have collision response
            if (!psb->getWorldInfo()->m_dispatcher->needsResponse(psb,c.m_cti.m_body))
                  continue;

            const 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);
            }
      }
}

//
void                    btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
{
for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
      {
      const SContact&         c=psb->m_scontacts[i];
      const btVector3&  nr=c.m_normal;
      Node&                   n=*c.m_node;
      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());
      }
}

//
void                    btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
{
      for(int i=0,ni=psb->m_links.size();i<ni;++i)
      {                 
            Link& l=psb->m_links[i];
            if(l.m_c0>0)
            {
                  Node&             a=*l.m_n[0];
                  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;
                  const btScalar    t=k*a.m_im;
                  a.m_x-=del*(k*a.m_im);
                  b.m_x+=del*(k*b.m_im);
            }
      }
}

//
void                    btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
{
for(int i=0,ni=psb->m_links.size();i<ni;++i)
      {                 
      Link&             l=psb->m_links[i];
      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);
      }
}

//
btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
{
switch(solver)
      {
      case  ePSolver::Anchors:            return(&btSoftBody::PSolve_Anchors);
      case  ePSolver::Linear:       return(&btSoftBody::PSolve_Links);
      case  ePSolver::RContacts:    return(&btSoftBody::PSolve_RContacts);
      case  ePSolver::SContacts:    return(&btSoftBody::PSolve_SContacts);    
      }
return(0);
}

//
btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
{
switch(solver)
      {
      case  eVSolver::Linear:       return(&btSoftBody::VSolve_Links);
      }
return(0);
}

//
void              btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
{
switch(m_cfg.collisions&fCollision::RVSmask)
      {
      case  fCollision::SDF_RS:
            {
            btSoftColliders::CollideSDF_RS      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;
            ATTRIBUTE_ALIGNED16(btDbvtVolume)         volume;
            pco->getCollisionShape()->getAabb(  pco->getInterpolationWorldTransform(),
                                                                  mins,
                                                                  maxs);
            volume=btDbvtVolume::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;
      case  fCollision::CL_RS:
            {
            btSoftColliders::CollideCL_RS collider;
            collider.Process(this,btRigidBody::upcast(pco));
            }
      break;
      }
}

//
void              btSoftBody::defaultCollisionHandler(btSoftBody* psb)
{
const int cf=m_cfg.collisions&psb->m_cfg.collisions;
switch(cf&fCollision::SVSmask)
      {
      case  fCollision::CL_SS:
            {
            btSoftColliders::CollideCL_SS docollide;
            docollide.Process(this,psb);
            }
      break;
      case  fCollision::VF_SS:
            {
            btSoftColliders::CollideVF_SS 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;
      }
}

Generated by  Doxygen 1.6.0   Back to index