Blender V2.61 - r43446

btSoftBody.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose,
00008 including commercial applications, and to alter it and redistribute it freely,
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00016 
00017 #include "btSoftBodyInternals.h"
00018 #include "BulletSoftBody/btSoftBodySolvers.h"
00019 #include "btSoftBodyData.h"
00020 #include "LinearMath/btSerializer.h"
00021 
00022 //
00023 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count,  const btVector3* x,  const btScalar* m)
00024 :m_worldInfo(worldInfo),m_softBodySolver(0)
00025 {   
00026     /* Init     */ 
00027     initDefaults();
00028 
00029     /* Default material */ 
00030     Material*   pm=appendMaterial();
00031     pm->m_kLST  =   1;
00032     pm->m_kAST  =   1;
00033     pm->m_kVST  =   1;
00034     pm->m_flags =   fMaterial::Default;
00035 
00036     /* Nodes            */ 
00037     const btScalar      margin=getCollisionShape()->getMargin();
00038     m_nodes.resize(node_count);
00039     for(int i=0,ni=node_count;i<ni;++i)
00040     {   
00041         Node&   n=m_nodes[i];
00042         ZeroInitialize(n);
00043         n.m_x       =   x?*x++:btVector3(0,0,0);
00044         n.m_q       =   n.m_x;
00045         n.m_im      =   m?*m++:1;
00046         n.m_im      =   n.m_im>0?1/n.m_im:0;
00047         n.m_leaf    =   m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
00048         n.m_material=   pm;
00049     }
00050     updateBounds(); 
00051 
00052 }
00053 
00054 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
00055 :m_worldInfo(worldInfo)
00056 {
00057     initDefaults();
00058 }
00059 
00060 
00061 void    btSoftBody::initDefaults()
00062 {
00063     m_internalType      =   CO_SOFT_BODY;
00064     m_cfg.aeromodel     =   eAeroModel::V_Point;
00065     m_cfg.kVCF          =   1;
00066     m_cfg.kDG           =   0;
00067     m_cfg.kLF           =   0;
00068     m_cfg.kDP           =   0;
00069     m_cfg.kPR           =   0;
00070     m_cfg.kVC           =   0;
00071     m_cfg.kDF           =   (btScalar)0.2;
00072     m_cfg.kMT           =   0;
00073     m_cfg.kCHR          =   (btScalar)1.0;
00074     m_cfg.kKHR          =   (btScalar)0.1;
00075     m_cfg.kSHR          =   (btScalar)1.0;
00076     m_cfg.kAHR          =   (btScalar)0.7;
00077     m_cfg.kSRHR_CL      =   (btScalar)0.1;
00078     m_cfg.kSKHR_CL      =   (btScalar)1;
00079     m_cfg.kSSHR_CL      =   (btScalar)0.5;
00080     m_cfg.kSR_SPLT_CL   =   (btScalar)0.5;
00081     m_cfg.kSK_SPLT_CL   =   (btScalar)0.5;
00082     m_cfg.kSS_SPLT_CL   =   (btScalar)0.5;
00083     m_cfg.maxvolume     =   (btScalar)1;
00084     m_cfg.timescale     =   1;
00085     m_cfg.viterations   =   0;
00086     m_cfg.piterations   =   1;  
00087     m_cfg.diterations   =   0;
00088     m_cfg.citerations   =   4;
00089     m_cfg.collisions    =   fCollision::Default;
00090     m_pose.m_bvolume    =   false;
00091     m_pose.m_bframe     =   false;
00092     m_pose.m_volume     =   0;
00093     m_pose.m_com        =   btVector3(0,0,0);
00094     m_pose.m_rot.setIdentity();
00095     m_pose.m_scl.setIdentity();
00096     m_tag               =   0;
00097     m_timeacc           =   0;
00098     m_bUpdateRtCst      =   true;
00099     m_bounds[0]         =   btVector3(0,0,0);
00100     m_bounds[1]         =   btVector3(0,0,0);
00101     m_worldTransform.setIdentity();
00102     setSolver(eSolverPresets::Positions);
00103     
00104     /* Collision shape  */ 
00106     m_collisionShape = new btSoftBodyCollisionShape(this);
00107     m_collisionShape->setMargin(0.25);
00108     
00109     m_initialWorldTransform.setIdentity();
00110 
00111     m_windVelocity = btVector3(0,0,0);
00112 
00113 }
00114 
00115 //
00116 btSoftBody::~btSoftBody()
00117 {
00118     //for now, delete the internal shape
00119     delete m_collisionShape;    
00120     int i;
00121 
00122     releaseClusters();
00123     for(i=0;i<m_materials.size();++i) 
00124         btAlignedFree(m_materials[i]);
00125     for(i=0;i<m_joints.size();++i) 
00126         btAlignedFree(m_joints[i]);
00127 }
00128 
00129 //
00130 bool            btSoftBody::checkLink(int node0,int node1) const
00131 {
00132     return(checkLink(&m_nodes[node0],&m_nodes[node1]));
00133 }
00134 
00135 //
00136 bool            btSoftBody::checkLink(const Node* node0,const Node* node1) const
00137 {
00138     const Node* n[]={node0,node1};
00139     for(int i=0,ni=m_links.size();i<ni;++i)
00140     {
00141         const Link& l=m_links[i];
00142         if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
00143             (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
00144         {
00145             return(true);
00146         }
00147     }
00148     return(false);
00149 }
00150 
00151 //
00152 bool            btSoftBody::checkFace(int node0,int node1,int node2) const
00153 {
00154     const Node* n[]={   &m_nodes[node0],
00155         &m_nodes[node1],
00156         &m_nodes[node2]};
00157     for(int i=0,ni=m_faces.size();i<ni;++i)
00158     {
00159         const Face& f=m_faces[i];
00160         int         c=0;
00161         for(int j=0;j<3;++j)
00162         {
00163             if( (f.m_n[j]==n[0])||
00164                 (f.m_n[j]==n[1])||
00165                 (f.m_n[j]==n[2])) c|=1<<j; else break;
00166         }
00167         if(c==7) return(true);
00168     }
00169     return(false);
00170 }
00171 
00172 //
00173 btSoftBody::Material*       btSoftBody::appendMaterial()
00174 {
00175     Material*   pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
00176     if(m_materials.size()>0)
00177         *pm=*m_materials[0];
00178     else
00179         ZeroInitialize(*pm);
00180     m_materials.push_back(pm);
00181     return(pm);
00182 }
00183 
00184 //
00185 void            btSoftBody::appendNote( const char* text,
00186                                        const btVector3& o,
00187                                        const btVector4& c,
00188                                        Node* n0,
00189                                        Node* n1,
00190                                        Node* n2,
00191                                        Node* n3)
00192 {
00193     Note    n;
00194     ZeroInitialize(n);
00195     n.m_rank        =   0;
00196     n.m_text        =   text;
00197     n.m_offset      =   o;
00198     n.m_coords[0]   =   c.x();
00199     n.m_coords[1]   =   c.y();
00200     n.m_coords[2]   =   c.z();
00201     n.m_coords[3]   =   c.w();
00202     n.m_nodes[0]    =   n0;n.m_rank+=n0?1:0;
00203     n.m_nodes[1]    =   n1;n.m_rank+=n1?1:0;
00204     n.m_nodes[2]    =   n2;n.m_rank+=n2?1:0;
00205     n.m_nodes[3]    =   n3;n.m_rank+=n3?1:0;
00206     m_notes.push_back(n);
00207 }
00208 
00209 //
00210 void            btSoftBody::appendNote( const char* text,
00211                                        const btVector3& o,
00212                                        Node* feature)
00213 {
00214     appendNote(text,o,btVector4(1,0,0,0),feature);
00215 }
00216 
00217 //
00218 void            btSoftBody::appendNote( const char* text,
00219                                        const btVector3& o,
00220                                        Link* feature)
00221 {
00222     static const btScalar   w=1/(btScalar)2;
00223     appendNote(text,o,btVector4(w,w,0,0),   feature->m_n[0],
00224         feature->m_n[1]);
00225 }
00226 
00227 //
00228 void            btSoftBody::appendNote( const char* text,
00229                                        const btVector3& o,
00230                                        Face* feature)
00231 {
00232     static const btScalar   w=1/(btScalar)3;
00233     appendNote(text,o,btVector4(w,w,w,0),   feature->m_n[0],
00234         feature->m_n[1],
00235         feature->m_n[2]);
00236 }
00237 
00238 //
00239 void            btSoftBody::appendNode( const btVector3& x,btScalar m)
00240 {
00241     if(m_nodes.capacity()==m_nodes.size())
00242     {
00243         pointersToIndices();
00244         m_nodes.reserve(m_nodes.size()*2+1);
00245         indicesToPointers();
00246     }
00247     const btScalar  margin=getCollisionShape()->getMargin();
00248     m_nodes.push_back(Node());
00249     Node&           n=m_nodes[m_nodes.size()-1];
00250     ZeroInitialize(n);
00251     n.m_x           =   x;
00252     n.m_q           =   n.m_x;
00253     n.m_im          =   m>0?1/m:0;
00254     n.m_material    =   m_materials[0];
00255     n.m_leaf        =   m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
00256 }
00257 
00258 //
00259 void            btSoftBody::appendLink(int model,Material* mat)
00260 {
00261     Link    l;
00262     if(model>=0)
00263         l=m_links[model];
00264     else
00265     { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
00266     m_links.push_back(l);
00267 }
00268 
00269 //
00270 void            btSoftBody::appendLink( int node0,
00271                                        int node1,
00272                                        Material* mat,
00273                                        bool bcheckexist)
00274 {
00275     appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
00276 }
00277 
00278 //
00279 void            btSoftBody::appendLink( Node* node0,
00280                                        Node* node1,
00281                                        Material* mat,
00282                                        bool bcheckexist)
00283 {
00284     if((!bcheckexist)||(!checkLink(node0,node1)))
00285     {
00286         appendLink(-1,mat);
00287         Link&   l=m_links[m_links.size()-1];
00288         l.m_n[0]        =   node0;
00289         l.m_n[1]        =   node1;
00290         l.m_rl          =   (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
00291         m_bUpdateRtCst=true;
00292     }
00293 }
00294 
00295 //
00296 void            btSoftBody::appendFace(int model,Material* mat)
00297 {
00298     Face    f;
00299     if(model>=0)
00300     { f=m_faces[model]; }
00301     else
00302     { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
00303     m_faces.push_back(f);
00304 }
00305 
00306 //
00307 void            btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
00308 {
00309     if (node0==node1)
00310         return;
00311     if (node1==node2)
00312         return;
00313     if (node2==node0)
00314         return;
00315 
00316     appendFace(-1,mat);
00317     Face&   f=m_faces[m_faces.size()-1];
00318     btAssert(node0!=node1);
00319     btAssert(node1!=node2);
00320     btAssert(node2!=node0);
00321     f.m_n[0]    =   &m_nodes[node0];
00322     f.m_n[1]    =   &m_nodes[node1];
00323     f.m_n[2]    =   &m_nodes[node2];
00324     f.m_ra      =   AreaOf( f.m_n[0]->m_x,
00325         f.m_n[1]->m_x,
00326         f.m_n[2]->m_x); 
00327     m_bUpdateRtCst=true;
00328 }
00329 
00330 //
00331 void            btSoftBody::appendTetra(int model,Material* mat)
00332 {
00333 Tetra   t;
00334 if(model>=0)
00335     t=m_tetras[model];
00336     else
00337     { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
00338 m_tetras.push_back(t);
00339 }
00340 
00341 //
00342 void            btSoftBody::appendTetra(int node0,
00343                                         int node1,
00344                                         int node2,
00345                                         int node3,
00346                                         Material* mat)
00347 {
00348     appendTetra(-1,mat);
00349     Tetra&  t=m_tetras[m_tetras.size()-1];
00350     t.m_n[0]    =   &m_nodes[node0];
00351     t.m_n[1]    =   &m_nodes[node1];
00352     t.m_n[2]    =   &m_nodes[node2];
00353     t.m_n[3]    =   &m_nodes[node3];
00354     t.m_rv      =   VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
00355     m_bUpdateRtCst=true;
00356 }
00357 
00358 //
00359 
00360 void            btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies)
00361 {
00362     btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x;
00363     appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies);
00364 }
00365 
00366 //
00367 void            btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies)
00368 {
00369     if (disableCollisionBetweenLinkedBodies)
00370     {
00371         if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
00372         {
00373             m_collisionDisabledObjects.push_back(body);
00374         }
00375     }
00376 
00377     Anchor  a;
00378     a.m_node            =   &m_nodes[node];
00379     a.m_body            =   body;
00380     a.m_local           =   localPivot;
00381     a.m_node->m_battach =   1;
00382     m_anchors.push_back(a);
00383 }
00384 
00385 //
00386 void            btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
00387 {
00388     LJoint*     pj  =   new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
00389     pj->m_bodies[0] =   body0;
00390     pj->m_bodies[1] =   body1;
00391     pj->m_refs[0]   =   pj->m_bodies[0].xform().inverse()*specs.position;
00392     pj->m_refs[1]   =   pj->m_bodies[1].xform().inverse()*specs.position;
00393     pj->m_cfm       =   specs.cfm;
00394     pj->m_erp       =   specs.erp;
00395     pj->m_split     =   specs.split;
00396     m_joints.push_back(pj);
00397 }
00398 
00399 //
00400 void            btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
00401 {
00402     appendLinearJoint(specs,m_clusters[0],body);
00403 }
00404 
00405 //
00406 void            btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
00407 {
00408     appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
00409 }
00410 
00411 //
00412 void            btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
00413 {
00414     AJoint*     pj  =   new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
00415     pj->m_bodies[0] =   body0;
00416     pj->m_bodies[1] =   body1;
00417     pj->m_refs[0]   =   pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
00418     pj->m_refs[1]   =   pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
00419     pj->m_cfm       =   specs.cfm;
00420     pj->m_erp       =   specs.erp;
00421     pj->m_split     =   specs.split;
00422     pj->m_icontrol  =   specs.icontrol;
00423     m_joints.push_back(pj);
00424 }
00425 
00426 //
00427 void            btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
00428 {
00429     appendAngularJoint(specs,m_clusters[0],body);
00430 }
00431 
00432 //
00433 void            btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
00434 {
00435     appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
00436 }
00437 
00438 //
00439 void            btSoftBody::addForce(const btVector3& force)
00440 {
00441     for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
00442 }
00443 
00444 //
00445 void            btSoftBody::addForce(const btVector3& force,int node)
00446 {
00447     Node&   n=m_nodes[node];
00448     if(n.m_im>0)
00449     {
00450         n.m_f   +=  force;
00451     }
00452 }
00453 
00454 //
00455 void            btSoftBody::addVelocity(const btVector3& velocity)
00456 {
00457     for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
00458 }
00459 
00460 /* Set velocity for the entire body                                     */ 
00461 void                btSoftBody::setVelocity(    const btVector3& velocity)
00462 {
00463     for(int i=0,ni=m_nodes.size();i<ni;++i) 
00464     {
00465         Node&   n=m_nodes[i];
00466         if(n.m_im>0)
00467         {
00468             n.m_v   =   velocity;
00469         }
00470     }
00471 }
00472 
00473 
00474 //
00475 void            btSoftBody::addVelocity(const btVector3& velocity,int node)
00476 {
00477     Node&   n=m_nodes[node];
00478     if(n.m_im>0)
00479     {
00480         n.m_v   +=  velocity;
00481     }
00482 }
00483 
00484 //
00485 void            btSoftBody::setMass(int node,btScalar mass)
00486 {
00487     m_nodes[node].m_im=mass>0?1/mass:0;
00488     m_bUpdateRtCst=true;
00489 }
00490 
00491 //
00492 btScalar        btSoftBody::getMass(int node) const
00493 {
00494     return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
00495 }
00496 
00497 //
00498 btScalar        btSoftBody::getTotalMass() const
00499 {
00500     btScalar    mass=0;
00501     for(int i=0;i<m_nodes.size();++i)
00502     {
00503         mass+=getMass(i);
00504     }
00505     return(mass);
00506 }
00507 
00508 //
00509 void            btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
00510 {
00511     int i;
00512 
00513     if(fromfaces)
00514     {
00515 
00516         for(i=0;i<m_nodes.size();++i)
00517         {
00518             m_nodes[i].m_im=0;
00519         }
00520         for(i=0;i<m_faces.size();++i)
00521         {
00522             const Face&     f=m_faces[i];
00523             const btScalar  twicearea=AreaOf(   f.m_n[0]->m_x,
00524                 f.m_n[1]->m_x,
00525                 f.m_n[2]->m_x);
00526             for(int j=0;j<3;++j)
00527             {
00528                 f.m_n[j]->m_im+=twicearea;
00529             }
00530         }
00531         for( i=0;i<m_nodes.size();++i)
00532         {
00533             m_nodes[i].m_im=1/m_nodes[i].m_im;
00534         }
00535     }
00536     const btScalar  tm=getTotalMass();
00537     const btScalar  itm=1/tm;
00538     for( i=0;i<m_nodes.size();++i)
00539     {
00540         m_nodes[i].m_im/=itm*mass;
00541     }
00542     m_bUpdateRtCst=true;
00543 }
00544 
00545 //
00546 void            btSoftBody::setTotalDensity(btScalar density)
00547 {
00548     setTotalMass(getVolume()*density,true);
00549 }
00550 
00551 //
00552 void            btSoftBody::setVolumeMass(btScalar mass)
00553 {
00554 btAlignedObjectArray<btScalar>  ranks;
00555 ranks.resize(m_nodes.size(),0);
00556 int i;
00557 
00558 for(i=0;i<m_nodes.size();++i)
00559     {
00560     m_nodes[i].m_im=0;
00561     }
00562 for(i=0;i<m_tetras.size();++i)
00563     {
00564     const Tetra& t=m_tetras[i];
00565     for(int j=0;j<4;++j)
00566         {
00567         t.m_n[j]->m_im+=btFabs(t.m_rv);
00568         ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
00569         }
00570     }
00571 for( i=0;i<m_nodes.size();++i)
00572     {
00573     if(m_nodes[i].m_im>0)
00574         {
00575         m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
00576         }
00577     }
00578 setTotalMass(mass,false);
00579 }
00580 
00581 //
00582 void            btSoftBody::setVolumeDensity(btScalar density)
00583 {
00584 btScalar    volume=0;
00585 for(int i=0;i<m_tetras.size();++i)
00586     {
00587     const Tetra& t=m_tetras[i];
00588     for(int j=0;j<4;++j)
00589         {
00590         volume+=btFabs(t.m_rv);
00591         }
00592     }
00593 setVolumeMass(volume*density/6);
00594 }
00595 
00596 //
00597 void            btSoftBody::transform(const btTransform& trs)
00598 {
00599     const btScalar  margin=getCollisionShape()->getMargin();
00600     ATTRIBUTE_ALIGNED16(btDbvtVolume)   vol;
00601     
00602     for(int i=0,ni=m_nodes.size();i<ni;++i)
00603     {
00604         Node&   n=m_nodes[i];
00605         n.m_x=trs*n.m_x;
00606         n.m_q=trs*n.m_q;
00607         n.m_n=trs.getBasis()*n.m_n;
00608         vol = btDbvtVolume::FromCR(n.m_x,margin);
00609         
00610         m_ndbvt.update(n.m_leaf,vol);
00611     }
00612     updateNormals();
00613     updateBounds();
00614     updateConstants();
00615     m_initialWorldTransform = trs;
00616 }
00617 
00618 //
00619 void            btSoftBody::translate(const btVector3& trs)
00620 {
00621     btTransform t;
00622     t.setIdentity();
00623     t.setOrigin(trs);
00624     transform(t);
00625 }
00626 
00627 //
00628 void            btSoftBody::rotate( const btQuaternion& rot)
00629 {
00630     btTransform t;
00631     t.setIdentity();
00632     t.setRotation(rot);
00633     transform(t);
00634 }
00635 
00636 //
00637 void            btSoftBody::scale(const btVector3& scl)
00638 {
00639 
00640     const btScalar  margin=getCollisionShape()->getMargin();
00641     ATTRIBUTE_ALIGNED16(btDbvtVolume)   vol;
00642     
00643     for(int i=0,ni=m_nodes.size();i<ni;++i)
00644     {
00645         Node&   n=m_nodes[i];
00646         n.m_x*=scl;
00647         n.m_q*=scl;
00648         vol = btDbvtVolume::FromCR(n.m_x,margin);
00649         m_ndbvt.update(n.m_leaf,vol);
00650     }
00651     updateNormals();
00652     updateBounds();
00653     updateConstants();
00654 }
00655 
00656 //
00657 void            btSoftBody::setPose(bool bvolume,bool bframe)
00658 {
00659     m_pose.m_bvolume    =   bvolume;
00660     m_pose.m_bframe     =   bframe;
00661     int i,ni;
00662 
00663     /* Weights      */ 
00664     const btScalar  omass=getTotalMass();
00665     const btScalar  kmass=omass*m_nodes.size()*1000;
00666     btScalar        tmass=omass;
00667     m_pose.m_wgh.resize(m_nodes.size());
00668     for(i=0,ni=m_nodes.size();i<ni;++i)
00669     {
00670         if(m_nodes[i].m_im<=0) tmass+=kmass;
00671     }
00672     for( i=0,ni=m_nodes.size();i<ni;++i)
00673     {
00674         Node&   n=m_nodes[i];
00675         m_pose.m_wgh[i]=    n.m_im>0                    ?
00676             1/(m_nodes[i].m_im*tmass)   :
00677         kmass/tmass;
00678     }
00679     /* Pos      */ 
00680     const btVector3 com=evaluateCom();
00681     m_pose.m_pos.resize(m_nodes.size());
00682     for( i=0,ni=m_nodes.size();i<ni;++i)
00683     {
00684         m_pose.m_pos[i]=m_nodes[i].m_x-com;
00685     }
00686     m_pose.m_volume =   bvolume?getVolume():0;
00687     m_pose.m_com    =   com;
00688     m_pose.m_rot.setIdentity();
00689     m_pose.m_scl.setIdentity();
00690     /* Aqq      */ 
00691     m_pose.m_aqq[0] =
00692         m_pose.m_aqq[1] =
00693         m_pose.m_aqq[2] =   btVector3(0,0,0);
00694     for( i=0,ni=m_nodes.size();i<ni;++i)
00695     {
00696         const btVector3&    q=m_pose.m_pos[i];
00697         const btVector3     mq=m_pose.m_wgh[i]*q;
00698         m_pose.m_aqq[0]+=mq.x()*q;
00699         m_pose.m_aqq[1]+=mq.y()*q;
00700         m_pose.m_aqq[2]+=mq.z()*q;
00701     }
00702     m_pose.m_aqq=m_pose.m_aqq.inverse();
00703     updateConstants();
00704 }
00705 
00706 //
00707 btScalar        btSoftBody::getVolume() const
00708 {
00709     btScalar    vol=0;
00710     if(m_nodes.size()>0)
00711     {
00712         int i,ni;
00713 
00714         const btVector3 org=m_nodes[0].m_x;
00715         for(i=0,ni=m_faces.size();i<ni;++i)
00716         {
00717             const Face& f=m_faces[i];
00718             vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
00719         }
00720         vol/=(btScalar)6;
00721     }
00722     return(vol);
00723 }
00724 
00725 //
00726 int             btSoftBody::clusterCount() const
00727 {
00728     return(m_clusters.size());
00729 }
00730 
00731 //
00732 btVector3       btSoftBody::clusterCom(const Cluster* cluster)
00733 {
00734     btVector3       com(0,0,0);
00735     for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
00736     {
00737         com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
00738     }
00739     return(com*cluster->m_imass);
00740 }
00741 
00742 //
00743 btVector3       btSoftBody::clusterCom(int cluster) const
00744 {
00745     return(clusterCom(m_clusters[cluster]));
00746 }
00747 
00748 //
00749 btVector3       btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
00750 {
00751     return(cluster->m_lv+btCross(cluster->m_av,rpos));
00752 }
00753 
00754 //
00755 void            btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
00756 {
00757     const btVector3 li=cluster->m_imass*impulse;
00758     const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
00759     cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
00760     cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
00761     cluster->m_nvimpulses++;
00762 }
00763 
00764 //
00765 void            btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
00766 {
00767     const btVector3 li=cluster->m_imass*impulse;
00768     const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
00769     cluster->m_dimpulses[0]+=li;
00770     cluster->m_dimpulses[1]+=ai;
00771     cluster->m_ndimpulses++;
00772 }
00773 
00774 //
00775 void            btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
00776 {
00777     if(impulse.m_asVelocity)    clusterVImpulse(cluster,rpos,impulse.m_velocity);
00778     if(impulse.m_asDrift)       clusterDImpulse(cluster,rpos,impulse.m_drift);
00779 }
00780 
00781 //
00782 void            btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
00783 {
00784     const btVector3 ai=cluster->m_invwi*impulse;
00785     cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
00786     cluster->m_nvimpulses++;
00787 }
00788 
00789 //
00790 void            btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
00791 {
00792     const btVector3 ai=cluster->m_invwi*impulse;
00793     cluster->m_dimpulses[1]+=ai;
00794     cluster->m_ndimpulses++;
00795 }
00796 
00797 //
00798 void            btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
00799 {
00800     if(impulse.m_asVelocity)    clusterVAImpulse(cluster,impulse.m_velocity);
00801     if(impulse.m_asDrift)       clusterDAImpulse(cluster,impulse.m_drift);
00802 }
00803 
00804 //
00805 void            btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
00806 {
00807     cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
00808     cluster->m_ndimpulses++;
00809 }
00810 
00811 struct NodeLinks
00812 {
00813     btAlignedObjectArray<int> m_links;
00814 };
00815 
00816 
00817 
00818 //
00819 int             btSoftBody::generateBendingConstraints(int distance,Material* mat)
00820 {
00821     int i,j;
00822 
00823     if(distance>1)
00824     {
00825         /* Build graph  */ 
00826         const int       n=m_nodes.size();
00827         const unsigned  inf=(~(unsigned)0)>>1;
00828         unsigned*       adj=new unsigned[n*n];
00829         
00830 
00831 #define IDX(_x_,_y_)    ((_y_)*n+(_x_))
00832         for(j=0;j<n;++j)
00833         {
00834             for(i=0;i<n;++i)
00835             {
00836                 if(i!=j)
00837                 {
00838                     adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
00839                 }
00840                 else
00841                 {
00842                     adj[IDX(i,j)]=adj[IDX(j,i)]=0;
00843                 }
00844             }
00845         }
00846         for( i=0;i<m_links.size();++i)
00847         {
00848             const int   ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
00849             const int   ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
00850             adj[IDX(ia,ib)]=1;
00851             adj[IDX(ib,ia)]=1;
00852         }
00853 
00854 
00855         //special optimized case for distance == 2
00856         if (distance == 2)
00857         {
00858 
00859             btAlignedObjectArray<NodeLinks> nodeLinks;
00860 
00861 
00862             /* Build node links */
00863             nodeLinks.resize(m_nodes.size());
00864 
00865             for( i=0;i<m_links.size();++i)
00866             {
00867                 const int   ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
00868                 const int   ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
00869                 if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
00870                     nodeLinks[ia].m_links.push_back(ib);
00871 
00872                 if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
00873                     nodeLinks[ib].m_links.push_back(ia);
00874             }
00875             for (int ii=0;ii<nodeLinks.size();ii++)
00876             {
00877                 int i=ii;
00878 
00879                 for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
00880                 {
00881                     int k = nodeLinks[ii].m_links[jj];
00882                     for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
00883                     {
00884                         int j = nodeLinks[k].m_links[kk];
00885                         if (i!=j)
00886                         {
00887                             const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
00888                             btAssert(sum==2);
00889                             if(adj[IDX(i,j)]>sum)
00890                             {
00891                                 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
00892                             }
00893                         }
00894 
00895                     }
00896                 }
00897             }
00898         } else
00899         {
00901             for(int k=0;k<n;++k)
00902             {
00903                 for(j=0;j<n;++j)
00904                 {
00905                     for(i=j+1;i<n;++i)
00906                     {
00907                         const unsigned  sum=adj[IDX(i,k)]+adj[IDX(k,j)];
00908                         if(adj[IDX(i,j)]>sum)
00909                         {
00910                             adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
00911                         }
00912                     }
00913                 }
00914             }
00915         }
00916 
00917 
00918         /* Build links  */ 
00919         int nlinks=0;
00920         for(j=0;j<n;++j)
00921         {
00922             for(i=j+1;i<n;++i)
00923             {
00924                 if(adj[IDX(i,j)]==(unsigned)distance)
00925                 {
00926                     appendLink(i,j,mat);
00927                     m_links[m_links.size()-1].m_bbending=1;
00928                     ++nlinks;
00929                 }
00930             }
00931         }
00932         delete[] adj;       
00933         return(nlinks);
00934     }
00935     return(0);
00936 }
00937 
00938 //
00939 void            btSoftBody::randomizeConstraints()
00940 {
00941     unsigned long   seed=243703;
00942 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
00943     int i,ni;
00944 
00945     for(i=0,ni=m_links.size();i<ni;++i)
00946     {
00947         btSwap(m_links[i],m_links[NEXTRAND%ni]);
00948     }
00949     for(i=0,ni=m_faces.size();i<ni;++i)
00950     {
00951         btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
00952     }
00953 #undef NEXTRAND
00954 }
00955 
00956 //
00957 void            btSoftBody::releaseCluster(int index)
00958 {
00959     Cluster*    c=m_clusters[index];
00960     if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
00961     c->~Cluster();
00962     btAlignedFree(c);
00963     m_clusters.remove(c);
00964 }
00965 
00966 //
00967 void            btSoftBody::releaseClusters()
00968 {
00969     while(m_clusters.size()>0) releaseCluster(0);
00970 }
00971 
00972 //
00973 int             btSoftBody::generateClusters(int k,int maxiterations)
00974 {
00975     int i;
00976     releaseClusters();
00977     m_clusters.resize(btMin(k,m_nodes.size()));
00978     for(i=0;i<m_clusters.size();++i)
00979     {
00980         m_clusters[i]           =   new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
00981         m_clusters[i]->m_collide=   true;
00982     }
00983     k=m_clusters.size();
00984     if(k>0)
00985     {
00986         /* Initialize       */ 
00987         btAlignedObjectArray<btVector3> centers;
00988         btVector3                       cog(0,0,0);
00989         int                             i;
00990         for(i=0;i<m_nodes.size();++i)
00991         {
00992             cog+=m_nodes[i].m_x;
00993             m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
00994         }
00995         cog/=(btScalar)m_nodes.size();
00996         centers.resize(k,cog);
00997         /* Iterate          */ 
00998         const btScalar  slope=16;
00999         bool            changed;
01000         int             iterations=0;
01001         do  {
01002             const btScalar  w=2-btMin<btScalar>(1,iterations/slope);
01003             changed=false;
01004             iterations++;   
01005             int i;
01006 
01007             for(i=0;i<k;++i)
01008             {
01009                 btVector3   c(0,0,0);
01010                 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
01011                 {
01012                     c+=m_clusters[i]->m_nodes[j]->m_x;
01013                 }
01014                 if(m_clusters[i]->m_nodes.size())
01015                 {
01016                     c           /=  (btScalar)m_clusters[i]->m_nodes.size();
01017                     c           =   centers[i]+(c-centers[i])*w;
01018                     changed     |=  ((c-centers[i]).length2()>SIMD_EPSILON);
01019                     centers[i]  =   c;
01020                     m_clusters[i]->m_nodes.resize(0);
01021                 }           
01022             }
01023             for(i=0;i<m_nodes.size();++i)
01024             {
01025                 const btVector3 nx=m_nodes[i].m_x;
01026                 int             kbest=0;
01027                 btScalar        kdist=ClusterMetric(centers[0],nx);
01028                 for(int j=1;j<k;++j)
01029                 {
01030                     const btScalar  d=ClusterMetric(centers[j],nx);
01031                     if(d<kdist)
01032                     {
01033                         kbest=j;
01034                         kdist=d;
01035                     }
01036                 }
01037                 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
01038             }       
01039         } while(changed&&(iterations<maxiterations));
01040         /* Merge        */ 
01041         btAlignedObjectArray<int>   cids;
01042         cids.resize(m_nodes.size(),-1);
01043         for(i=0;i<m_clusters.size();++i)
01044         {
01045             for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
01046             {
01047                 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
01048             }
01049         }
01050         for(i=0;i<m_faces.size();++i)
01051         {
01052             const int idx[]={   int(m_faces[i].m_n[0]-&m_nodes[0]),
01053                 int(m_faces[i].m_n[1]-&m_nodes[0]),
01054                 int(m_faces[i].m_n[2]-&m_nodes[0])};
01055             for(int j=0;j<3;++j)
01056             {
01057                 const int cid=cids[idx[j]];
01058                 for(int q=1;q<3;++q)
01059                 {
01060                     const int kid=idx[(j+q)%3];
01061                     if(cids[kid]!=cid)
01062                     {
01063                         if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
01064                         {
01065                             m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
01066                         }
01067                     }
01068                 }
01069             }
01070         }
01071         /* Master       */ 
01072         if(m_clusters.size()>1)
01073         {
01074             Cluster*    pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01075             pmaster->m_collide  =   false;
01076             pmaster->m_nodes.reserve(m_nodes.size());
01077             for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
01078             m_clusters.push_back(pmaster);
01079             btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
01080         }
01081         /* Terminate    */ 
01082         for(i=0;i<m_clusters.size();++i)
01083         {
01084             if(m_clusters[i]->m_nodes.size()==0)
01085             {
01086                 releaseCluster(i--);
01087             }
01088         }
01089     } else
01090     {
01091         //create a cluster for each tetrahedron (if tetrahedra exist) or each face
01092         if (m_tetras.size())
01093         {
01094             m_clusters.resize(m_tetras.size());
01095             for(i=0;i<m_clusters.size();++i)
01096             {
01097                 m_clusters[i]           =   new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01098                 m_clusters[i]->m_collide=   true;
01099             }
01100             for (i=0;i<m_tetras.size();i++)
01101             {
01102                 for (int j=0;j<4;j++)
01103                 {
01104                     m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
01105                 }
01106             }
01107 
01108         } else
01109         {
01110             m_clusters.resize(m_faces.size());
01111             for(i=0;i<m_clusters.size();++i)
01112             {
01113                 m_clusters[i]           =   new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01114                 m_clusters[i]->m_collide=   true;
01115             }
01116 
01117             for(i=0;i<m_faces.size();++i)
01118             {
01119                 for(int j=0;j<3;++j)
01120                 {
01121                     m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
01122                 }
01123             }
01124         }
01125     }
01126 
01127     if (m_clusters.size())
01128     {
01129         initializeClusters();
01130         updateClusters();
01131 
01132 
01133         //for self-collision
01134         m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
01135         {
01136             for (int c0=0;c0<m_clusters.size();c0++)
01137             {
01138                 m_clusters[c0]->m_clusterIndex=c0;
01139                 for (int c1=0;c1<m_clusters.size();c1++)
01140                 {
01141                     
01142                     bool connected=false;
01143                     Cluster* cla = m_clusters[c0];
01144                     Cluster* clb = m_clusters[c1];
01145                     for (int i=0;!connected&&i<cla->m_nodes.size();i++)
01146                     {
01147                         for (int j=0;j<clb->m_nodes.size();j++)
01148                         {
01149                             if (cla->m_nodes[i] == clb->m_nodes[j])
01150                             {
01151                                 connected=true;
01152                                 break;
01153                             }
01154                         }
01155                     }
01156                     m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
01157                 }
01158             }
01159         }
01160     }
01161 
01162     return(m_clusters.size());
01163 }
01164 
01165 //
01166 void            btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
01167 {
01168     const Node*         nbase = &m_nodes[0];
01169     int                 ncount = m_nodes.size();
01170     btSymMatrix<int>    edges(ncount,-2);
01171     int                 newnodes=0;
01172     int i,j,k,ni;
01173 
01174     /* Filter out       */ 
01175     for(i=0;i<m_links.size();++i)
01176     {
01177         Link&   l=m_links[i];
01178         if(l.m_bbending)
01179         {
01180             if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
01181             {
01182                 btSwap(m_links[i],m_links[m_links.size()-1]);
01183                 m_links.pop_back();--i;
01184             }
01185         }   
01186     }
01187     /* Fill edges       */ 
01188     for(i=0;i<m_links.size();++i)
01189     {
01190         Link&   l=m_links[i];
01191         edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
01192     }
01193     for(i=0;i<m_faces.size();++i)
01194     {   
01195         Face&   f=m_faces[i];
01196         edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
01197         edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
01198         edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
01199     }
01200     /* Intersect        */ 
01201     for(i=0;i<ncount;++i)
01202     {
01203         for(j=i+1;j<ncount;++j)
01204         {
01205             if(edges(i,j)==-1)
01206             {
01207                 Node&           a=m_nodes[i];
01208                 Node&           b=m_nodes[j];
01209                 const btScalar  t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
01210                 if(t>0)
01211                 {
01212                     const btVector3 x=Lerp(a.m_x,b.m_x,t);
01213                     const btVector3 v=Lerp(a.m_v,b.m_v,t);
01214                     btScalar        m=0;
01215                     if(a.m_im>0)
01216                     {
01217                         if(b.m_im>0)
01218                         {
01219                             const btScalar  ma=1/a.m_im;
01220                             const btScalar  mb=1/b.m_im;
01221                             const btScalar  mc=Lerp(ma,mb,t);
01222                             const btScalar  f=(ma+mb)/(ma+mb+mc);
01223                             a.m_im=1/(ma*f);
01224                             b.m_im=1/(mb*f);
01225                             m=mc*f;
01226                         }
01227                         else
01228                         { a.m_im/=0.5;m=1/a.m_im; }
01229                     }
01230                     else
01231                     {
01232                         if(b.m_im>0)
01233                         { b.m_im/=0.5;m=1/b.m_im; }
01234                         else
01235                             m=0;
01236                     }
01237                     appendNode(x,m);
01238                     edges(i,j)=m_nodes.size()-1;
01239                     m_nodes[edges(i,j)].m_v=v;
01240                     ++newnodes;
01241                 }
01242             }
01243         }
01244     }
01245     nbase=&m_nodes[0];
01246     /* Refine links     */ 
01247     for(i=0,ni=m_links.size();i<ni;++i)
01248     {
01249         Link&       feat=m_links[i];
01250         const int   idx[]={ int(feat.m_n[0]-nbase),
01251             int(feat.m_n[1]-nbase)};
01252         if((idx[0]<ncount)&&(idx[1]<ncount))
01253         {
01254             const int ni=edges(idx[0],idx[1]);
01255             if(ni>0)
01256             {
01257                 appendLink(i);
01258                 Link*       pft[]={ &m_links[i],
01259                     &m_links[m_links.size()-1]};            
01260                 pft[0]->m_n[0]=&m_nodes[idx[0]];
01261                 pft[0]->m_n[1]=&m_nodes[ni];
01262                 pft[1]->m_n[0]=&m_nodes[ni];
01263                 pft[1]->m_n[1]=&m_nodes[idx[1]];
01264             }
01265         }
01266     }
01267     /* Refine faces     */ 
01268     for(i=0;i<m_faces.size();++i)
01269     {
01270         const Face& feat=m_faces[i];
01271         const int   idx[]={ int(feat.m_n[0]-nbase),
01272             int(feat.m_n[1]-nbase),
01273             int(feat.m_n[2]-nbase)};
01274         for(j=2,k=0;k<3;j=k++)
01275         {
01276             if((idx[j]<ncount)&&(idx[k]<ncount))
01277             {
01278                 const int ni=edges(idx[j],idx[k]);
01279                 if(ni>0)
01280                 {
01281                     appendFace(i);
01282                     const int   l=(k+1)%3;
01283                     Face*       pft[]={ &m_faces[i],
01284                         &m_faces[m_faces.size()-1]};
01285                     pft[0]->m_n[0]=&m_nodes[idx[l]];
01286                     pft[0]->m_n[1]=&m_nodes[idx[j]];
01287                     pft[0]->m_n[2]=&m_nodes[ni];
01288                     pft[1]->m_n[0]=&m_nodes[ni];
01289                     pft[1]->m_n[1]=&m_nodes[idx[k]];
01290                     pft[1]->m_n[2]=&m_nodes[idx[l]];
01291                     appendLink(ni,idx[l],pft[0]->m_material);
01292                     --i;break;
01293                 }
01294             }
01295         }
01296     }
01297     /* Cut              */ 
01298     if(cut)
01299     {   
01300         btAlignedObjectArray<int>   cnodes;
01301         const int                   pcount=ncount;
01302         int                         i;
01303         ncount=m_nodes.size();
01304         cnodes.resize(ncount,0);
01305         /* Nodes        */ 
01306         for(i=0;i<ncount;++i)
01307         {
01308             const btVector3 x=m_nodes[i].m_x;
01309             if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
01310             {
01311                 const btVector3 v=m_nodes[i].m_v;
01312                 btScalar        m=getMass(i);
01313                 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
01314                 appendNode(x,m);
01315                 cnodes[i]=m_nodes.size()-1;
01316                 m_nodes[cnodes[i]].m_v=v;
01317             }
01318         }
01319         nbase=&m_nodes[0];
01320         /* Links        */ 
01321         for(i=0,ni=m_links.size();i<ni;++i)
01322         {
01323             const int       id[]={  int(m_links[i].m_n[0]-nbase),
01324                 int(m_links[i].m_n[1]-nbase)};
01325             int             todetach=0;
01326             if(cnodes[id[0]]&&cnodes[id[1]])
01327             {
01328                 appendLink(i);
01329                 todetach=m_links.size()-1;
01330             }
01331             else
01332             {
01333                 if((    (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
01334                     (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
01335                     todetach=i;
01336             }
01337             if(todetach)
01338             {
01339                 Link&   l=m_links[todetach];
01340                 for(int j=0;j<2;++j)
01341                 {
01342                     int cn=cnodes[int(l.m_n[j]-nbase)];
01343                     if(cn) l.m_n[j]=&m_nodes[cn];
01344                 }           
01345             }
01346         }
01347         /* Faces        */ 
01348         for(i=0,ni=m_faces.size();i<ni;++i)
01349         {
01350             Node**          n=  m_faces[i].m_n;
01351             if( (ifn->Eval(n[0]->m_x)<accurary)&&
01352                 (ifn->Eval(n[1]->m_x)<accurary)&&
01353                 (ifn->Eval(n[2]->m_x)<accurary))
01354             {
01355                 for(int j=0;j<3;++j)
01356                 {
01357                     int cn=cnodes[int(n[j]-nbase)];
01358                     if(cn) n[j]=&m_nodes[cn];
01359                 }
01360             }
01361         }
01362         /* Clean orphans    */ 
01363         int                         nnodes=m_nodes.size();
01364         btAlignedObjectArray<int>   ranks;
01365         btAlignedObjectArray<int>   todelete;
01366         ranks.resize(nnodes,0);
01367         for(i=0,ni=m_links.size();i<ni;++i)
01368         {
01369             for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
01370         }
01371         for(i=0,ni=m_faces.size();i<ni;++i)
01372         {
01373             for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
01374         }
01375         for(i=0;i<m_links.size();++i)
01376         {
01377             const int   id[]={  int(m_links[i].m_n[0]-nbase),
01378                 int(m_links[i].m_n[1]-nbase)};
01379             const bool  sg[]={  ranks[id[0]]==1,
01380                 ranks[id[1]]==1};
01381             if(sg[0]||sg[1])
01382             {
01383                 --ranks[id[0]];
01384                 --ranks[id[1]];
01385                 btSwap(m_links[i],m_links[m_links.size()-1]);
01386                 m_links.pop_back();--i;
01387             }
01388         }
01389 #if 0   
01390         for(i=nnodes-1;i>=0;--i)
01391         {
01392             if(!ranks[i]) todelete.push_back(i);
01393         }   
01394         if(todelete.size())
01395         {       
01396             btAlignedObjectArray<int>&  map=ranks;
01397             for(int i=0;i<nnodes;++i) map[i]=i;
01398             PointersToIndices(this);
01399             for(int i=0,ni=todelete.size();i<ni;++i)
01400             {
01401                 int     j=todelete[i];
01402                 int&    a=map[j];
01403                 int&    b=map[--nnodes];
01404                 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
01405                 btSwap(m_nodes[a],m_nodes[b]);
01406                 j=a;a=b;b=j;            
01407             }
01408             IndicesToPointers(this,&map[0]);
01409             m_nodes.resize(nnodes);
01410         }
01411 #endif
01412     }
01413     m_bUpdateRtCst=true;
01414 }
01415 
01416 //
01417 bool            btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
01418 {
01419     return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
01420 }
01421 
01422 //
01423 bool            btSoftBody::cutLink(int node0,int node1,btScalar position)
01424 {
01425     bool            done=false;
01426     int i,ni;
01427     const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
01428     const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
01429     const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
01430     const btScalar  m=1;
01431     appendNode(x,m);
01432     appendNode(x,m);
01433     Node*           pa=&m_nodes[node0];
01434     Node*           pb=&m_nodes[node1];
01435     Node*           pn[2]={ &m_nodes[m_nodes.size()-2],
01436         &m_nodes[m_nodes.size()-1]};
01437     pn[0]->m_v=v;
01438     pn[1]->m_v=v;
01439     for(i=0,ni=m_links.size();i<ni;++i)
01440     {
01441         const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
01442         if(mtch!=-1)
01443         {
01444             appendLink(i);
01445             Link*   pft[]={&m_links[i],&m_links[m_links.size()-1]};
01446             pft[0]->m_n[1]=pn[mtch];
01447             pft[1]->m_n[0]=pn[1-mtch];
01448             done=true;
01449         }
01450     }
01451     for(i=0,ni=m_faces.size();i<ni;++i)
01452     {
01453         for(int k=2,l=0;l<3;k=l++)
01454         {
01455             const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
01456             if(mtch!=-1)
01457             {
01458                 appendFace(i);
01459                 Face*   pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
01460                 pft[0]->m_n[l]=pn[mtch];
01461                 pft[1]->m_n[k]=pn[1-mtch];
01462                 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
01463                 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
01464             }
01465         }
01466     }
01467     if(!done)
01468     {
01469         m_ndbvt.remove(pn[0]->m_leaf);
01470         m_ndbvt.remove(pn[1]->m_leaf);
01471         m_nodes.pop_back();
01472         m_nodes.pop_back();
01473     }
01474     return(done);
01475 }
01476 
01477 //
01478 bool            btSoftBody::rayTest(const btVector3& rayFrom,
01479                                     const btVector3& rayTo,
01480                                     sRayCast& results)
01481 {
01482     if(m_faces.size()&&m_fdbvt.empty()) 
01483         initializeFaceTree();
01484 
01485     results.body    =   this;
01486     results.fraction = 1.f;
01487     results.feature =   eFeature::None;
01488     results.index   =   -1;
01489 
01490     return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
01491 }
01492 
01493 //
01494 void            btSoftBody::setSolver(eSolverPresets::_ preset)
01495 {
01496     m_cfg.m_vsequence.clear();
01497     m_cfg.m_psequence.clear();
01498     m_cfg.m_dsequence.clear();
01499     switch(preset)
01500     {
01501     case    eSolverPresets::Positions:
01502         m_cfg.m_psequence.push_back(ePSolver::Anchors);
01503         m_cfg.m_psequence.push_back(ePSolver::RContacts);
01504         m_cfg.m_psequence.push_back(ePSolver::SContacts);
01505         m_cfg.m_psequence.push_back(ePSolver::Linear);  
01506         break;  
01507     case    eSolverPresets::Velocities:
01508         m_cfg.m_vsequence.push_back(eVSolver::Linear);
01509 
01510         m_cfg.m_psequence.push_back(ePSolver::Anchors);
01511         m_cfg.m_psequence.push_back(ePSolver::RContacts);
01512         m_cfg.m_psequence.push_back(ePSolver::SContacts);
01513 
01514         m_cfg.m_dsequence.push_back(ePSolver::Linear);
01515         break;
01516     }
01517 }
01518 
01519 //
01520 void            btSoftBody::predictMotion(btScalar dt)
01521 {
01522 
01523     int i,ni;
01524 
01525     /* Update               */ 
01526     if(m_bUpdateRtCst)
01527     {
01528         m_bUpdateRtCst=false;
01529         updateConstants();
01530         m_fdbvt.clear();
01531         if(m_cfg.collisions&fCollision::VF_SS)
01532         {
01533             initializeFaceTree();           
01534         }
01535     }
01536 
01537     /* Prepare              */ 
01538     m_sst.sdt       =   dt*m_cfg.timescale;
01539     m_sst.isdt      =   1/m_sst.sdt;
01540     m_sst.velmrg    =   m_sst.sdt*3;
01541     m_sst.radmrg    =   getCollisionShape()->getMargin();
01542     m_sst.updmrg    =   m_sst.radmrg*(btScalar)0.25;
01543     /* Forces               */ 
01544     addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
01545     applyForces();
01546     /* Integrate            */ 
01547     for(i=0,ni=m_nodes.size();i<ni;++i)
01548     {
01549         Node&   n=m_nodes[i];
01550         n.m_q   =   n.m_x;
01551         n.m_v   +=  n.m_f*n.m_im*m_sst.sdt;
01552         n.m_x   +=  n.m_v*m_sst.sdt;
01553         n.m_f   =   btVector3(0,0,0);
01554     }
01555     /* Clusters             */ 
01556     updateClusters();
01557     /* Bounds               */ 
01558     updateBounds(); 
01559     /* Nodes                */ 
01560     ATTRIBUTE_ALIGNED16(btDbvtVolume)   vol;
01561     for(i=0,ni=m_nodes.size();i<ni;++i)
01562     {
01563         Node&   n=m_nodes[i];
01564         vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
01565         m_ndbvt.update( n.m_leaf,
01566             vol,
01567             n.m_v*m_sst.velmrg,
01568             m_sst.updmrg);
01569     }
01570     /* Faces                */ 
01571     if(!m_fdbvt.empty())
01572     {
01573         for(int i=0;i<m_faces.size();++i)
01574         {
01575             Face&           f=m_faces[i];
01576             const btVector3 v=( f.m_n[0]->m_v+
01577                 f.m_n[1]->m_v+
01578                 f.m_n[2]->m_v)/3;
01579             vol = VolumeOf(f,m_sst.radmrg);
01580             m_fdbvt.update( f.m_leaf,
01581                 vol,
01582                 v*m_sst.velmrg,
01583                 m_sst.updmrg);
01584         }
01585     }
01586     /* Pose                 */ 
01587     updatePose();
01588     /* Match                */ 
01589     if(m_pose.m_bframe&&(m_cfg.kMT>0))
01590     {
01591         const btMatrix3x3   posetrs=m_pose.m_rot;
01592         for(int i=0,ni=m_nodes.size();i<ni;++i)
01593         {
01594             Node&   n=m_nodes[i];
01595             if(n.m_im>0)
01596             {
01597                 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
01598                 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
01599             }
01600         }
01601     }
01602     /* Clear contacts       */ 
01603     m_rcontacts.resize(0);
01604     m_scontacts.resize(0);
01605     /* Optimize dbvt's      */ 
01606     m_ndbvt.optimizeIncremental(1);
01607     m_fdbvt.optimizeIncremental(1);
01608     m_cdbvt.optimizeIncremental(1);
01609 }
01610 
01611 //
01612 void            btSoftBody::solveConstraints()
01613 {
01614 
01615     /* Apply clusters       */ 
01616     applyClusters(false);
01617     /* Prepare links        */ 
01618 
01619     int i,ni;
01620 
01621     for(i=0,ni=m_links.size();i<ni;++i)
01622     {
01623         Link&   l=m_links[i];
01624         l.m_c3      =   l.m_n[1]->m_q-l.m_n[0]->m_q;
01625         l.m_c2      =   1/(l.m_c3.length2()*l.m_c0);
01626     }
01627     /* Prepare anchors      */ 
01628     for(i=0,ni=m_anchors.size();i<ni;++i)
01629     {
01630         Anchor&         a=m_anchors[i];
01631         const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
01632         a.m_c0  =   ImpulseMatrix(  m_sst.sdt,
01633             a.m_node->m_im,
01634             a.m_body->getInvMass(),
01635             a.m_body->getInvInertiaTensorWorld(),
01636             ra);
01637         a.m_c1  =   ra;
01638         a.m_c2  =   m_sst.sdt*a.m_node->m_im;
01639         a.m_body->activate();
01640     }
01641     /* Solve velocities     */ 
01642     if(m_cfg.viterations>0)
01643     {
01644         /* Solve            */ 
01645         for(int isolve=0;isolve<m_cfg.viterations;++isolve)
01646         {
01647             for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
01648             {
01649                 getSolver(m_cfg.m_vsequence[iseq])(this,1);
01650             }
01651         }
01652         /* Update           */ 
01653         for(i=0,ni=m_nodes.size();i<ni;++i)
01654         {
01655             Node&   n=m_nodes[i];
01656             n.m_x   =   n.m_q+n.m_v*m_sst.sdt;
01657         }
01658     }
01659     /* Solve positions      */ 
01660     if(m_cfg.piterations>0)
01661     {
01662         for(int isolve=0;isolve<m_cfg.piterations;++isolve)
01663         {
01664             const btScalar ti=isolve/(btScalar)m_cfg.piterations;
01665             for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
01666             {
01667                 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
01668             }
01669         }
01670         const btScalar  vc=m_sst.isdt*(1-m_cfg.kDP);
01671         for(i=0,ni=m_nodes.size();i<ni;++i)
01672         {
01673             Node&   n=m_nodes[i];
01674             n.m_v   =   (n.m_x-n.m_q)*vc;
01675             n.m_f   =   btVector3(0,0,0);       
01676         }
01677     }
01678     /* Solve drift          */ 
01679     if(m_cfg.diterations>0)
01680     {
01681         const btScalar  vcf=m_cfg.kVCF*m_sst.isdt;
01682         for(i=0,ni=m_nodes.size();i<ni;++i)
01683         {
01684             Node&   n=m_nodes[i];
01685             n.m_q   =   n.m_x;
01686         }
01687         for(int idrift=0;idrift<m_cfg.diterations;++idrift)
01688         {
01689             for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
01690             {
01691                 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
01692             }
01693         }
01694         for(int i=0,ni=m_nodes.size();i<ni;++i)
01695         {
01696             Node&   n=m_nodes[i];
01697             n.m_v   +=  (n.m_x-n.m_q)*vcf;
01698         }
01699     }
01700     /* Apply clusters       */ 
01701     dampClusters();
01702     applyClusters(true);
01703 }
01704 
01705 //
01706 void            btSoftBody::staticSolve(int iterations)
01707 {
01708     for(int isolve=0;isolve<iterations;++isolve)
01709     {
01710         for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
01711         {
01712             getSolver(m_cfg.m_psequence[iseq])(this,1,0);
01713         }
01714     }
01715 }
01716 
01717 //
01718 void            btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
01719 {
01721 }
01722 
01723 //
01724 void            btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
01725 {
01726     const int   nb=bodies.size();
01727     int         iterations=0;
01728     int i;
01729 
01730     for(i=0;i<nb;++i)
01731     {
01732         iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
01733     }
01734     for(i=0;i<nb;++i)
01735     {
01736         bodies[i]->prepareClusters(iterations);
01737     }
01738     for(i=0;i<iterations;++i)
01739     {
01740         const btScalar sor=1;
01741         for(int j=0;j<nb;++j)
01742         {
01743             bodies[j]->solveClusters(sor);
01744         }
01745     }
01746     for(i=0;i<nb;++i)
01747     {
01748         bodies[i]->cleanupClusters();
01749     }
01750 }
01751 
01752 //
01753 void            btSoftBody::integrateMotion()
01754 {
01755     /* Update           */ 
01756     updateNormals();
01757 }
01758 
01759 //
01760 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
01761 {
01762     m_rayFrom = rayFrom;
01763     m_rayNormalizedDirection = (rayTo-rayFrom);
01764     m_rayTo = rayTo;
01765     m_mint  =   mxt;
01766     m_face  =   0;
01767     m_tests =   0;
01768 }
01769 
01770 //
01771 void                btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
01772 {
01773     btSoftBody::Face&   f=*(btSoftBody::Face*)leaf->data;
01774     const btScalar      t=rayFromToTriangle(    m_rayFrom,m_rayTo,m_rayNormalizedDirection,
01775         f.m_n[0]->m_x,
01776         f.m_n[1]->m_x,
01777         f.m_n[2]->m_x,
01778         m_mint);
01779     if((t>0)&&(t<m_mint)) 
01780     { 
01781         m_mint=t;m_face=&f; 
01782     }
01783     ++m_tests;
01784 }
01785 
01786 //
01787 btScalar            btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
01788                                                                    const btVector3& rayTo,
01789                                                                    const btVector3& rayNormalizedDirection,
01790                                                                    const btVector3& a,
01791                                                                    const btVector3& b,
01792                                                                    const btVector3& c,
01793                                                                    btScalar maxt)
01794 {
01795     static const btScalar   ceps=-SIMD_EPSILON*10;
01796     static const btScalar   teps=SIMD_EPSILON*10;
01797 
01798     const btVector3         n=btCross(b-a,c-a);
01799     const btScalar          d=btDot(a,n);
01800     const btScalar          den=btDot(rayNormalizedDirection,n);
01801     if(!btFuzzyZero(den))
01802     {
01803         const btScalar      num=btDot(rayFrom,n)-d;
01804         const btScalar      t=-num/den;
01805         if((t>teps)&&(t<maxt))
01806         {
01807             const btVector3 hit=rayFrom+rayNormalizedDirection*t;
01808             if( (btDot(n,btCross(a-hit,b-hit))>ceps)    &&          
01809                 (btDot(n,btCross(b-hit,c-hit))>ceps)    &&
01810                 (btDot(n,btCross(c-hit,a-hit))>ceps))
01811             {
01812                 return(t);
01813             }
01814         }
01815     }
01816     return(-1);
01817 }
01818 
01819 //
01820 void                btSoftBody::pointersToIndices()
01821 {
01822 #define PTR2IDX(_p_,_b_)    reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
01823     btSoftBody::Node*   base=&m_nodes[0];
01824     int i,ni;
01825 
01826     for(i=0,ni=m_nodes.size();i<ni;++i)
01827     {
01828         if(m_nodes[i].m_leaf)
01829         {
01830             m_nodes[i].m_leaf->data=*(void**)&i;
01831         }
01832     }
01833     for(i=0,ni=m_links.size();i<ni;++i)
01834     {
01835         m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
01836         m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
01837     }
01838     for(i=0,ni=m_faces.size();i<ni;++i)
01839     {
01840         m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
01841         m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
01842         m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
01843         if(m_faces[i].m_leaf)
01844         {
01845             m_faces[i].m_leaf->data=*(void**)&i;
01846         }
01847     }
01848     for(i=0,ni=m_anchors.size();i<ni;++i)
01849     {
01850         m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
01851     }
01852     for(i=0,ni=m_notes.size();i<ni;++i)
01853     {
01854         for(int j=0;j<m_notes[i].m_rank;++j)
01855         {
01856             m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
01857         }
01858     }
01859 #undef  PTR2IDX
01860 }
01861 
01862 //
01863 void                btSoftBody::indicesToPointers(const int* map)
01864 {
01865 #define IDX2PTR(_p_,_b_)    map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
01866     (&(_b_)[(((char*)_p_)-(char*)0)])
01867     btSoftBody::Node*   base=&m_nodes[0];
01868     int i,ni;
01869 
01870     for(i=0,ni=m_nodes.size();i<ni;++i)
01871     {
01872         if(m_nodes[i].m_leaf)
01873         {
01874             m_nodes[i].m_leaf->data=&m_nodes[i];
01875         }
01876     }
01877     for(i=0,ni=m_links.size();i<ni;++i)
01878     {
01879         m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
01880         m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
01881     }
01882     for(i=0,ni=m_faces.size();i<ni;++i)
01883     {
01884         m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
01885         m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
01886         m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
01887         if(m_faces[i].m_leaf)
01888         {
01889             m_faces[i].m_leaf->data=&m_faces[i];
01890         }
01891     }
01892     for(i=0,ni=m_anchors.size();i<ni;++i)
01893     {
01894         m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
01895     }
01896     for(i=0,ni=m_notes.size();i<ni;++i)
01897     {
01898         for(int j=0;j<m_notes[i].m_rank;++j)
01899         {
01900             m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
01901         }
01902     }
01903 #undef  IDX2PTR
01904 }
01905 
01906 //
01907 int                 btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
01908                                         btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
01909 {
01910     int cnt=0;
01911     if(bcountonly||m_fdbvt.empty())
01912     {/* Full search */ 
01913         btVector3 dir = rayTo-rayFrom;
01914         dir.normalize();
01915 
01916         for(int i=0,ni=m_faces.size();i<ni;++i)
01917         {
01918             const btSoftBody::Face& f=m_faces[i];
01919 
01920             const btScalar          t=RayFromToCaster::rayFromToTriangle(   rayFrom,rayTo,dir,
01921                 f.m_n[0]->m_x,
01922                 f.m_n[1]->m_x,
01923                 f.m_n[2]->m_x,
01924                 mint);
01925             if(t>0)
01926             {
01927                 ++cnt;
01928                 if(!bcountonly)
01929                 {
01930                     feature=btSoftBody::eFeature::Face;
01931                     index=i;
01932                     mint=t;
01933                 }
01934             }
01935         }
01936     }
01937     else
01938     {/* Use dbvt    */ 
01939         RayFromToCaster collider(rayFrom,rayTo,mint);
01940 
01941         btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
01942         if(collider.m_face)
01943         {
01944             mint=collider.m_mint;
01945             feature=btSoftBody::eFeature::Face;
01946             index=(int)(collider.m_face-&m_faces[0]);
01947             cnt=1;
01948         }
01949     }
01950     return(cnt);
01951 }
01952 
01953 //
01954 void            btSoftBody::initializeFaceTree()
01955 {
01956     m_fdbvt.clear();
01957     for(int i=0;i<m_faces.size();++i)
01958     {
01959         Face&   f=m_faces[i];
01960         f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
01961     }
01962 }
01963 
01964 //
01965 btVector3       btSoftBody::evaluateCom() const
01966 {
01967     btVector3   com(0,0,0);
01968     if(m_pose.m_bframe)
01969     {
01970         for(int i=0,ni=m_nodes.size();i<ni;++i)
01971         {
01972             com+=m_nodes[i].m_x*m_pose.m_wgh[i];
01973         }
01974     }
01975     return(com);
01976 }
01977 
01978 //
01979 bool                btSoftBody::checkContact(   btCollisionObject* colObj,
01980                                              const btVector3& x,
01981                                              btScalar margin,
01982                                              btSoftBody::sCti& cti) const
01983 {
01984     btVector3 nrm;
01985     btCollisionShape *shp = colObj->getCollisionShape();
01986     btRigidBody *tmpRigid = btRigidBody::upcast(colObj);
01987     const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObj->getWorldTransform();
01988     btScalar dst = 
01989         m_worldInfo->m_sparsesdf.Evaluate(  
01990             wtr.invXform(x),
01991             shp,
01992             nrm,
01993             margin);
01994     if(dst<0)
01995     {
01996         cti.m_colObj = colObj;
01997         cti.m_normal = wtr.getBasis()*nrm;
01998         cti.m_offset = -btDot( cti.m_normal, x - cti.m_normal * dst );
01999         return(true);
02000     }
02001     return(false);
02002 }
02003 
02004 //
02005 void                    btSoftBody::updateNormals()
02006 {
02007 
02008     const btVector3 zv(0,0,0);
02009     int i,ni;
02010 
02011     for(i=0,ni=m_nodes.size();i<ni;++i)
02012     {
02013         m_nodes[i].m_n=zv;
02014     }
02015     for(i=0,ni=m_faces.size();i<ni;++i)
02016     {
02017         btSoftBody::Face&   f=m_faces[i];
02018         const btVector3     n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
02019             f.m_n[2]->m_x-f.m_n[0]->m_x);
02020         f.m_normal=n.normalized();
02021         f.m_n[0]->m_n+=n;
02022         f.m_n[1]->m_n+=n;
02023         f.m_n[2]->m_n+=n;
02024     }
02025     for(i=0,ni=m_nodes.size();i<ni;++i)
02026     {
02027         btScalar len = m_nodes[i].m_n.length();
02028         if (len>SIMD_EPSILON)
02029             m_nodes[i].m_n /= len;
02030     }
02031 }
02032 
02033 //
02034 void                    btSoftBody::updateBounds()
02035 {
02036     /*if( m_acceleratedSoftBody )
02037     {
02038         // If we have an accelerated softbody we need to obtain the bounds correctly
02039         // For now (slightly hackily) just have a very large AABB
02040         // TODO: Write get bounds kernel
02041         // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
02042         // probably do a test and exchange reasonably efficiently.
02043 
02044         m_bounds[0] = btVector3(-1000, -1000, -1000);
02045         m_bounds[1] = btVector3(1000, 1000, 1000);
02046 
02047     } else {*/
02048         if(m_ndbvt.m_root)
02049         {
02050             const btVector3&    mins=m_ndbvt.m_root->volume.Mins();
02051             const btVector3&    maxs=m_ndbvt.m_root->volume.Maxs();
02052             const btScalar      csm=getCollisionShape()->getMargin();
02053             const btVector3     mrg=btVector3(  csm,
02054                 csm,
02055                 csm)*1; // ??? to investigate...
02056             m_bounds[0]=mins-mrg;
02057             m_bounds[1]=maxs+mrg;
02058             if(0!=getBroadphaseHandle())
02059             {                   
02060                 m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
02061                     m_bounds[0],
02062                     m_bounds[1],
02063                     m_worldInfo->m_dispatcher);
02064             }
02065         }
02066         else
02067         {
02068             m_bounds[0]=
02069                 m_bounds[1]=btVector3(0,0,0);
02070         }       
02071     //}
02072 }
02073 
02074 
02075 //
02076 void                    btSoftBody::updatePose()
02077 {
02078     if(m_pose.m_bframe)
02079     {
02080         btSoftBody::Pose&   pose=m_pose;
02081         const btVector3     com=evaluateCom();
02082         /* Com          */ 
02083         pose.m_com  =   com;
02084         /* Rotation     */ 
02085         btMatrix3x3     Apq;
02086         const btScalar  eps=SIMD_EPSILON;
02087         Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
02088         Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
02089         for(int i=0,ni=m_nodes.size();i<ni;++i)
02090         {
02091             const btVector3     a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
02092             const btVector3&    b=pose.m_pos[i];
02093             Apq[0]+=a.x()*b;
02094             Apq[1]+=a.y()*b;
02095             Apq[2]+=a.z()*b;
02096         }
02097         btMatrix3x3     r,s;
02098         PolarDecompose(Apq,r,s);
02099         pose.m_rot=r;
02100         pose.m_scl=pose.m_aqq*r.transpose()*Apq;
02101         if(m_cfg.maxvolume>1)
02102         {
02103             const btScalar  idet=Clamp<btScalar>(   1/pose.m_scl.determinant(),
02104                 1,m_cfg.maxvolume);
02105             pose.m_scl=Mul(pose.m_scl,idet);
02106         }
02107 
02108     }
02109 }
02110 
02111 //
02112 void                btSoftBody::updateConstants()
02113 {
02114     int i,ni;
02115 
02116     /* Links        */ 
02117     for(i=0,ni=m_links.size();i<ni;++i)
02118     {
02119         Link&       l=m_links[i];
02120         Material&   m=*l.m_material;
02121         l.m_rl  =   (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
02122         l.m_c0  =   (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
02123         l.m_c1  =   l.m_rl*l.m_rl;
02124     }
02125     /* Faces        */ 
02126     for(i=0,ni=m_faces.size();i<ni;++i)
02127     {
02128         Face&       f=m_faces[i];
02129         f.m_ra  =   AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
02130     }
02131     /* Area's       */ 
02132     btAlignedObjectArray<int>   counts;
02133     counts.resize(m_nodes.size(),0);
02134     for(i=0,ni=m_nodes.size();i<ni;++i)
02135     {
02136         m_nodes[i].m_area   =   0;
02137     }
02138     for(i=0,ni=m_faces.size();i<ni;++i)
02139     {
02140         btSoftBody::Face&   f=m_faces[i];
02141         for(int j=0;j<3;++j)
02142         {
02143             const int index=(int)(f.m_n[j]-&m_nodes[0]);
02144             counts[index]++;
02145             f.m_n[j]->m_area+=btFabs(f.m_ra);
02146         }
02147     }
02148     for(i=0,ni=m_nodes.size();i<ni;++i)
02149     {
02150         if(counts[i]>0)
02151             m_nodes[i].m_area/=(btScalar)counts[i];
02152         else
02153             m_nodes[i].m_area=0;
02154     }
02155 }
02156 
02157 //
02158 void                    btSoftBody::initializeClusters()
02159 {
02160     int i;
02161 
02162     for( i=0;i<m_clusters.size();++i)
02163     {
02164         Cluster&    c=*m_clusters[i];
02165         c.m_imass=0;
02166         c.m_masses.resize(c.m_nodes.size());
02167         for(int j=0;j<c.m_nodes.size();++j)
02168         {
02169             if (c.m_nodes[j]->m_im==0)
02170             {
02171                 c.m_containsAnchor = true;
02172                 c.m_masses[j]   =   BT_LARGE_FLOAT;
02173             } else
02174             {
02175                 c.m_masses[j]   =   btScalar(1.)/c.m_nodes[j]->m_im;
02176             }
02177             c.m_imass       +=  c.m_masses[j];
02178         }
02179         c.m_imass       =   btScalar(1.)/c.m_imass;
02180         c.m_com         =   btSoftBody::clusterCom(&c);
02181         c.m_lv          =   btVector3(0,0,0);
02182         c.m_av          =   btVector3(0,0,0);
02183         c.m_leaf        =   0;
02184         /* Inertia  */ 
02185         btMatrix3x3&    ii=c.m_locii;
02186         ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
02187         {
02188             int i,ni;
02189 
02190             for(i=0,ni=c.m_nodes.size();i<ni;++i)
02191             {
02192                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
02193                 const btVector3 q=k*k;
02194                 const btScalar  m=c.m_masses[i];
02195                 ii[0][0]    +=  m*(q[1]+q[2]);
02196                 ii[1][1]    +=  m*(q[0]+q[2]);
02197                 ii[2][2]    +=  m*(q[0]+q[1]);
02198                 ii[0][1]    -=  m*k[0]*k[1];
02199                 ii[0][2]    -=  m*k[0]*k[2];
02200                 ii[1][2]    -=  m*k[1]*k[2];
02201             }
02202         }
02203         ii[1][0]=ii[0][1];
02204         ii[2][0]=ii[0][2];
02205         ii[2][1]=ii[1][2];
02206         
02207         ii = ii.inverse();
02208 
02209         /* Frame    */ 
02210         c.m_framexform.setIdentity();
02211         c.m_framexform.setOrigin(c.m_com);
02212         c.m_framerefs.resize(c.m_nodes.size());
02213         {
02214             int i;
02215             for(i=0;i<c.m_framerefs.size();++i)
02216             {
02217                 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
02218             }
02219         }
02220     }
02221 }
02222 
02223 //
02224 void                    btSoftBody::updateClusters()
02225 {
02226     BT_PROFILE("UpdateClusters");
02227     int i;
02228 
02229     for(i=0;i<m_clusters.size();++i)
02230     {
02231         btSoftBody::Cluster&    c=*m_clusters[i];
02232         const int               n=c.m_nodes.size();
02233         //const btScalar            invn=1/(btScalar)n;
02234         if(n)
02235         {
02236             /* Frame                */ 
02237             const btScalar  eps=btScalar(0.0001);
02238             btMatrix3x3     m,r,s;
02239             m[0]=m[1]=m[2]=btVector3(0,0,0);
02240             m[0][0]=eps*1;
02241             m[1][1]=eps*2;
02242             m[2][2]=eps*3;
02243             c.m_com=clusterCom(&c);
02244             for(int i=0;i<c.m_nodes.size();++i)
02245             {
02246                 const btVector3     a=c.m_nodes[i]->m_x-c.m_com;
02247                 const btVector3&    b=c.m_framerefs[i];
02248                 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
02249             }
02250             PolarDecompose(m,r,s);
02251             c.m_framexform.setOrigin(c.m_com);
02252             c.m_framexform.setBasis(r);     
02253             /* Inertia          */ 
02254 #if 1/* Constant    */ 
02255             c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
02256 #else
02257 #if 0/* Sphere  */ 
02258             const btScalar  rk=(2*c.m_extents.length2())/(5*c.m_imass);
02259             const btVector3 inertia(rk,rk,rk);
02260             const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
02261                 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
02262                 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
02263 
02264             c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
02265 #else/* Actual  */      
02266             c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
02267             for(int i=0;i<n;++i)
02268             {
02269                 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
02270                 const btVector3     q=k*k;
02271                 const btScalar      m=1/c.m_nodes[i]->m_im;
02272                 c.m_invwi[0][0] +=  m*(q[1]+q[2]);
02273                 c.m_invwi[1][1] +=  m*(q[0]+q[2]);
02274                 c.m_invwi[2][2] +=  m*(q[0]+q[1]);
02275                 c.m_invwi[0][1] -=  m*k[0]*k[1];
02276                 c.m_invwi[0][2] -=  m*k[0]*k[2];
02277                 c.m_invwi[1][2] -=  m*k[1]*k[2];
02278             }
02279             c.m_invwi[1][0]=c.m_invwi[0][1];
02280             c.m_invwi[2][0]=c.m_invwi[0][2];
02281             c.m_invwi[2][1]=c.m_invwi[1][2];
02282             c.m_invwi=c.m_invwi.inverse();
02283 #endif
02284 #endif
02285             /* Velocities           */ 
02286             c.m_lv=btVector3(0,0,0);
02287             c.m_av=btVector3(0,0,0);
02288             {
02289                 int i;
02290 
02291                 for(i=0;i<n;++i)
02292                 {
02293                     const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
02294                     c.m_lv  +=  v;
02295                     c.m_av  +=  btCross(c.m_nodes[i]->m_x-c.m_com,v);
02296                 }
02297             }
02298             c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
02299             c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
02300             c.m_vimpulses[0]    =
02301                 c.m_vimpulses[1]    = btVector3(0,0,0);
02302             c.m_dimpulses[0]    =
02303                 c.m_dimpulses[1]    = btVector3(0,0,0);
02304             c.m_nvimpulses      = 0;
02305             c.m_ndimpulses      = 0;
02306             /* Matching             */ 
02307             if(c.m_matching>0)
02308             {
02309                 for(int j=0;j<c.m_nodes.size();++j)
02310                 {
02311                     Node&           n=*c.m_nodes[j];
02312                     const btVector3 x=c.m_framexform*c.m_framerefs[j];
02313                     n.m_x=Lerp(n.m_x,x,c.m_matching);
02314                 }
02315             }           
02316             /* Dbvt                 */ 
02317             if(c.m_collide)
02318             {
02319                 btVector3   mi=c.m_nodes[0]->m_x;
02320                 btVector3   mx=mi;
02321                 for(int j=1;j<n;++j)
02322                 {
02323                     mi.setMin(c.m_nodes[j]->m_x);
02324                     mx.setMax(c.m_nodes[j]->m_x);
02325                 }           
02326                 ATTRIBUTE_ALIGNED16(btDbvtVolume)   bounds=btDbvtVolume::FromMM(mi,mx);
02327                 if(c.m_leaf)
02328                     m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
02329                 else
02330                     c.m_leaf=m_cdbvt.insert(bounds,&c);
02331             }
02332         }
02333     }
02334 
02335 
02336 }
02337 
02338 
02339 
02340 
02341 //
02342 void                    btSoftBody::cleanupClusters()
02343 {
02344     for(int i=0;i<m_joints.size();++i)
02345     {
02346         m_joints[i]->Terminate(m_sst.sdt);
02347         if(m_joints[i]->m_delete)
02348         {
02349             btAlignedFree(m_joints[i]);
02350             m_joints.remove(m_joints[i--]);
02351         }   
02352     }
02353 }
02354 
02355 //
02356 void                    btSoftBody::prepareClusters(int iterations)
02357 {
02358     for(int i=0;i<m_joints.size();++i)
02359     {
02360         m_joints[i]->Prepare(m_sst.sdt,iterations);
02361     }
02362 }
02363 
02364 
02365 //
02366 void                    btSoftBody::solveClusters(btScalar sor)
02367 {
02368     for(int i=0,ni=m_joints.size();i<ni;++i)
02369     {
02370         m_joints[i]->Solve(m_sst.sdt,sor);
02371     }
02372 }
02373 
02374 //
02375 void                    btSoftBody::applyClusters(bool drift)
02376 {
02377     BT_PROFILE("ApplyClusters");
02378 //  const btScalar                  f0=m_sst.sdt;
02379     //const btScalar                    f1=f0/2;
02380     btAlignedObjectArray<btVector3> deltas;
02381     btAlignedObjectArray<btScalar> weights;
02382     deltas.resize(m_nodes.size(),btVector3(0,0,0));
02383     weights.resize(m_nodes.size(),0);
02384     int i;
02385 
02386     if(drift)
02387     {
02388         for(i=0;i<m_clusters.size();++i)
02389         {
02390             Cluster&    c=*m_clusters[i];
02391             if(c.m_ndimpulses)
02392             {
02393                 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
02394                 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
02395             }
02396         }
02397     }
02398     
02399     for(i=0;i<m_clusters.size();++i)
02400     {
02401         Cluster&    c=*m_clusters[i];   
02402         if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
02403         {
02404             const btVector3     v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
02405             const btVector3     w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
02406             for(int j=0;j<c.m_nodes.size();++j)
02407             {
02408                 const int           idx=int(c.m_nodes[j]-&m_nodes[0]);
02409                 const btVector3&    x=c.m_nodes[j]->m_x;
02410                 const btScalar      q=c.m_masses[j];
02411                 deltas[idx]     +=  (v+btCross(w,x-c.m_com))*q;
02412                 weights[idx]    +=  q;
02413             }
02414         }
02415     }
02416     for(i=0;i<deltas.size();++i)
02417     {
02418         if(weights[i]>0) 
02419         {
02420             m_nodes[i].m_x+=deltas[i]/weights[i];
02421         }
02422     }
02423 }
02424 
02425 //
02426 void                    btSoftBody::dampClusters()
02427 {
02428     int i;
02429 
02430     for(i=0;i<m_clusters.size();++i)
02431     {
02432         Cluster&    c=*m_clusters[i];   
02433         if(c.m_ndamping>0)
02434         {
02435             for(int j=0;j<c.m_nodes.size();++j)
02436             {
02437                 Node&           n=*c.m_nodes[j];
02438                 if(n.m_im>0)
02439                 {
02440                     const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
02441                     if(vx.length2()<=n.m_v.length2())
02442                         {
02443                         n.m_v   +=  c.m_ndamping*(vx-n.m_v);
02444                         }
02445                 }
02446             }
02447         }
02448     }
02449 }
02450 
02451 //
02452 void                btSoftBody::Joint::Prepare(btScalar dt,int)
02453 {
02454     m_bodies[0].activate();
02455     m_bodies[1].activate();
02456 }
02457 
02458 //
02459 void                btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
02460 {
02461     static const btScalar   maxdrift=4;
02462     Joint::Prepare(dt,iterations);
02463     m_rpos[0]       =   m_bodies[0].xform()*m_refs[0];
02464     m_rpos[1]       =   m_bodies[1].xform()*m_refs[1];
02465     m_drift         =   Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
02466     m_rpos[0]       -=  m_bodies[0].xform().getOrigin();
02467     m_rpos[1]       -=  m_bodies[1].xform().getOrigin();
02468     m_massmatrix    =   ImpulseMatrix(  m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
02469         m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
02470     if(m_split>0)
02471     {
02472         m_sdrift    =   m_massmatrix*(m_drift*m_split);
02473         m_drift     *=  1-m_split;
02474     }
02475     m_drift /=(btScalar)iterations;
02476 }
02477 
02478 //
02479 void                btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
02480 {
02481     const btVector3     va=m_bodies[0].velocity(m_rpos[0]);
02482     const btVector3     vb=m_bodies[1].velocity(m_rpos[1]);
02483     const btVector3     vr=va-vb;
02484     btSoftBody::Impulse impulse;
02485     impulse.m_asVelocity    =   1;
02486     impulse.m_velocity      =   m_massmatrix*(m_drift+vr*m_cfm)*sor;
02487     m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
02488     m_bodies[1].applyImpulse( impulse,m_rpos[1]);
02489 }
02490 
02491 //
02492 void                btSoftBody::LJoint::Terminate(btScalar dt)
02493 {
02494     if(m_split>0)
02495     {
02496         m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
02497         m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
02498     }
02499 }
02500 
02501 //
02502 void                btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
02503 {
02504     static const btScalar   maxdrift=SIMD_PI/16;
02505     m_icontrol->Prepare(this);
02506     Joint::Prepare(dt,iterations);
02507     m_axis[0]   =   m_bodies[0].xform().getBasis()*m_refs[0];
02508     m_axis[1]   =   m_bodies[1].xform().getBasis()*m_refs[1];
02509     m_drift     =   NormalizeAny(btCross(m_axis[1],m_axis[0]));
02510     m_drift     *=  btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
02511     m_drift     *=  m_erp/dt;
02512     m_massmatrix=   AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
02513     if(m_split>0)
02514     {
02515         m_sdrift    =   m_massmatrix*(m_drift*m_split);
02516         m_drift     *=  1-m_split;
02517     }
02518     m_drift /=(btScalar)iterations;
02519 }
02520 
02521 //
02522 void                btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
02523 {
02524     const btVector3     va=m_bodies[0].angularVelocity();
02525     const btVector3     vb=m_bodies[1].angularVelocity();
02526     const btVector3     vr=va-vb;
02527     const btScalar      sp=btDot(vr,m_axis[0]);
02528     const btVector3     vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
02529     btSoftBody::Impulse impulse;
02530     impulse.m_asVelocity    =   1;
02531     impulse.m_velocity      =   m_massmatrix*(m_drift+vc*m_cfm)*sor;
02532     m_bodies[0].applyAImpulse(-impulse);
02533     m_bodies[1].applyAImpulse( impulse);
02534 }
02535 
02536 //
02537 void                btSoftBody::AJoint::Terminate(btScalar dt)
02538 {
02539     if(m_split>0)
02540     {
02541         m_bodies[0].applyDAImpulse(-m_sdrift);
02542         m_bodies[1].applyDAImpulse( m_sdrift);
02543     }
02544 }
02545 
02546 //
02547 void                btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
02548 {
02549     Joint::Prepare(dt,iterations);
02550     const bool  dodrift=(m_life==0);
02551     m_delete=(++m_life)>m_maxlife;
02552     if(dodrift)
02553     {
02554         m_drift=m_drift*m_erp/dt;
02555         if(m_split>0)
02556         {
02557             m_sdrift    =   m_massmatrix*(m_drift*m_split);
02558             m_drift     *=  1-m_split;
02559         }
02560         m_drift/=(btScalar)iterations;
02561     }
02562     else
02563     {
02564         m_drift=m_sdrift=btVector3(0,0,0);
02565     }
02566 }
02567 
02568 //
02569 void                btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
02570 {
02571     const btVector3     va=m_bodies[0].velocity(m_rpos[0]);
02572     const btVector3     vb=m_bodies[1].velocity(m_rpos[1]);
02573     const btVector3     vrel=va-vb;
02574     const btScalar      rvac=btDot(vrel,m_normal);
02575     btSoftBody::Impulse impulse;
02576     impulse.m_asVelocity    =   1;
02577     impulse.m_velocity      =   m_drift;
02578     if(rvac<0)
02579     {
02580         const btVector3 iv=m_normal*rvac;
02581         const btVector3 fv=vrel-iv;
02582         impulse.m_velocity  +=  iv+fv*m_friction;
02583     }
02584     impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
02585     
02586     if (m_bodies[0].m_soft==m_bodies[1].m_soft)
02587     {
02588         if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
02589             (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
02590         {
02591             if (impulse.m_asVelocity)
02592             {
02593                 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
02594                 {
02595                     
02596                 } else
02597                 {
02598                     m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
02599                     m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
02600                 }
02601             }
02602         }
02603     } else
02604     {
02605         m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
02606         m_bodies[1].applyImpulse( impulse,m_rpos[1]);
02607     }
02608 }
02609 
02610 //
02611 void                btSoftBody::CJoint::Terminate(btScalar dt)
02612 {
02613     if(m_split>0)
02614     {
02615         m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
02616         m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
02617     }
02618 }
02619 
02620 //
02621 void                btSoftBody::applyForces()
02622 {
02623 
02624     BT_PROFILE("SoftBody applyForces");
02625     const btScalar                  dt =            m_sst.sdt;
02626     const btScalar                  kLF =           m_cfg.kLF;
02627     const btScalar                  kDG =           m_cfg.kDG;
02628     const btScalar                  kPR =           m_cfg.kPR;
02629     const btScalar                  kVC =           m_cfg.kVC;
02630     const bool                      as_lift =       kLF>0;
02631     const bool                      as_drag =       kDG>0;
02632     const bool                      as_pressure =   kPR!=0;
02633     const bool                      as_volume =     kVC>0;
02634     const bool                      as_aero =       as_lift ||
02635                                                     as_drag     ;
02636     const bool                      as_vaero =      as_aero &&
02637                                                     (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
02638     const bool                      as_faero =      as_aero &&
02639                                                     (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
02640     const bool                      use_medium =    as_aero;
02641     const bool                      use_volume =    as_pressure ||
02642         as_volume   ;
02643     btScalar                        volume =        0;
02644     btScalar                        ivolumetp =     0;
02645     btScalar                        dvolumetv =     0;
02646     btSoftBody::sMedium medium;
02647     if(use_volume)
02648     {
02649         volume      =   getVolume();
02650         ivolumetp   =   1/btFabs(volume)*kPR;
02651         dvolumetv   =   (m_pose.m_volume-volume)*kVC;
02652     }
02653     /* Per vertex forces            */ 
02654     int i,ni;
02655 
02656     for(i=0,ni=m_nodes.size();i<ni;++i)
02657     {
02658         btSoftBody::Node&   n=m_nodes[i];
02659         if(n.m_im>0)
02660         {
02661             if(use_medium)
02662             {
02663                 EvaluateMedium(m_worldInfo, n.m_x, medium);
02664                 medium.m_velocity = m_windVelocity;
02665                 medium.m_density = m_worldInfo->air_density;
02666 
02667                 /* Aerodynamics         */ 
02668                 if(as_vaero)
02669                 {               
02670                     const btVector3 rel_v = n.m_v - medium.m_velocity;
02671                     const btScalar  rel_v2 = rel_v.length2();
02672                     if(rel_v2>SIMD_EPSILON)
02673                     {
02674                         btVector3   nrm = n.m_n;
02675                         /* Setup normal     */ 
02676                         switch(m_cfg.aeromodel)
02677                         {
02678                         case    btSoftBody::eAeroModel::V_Point:
02679                             nrm = NormalizeAny(rel_v);
02680                             break;
02681                         case    btSoftBody::eAeroModel::V_TwoSided:
02682                             nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1);
02683                             break;                          
02684                         default:
02685                             {
02686                             }
02687                         }
02688                         const btScalar dvn = btDot(rel_v,nrm);
02689                         /* Compute forces   */ 
02690                         if(dvn>0)
02691                         {
02692                             btVector3       force(0,0,0);
02693                             const btScalar  c0  =   n.m_area * dvn * rel_v2/2;
02694                             const btScalar  c1  =   c0 * medium.m_density;
02695                             force   +=  nrm*(-c1*kLF);
02696                             force   +=  rel_v.normalized() * (-c1 * kDG);
02697                             ApplyClampedForce(n, force, dt);
02698                         }
02699                     }
02700                 }
02701             }
02702             /* Pressure             */ 
02703             if(as_pressure)
02704             {
02705                 n.m_f   +=  n.m_n*(n.m_area*ivolumetp);
02706             }
02707             /* Volume               */ 
02708             if(as_volume)
02709             {
02710                 n.m_f   +=  n.m_n*(n.m_area*dvolumetv);
02711             }
02712         }
02713     }
02714     /* Per face forces              */ 
02715     for(i=0,ni=m_faces.size();i<ni;++i)
02716     {
02717         btSoftBody::Face&   f=m_faces[i];
02718         if(as_faero)
02719         {
02720             const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
02721             const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
02722             EvaluateMedium(m_worldInfo,x,medium);
02723             const btVector3 rel_v=v-medium.m_velocity;
02724             const btScalar  rel_v2=rel_v.length2();
02725             if(rel_v2>SIMD_EPSILON)
02726             {
02727                 btVector3   nrm=f.m_normal;
02728                 /* Setup normal     */ 
02729                 switch(m_cfg.aeromodel)
02730                 {
02731                 case    btSoftBody::eAeroModel::F_TwoSided:
02732                     nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
02733                     default:
02734                     {
02735                     }
02736                 }
02737                 const btScalar  dvn=btDot(rel_v,nrm);
02738                 /* Compute forces   */ 
02739                 if(dvn>0)
02740                 {
02741                     btVector3       force(0,0,0);
02742                     const btScalar  c0  =   f.m_ra*dvn*rel_v2;
02743                     const btScalar  c1  =   c0*medium.m_density;
02744                     force   +=  nrm*(-c1*kLF);
02745                     force   +=  rel_v.normalized()*(-c1*kDG);
02746                     force   /=  3;
02747                     for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
02748                 }
02749             }
02750         }
02751     }
02752 }
02753 
02754 //
02755 void                btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
02756 {
02757     const btScalar  kAHR=psb->m_cfg.kAHR*kst;
02758     const btScalar  dt=psb->m_sst.sdt;
02759     for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
02760     {
02761         const Anchor&       a=psb->m_anchors[i];
02762         const btTransform&  t=a.m_body->getWorldTransform();
02763         Node&               n=*a.m_node;
02764         const btVector3     wa=t*a.m_local;
02765         const btVector3     va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
02766         const btVector3     vb=n.m_x-n.m_q;
02767         const btVector3     vr=(va-vb)+(wa-n.m_x)*kAHR;
02768         const btVector3     impulse=a.m_c0*vr;
02769         n.m_x+=impulse*a.m_c2;
02770         a.m_body->applyImpulse(-impulse,a.m_c1);
02771     }
02772 }
02773 
02774 //
02775 void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
02776 {
02777     const btScalar  dt = psb->m_sst.sdt;
02778     const btScalar  mrg = psb->getCollisionShape()->getMargin();
02779     for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
02780     {
02781         const RContact&     c = psb->m_rcontacts[i];
02782         const sCti&         cti = c.m_cti;  
02783         btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj);
02784 
02785         const btVector3     va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
02786         const btVector3     vb = c.m_node->m_x-c.m_node->m_q;   
02787         const btVector3     vr = vb-va;
02788         const btScalar      dn = btDot(vr, cti.m_normal);       
02789         if(dn<=SIMD_EPSILON)
02790         {
02791             const btScalar      dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg );
02792             const btVector3     fv = vr - (cti.m_normal * dn);
02793             // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
02794             const btVector3     impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst );
02795             c.m_node->m_x -= impulse * c.m_c2;
02796             if (tmpRigid)
02797                 tmpRigid->applyImpulse(impulse,c.m_c1);
02798         }
02799     }
02800 }
02801 
02802 //
02803 void                btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
02804 {
02805     for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
02806     {
02807         const SContact&     c=psb->m_scontacts[i];
02808         const btVector3&    nr=c.m_normal;
02809         Node&               n=*c.m_node;
02810         Face&               f=*c.m_face;
02811         const btVector3     p=BaryEval( f.m_n[0]->m_x,
02812             f.m_n[1]->m_x,
02813             f.m_n[2]->m_x,
02814             c.m_weights);
02815         const btVector3     q=BaryEval( f.m_n[0]->m_q,
02816             f.m_n[1]->m_q,
02817             f.m_n[2]->m_q,
02818             c.m_weights);                                           
02819         const btVector3     vr=(n.m_x-n.m_q)-(p-q);
02820         btVector3           corr(0,0,0);
02821         btScalar dot = btDot(vr,nr);
02822         if(dot<0)
02823         {
02824             const btScalar  j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
02825             corr+=c.m_normal*j;
02826         }
02827         corr            -=  ProjectOnPlane(vr,nr)*c.m_friction;
02828         n.m_x           +=  corr*c.m_cfm[0];
02829         f.m_n[0]->m_x   -=  corr*(c.m_cfm[1]*c.m_weights.x());
02830         f.m_n[1]->m_x   -=  corr*(c.m_cfm[1]*c.m_weights.y());
02831         f.m_n[2]->m_x   -=  corr*(c.m_cfm[1]*c.m_weights.z());
02832     }
02833 }
02834 
02835 //
02836 void                btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
02837 {
02838     for(int i=0,ni=psb->m_links.size();i<ni;++i)
02839     {           
02840         Link&   l=psb->m_links[i];
02841         if(l.m_c0>0)
02842         {
02843             Node&           a=*l.m_n[0];
02844             Node&           b=*l.m_n[1];
02845             const btVector3 del=b.m_x-a.m_x;
02846             const btScalar  len=del.length2();
02847             if (l.m_c1+len > SIMD_EPSILON)
02848             {
02849                 const btScalar  k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
02850                 a.m_x-=del*(k*a.m_im);
02851                 b.m_x+=del*(k*b.m_im);
02852             }
02853         }
02854     }
02855 }
02856 
02857 //
02858 void                btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
02859 {
02860     for(int i=0,ni=psb->m_links.size();i<ni;++i)
02861     {           
02862         Link&           l=psb->m_links[i];
02863         Node**          n=l.m_n;
02864         const btScalar  j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
02865         n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
02866         n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
02867     }
02868 }
02869 
02870 //
02871 btSoftBody::psolver_t   btSoftBody::getSolver(ePSolver::_ solver)
02872 {
02873     switch(solver)
02874     {
02875     case    ePSolver::Anchors:      
02876         return(&btSoftBody::PSolve_Anchors);
02877     case    ePSolver::Linear:       
02878         return(&btSoftBody::PSolve_Links);
02879     case    ePSolver::RContacts:    
02880         return(&btSoftBody::PSolve_RContacts);
02881     case    ePSolver::SContacts:    
02882         return(&btSoftBody::PSolve_SContacts);  
02883         default:
02884         {
02885         }
02886     }
02887     return(0);
02888 }
02889 
02890 //
02891 btSoftBody::vsolver_t   btSoftBody::getSolver(eVSolver::_ solver)
02892 {
02893     switch(solver)
02894     {
02895     case    eVSolver::Linear:       return(&btSoftBody::VSolve_Links);
02896         default:
02897         {
02898         }
02899     }
02900     return(0);
02901 }
02902 
02903 //
02904 void            btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
02905 {
02906 
02907     switch(m_cfg.collisions&fCollision::RVSmask)
02908     {
02909     case    fCollision::SDF_RS:
02910         {
02911             btSoftColliders::CollideSDF_RS  docollide;      
02912             btRigidBody*        prb1=btRigidBody::upcast(pco);
02913             btTransform wtr=pco->getWorldTransform();
02914 
02915             const btTransform   ctr=pco->getWorldTransform();
02916             const btScalar      timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
02917             const btScalar      basemargin=getCollisionShape()->getMargin();
02918             btVector3           mins;
02919             btVector3           maxs;
02920             ATTRIBUTE_ALIGNED16(btDbvtVolume)       volume;
02921             pco->getCollisionShape()->getAabb(  pco->getWorldTransform(),
02922                 mins,
02923                 maxs);
02924             volume=btDbvtVolume::FromMM(mins,maxs);
02925             volume.Expand(btVector3(basemargin,basemargin,basemargin));     
02926             docollide.psb       =   this;
02927             docollide.m_colObj1 = pco;
02928             docollide.m_rigidBody = prb1;
02929 
02930             docollide.dynmargin =   basemargin+timemargin;
02931             docollide.stamargin =   basemargin;
02932             m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
02933         }
02934         break;
02935     case    fCollision::CL_RS:
02936         {
02937             btSoftColliders::CollideCL_RS   collider;
02938             collider.Process(this,pco);
02939         }
02940         break;
02941     }
02942 }
02943 
02944 //
02945 void            btSoftBody::defaultCollisionHandler(btSoftBody* psb)
02946 {
02947     const int cf=m_cfg.collisions&psb->m_cfg.collisions;
02948     switch(cf&fCollision::SVSmask)
02949     {
02950     case    fCollision::CL_SS:
02951         {
02952             
02953             //support self-collision if CL_SELF flag set
02954             if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
02955             {
02956                 btSoftColliders::CollideCL_SS   docollide;
02957                 docollide.Process(this,psb);
02958             }
02959             
02960         }
02961         break;
02962     case    fCollision::VF_SS:
02963         {
02964             //only self-collision for Cluster, not Vertex-Face yet
02965             if (this!=psb)
02966             {
02967                 btSoftColliders::CollideVF_SS   docollide;
02968                 /* common                   */ 
02969                 docollide.mrg=  getCollisionShape()->getMargin()+
02970                     psb->getCollisionShape()->getMargin();
02971                 /* psb0 nodes vs psb1 faces */ 
02972                 docollide.psb[0]=this;
02973                 docollide.psb[1]=psb;
02974                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
02975                     docollide.psb[1]->m_fdbvt.m_root,
02976                     docollide);
02977                 /* psb1 nodes vs psb0 faces */ 
02978                 docollide.psb[0]=psb;
02979                 docollide.psb[1]=this;
02980                 docollide.psb[0]->m_ndbvt.collideTT(    docollide.psb[0]->m_ndbvt.m_root,
02981                     docollide.psb[1]->m_fdbvt.m_root,
02982                     docollide);
02983             }
02984         }
02985         break;
02986     default:
02987         {
02988             
02989         }
02990     }
02991 }
02992 
02993 
02994 
02995 void btSoftBody::setWindVelocity( const btVector3 &velocity )
02996 {
02997     m_windVelocity = velocity;
02998 }
02999 
03000 
03001 const btVector3& btSoftBody::getWindVelocity()
03002 {
03003     return m_windVelocity;
03004 }
03005 
03006 
03007 
03008 int btSoftBody::calculateSerializeBufferSize()  const
03009 {
03010     int sz = sizeof(btSoftBodyData);
03011     return sz;
03012 }
03013 
03015 const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
03016 {
03017     btSoftBodyData* sbd = (btSoftBodyData*) dataBuffer;
03018 
03019     btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
03020 
03021     btHashMap<btHashPtr,int>    m_nodeIndexMap;
03022 
03023     sbd->m_numMaterials = m_materials.size();
03024     sbd->m_materials = sbd->m_numMaterials? (SoftBodyMaterialData**) serializer->getUniquePointer((void*)&m_materials): 0;
03025 
03026     if (sbd->m_materials)
03027     {
03028         int sz = sizeof(SoftBodyMaterialData*);
03029         int numElem = sbd->m_numMaterials;
03030         btChunk* chunk = serializer->allocate(sz,numElem);
03031         //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
03032         SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
03033         for (int i=0;i<numElem;i++,memPtr++)
03034         {
03035             btSoftBody::Material* mat = m_materials[i];
03036             *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
03037             if (!serializer->findPointer(mat))
03038             {
03039                 //serialize it here
03040                 btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData),1);
03041                 SoftBodyMaterialData* memPtr = (SoftBodyMaterialData*)chunk->m_oldPtr;
03042                 memPtr->m_flags = mat->m_flags;
03043                 memPtr->m_angularStiffness = mat->m_kAST;
03044                 memPtr->m_linearStiffness = mat->m_kLST;
03045                 memPtr->m_volumeStiffness = mat->m_kVST;
03046                 serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_SBMATERIAL_CODE,mat);
03047             }
03048         }
03049         serializer->finalizeChunk(chunk,"SoftBodyMaterialData",BT_ARRAY_CODE,(void*) &m_materials);
03050     }
03051 
03052 
03053     
03054 
03055     sbd->m_numNodes = m_nodes.size();
03056     sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes): 0;
03057     if (sbd->m_nodes)
03058     {
03059         int sz = sizeof(SoftBodyNodeData);
03060         int numElem = sbd->m_numNodes;
03061         btChunk* chunk = serializer->allocate(sz,numElem);
03062         SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
03063         for (int i=0;i<numElem;i++,memPtr++)
03064         {
03065             m_nodes[i].m_f.serializeFloat( memPtr->m_accumulatedForce);
03066             memPtr->m_area = m_nodes[i].m_area;
03067             memPtr->m_attach = m_nodes[i].m_battach;
03068             memPtr->m_inverseMass = m_nodes[i].m_im;
03069             memPtr->m_material = m_nodes[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_nodes[i].m_material):0;
03070             m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
03071             m_nodes[i].m_x.serializeFloat(memPtr->m_position);
03072             m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
03073             m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
03074             m_nodeIndexMap.insert(&m_nodes[i],i);
03075         }
03076         serializer->finalizeChunk(chunk,"SoftBodyNodeData",BT_SBNODE_CODE,(void*) &m_nodes);
03077     }
03078 
03079     sbd->m_numLinks = m_links.size();
03080     sbd->m_links = sbd->m_numLinks? (SoftBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
03081     if (sbd->m_links)
03082     {
03083         int sz = sizeof(SoftBodyLinkData);
03084         int numElem = sbd->m_numLinks;
03085         btChunk* chunk = serializer->allocate(sz,numElem);
03086         SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
03087         for (int i=0;i<numElem;i++,memPtr++)
03088         {
03089             memPtr->m_bbending = m_links[i].m_bbending;
03090             memPtr->m_material = m_links[i].m_material? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_links[i].m_material):0;
03091             memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0]: -1;
03092             memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0]: -1;
03093             btAssert(memPtr->m_nodeIndices[0]<m_nodes.size());
03094             btAssert(memPtr->m_nodeIndices[1]<m_nodes.size());
03095             memPtr->m_restLength = m_links[i].m_rl;
03096         }
03097         serializer->finalizeChunk(chunk,"SoftBodyLinkData",BT_ARRAY_CODE,(void*) &m_links[0]);
03098 
03099     }
03100 
03101 
03102     sbd->m_numFaces = m_faces.size();
03103     sbd->m_faces = sbd->m_numFaces? (SoftBodyFaceData*) serializer->getUniquePointer((void*)&m_faces[0]):0;
03104     if (sbd->m_faces)
03105     {
03106         int sz = sizeof(SoftBodyFaceData);
03107         int numElem = sbd->m_numFaces;
03108         btChunk* chunk = serializer->allocate(sz,numElem);
03109         SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
03110         for (int i=0;i<numElem;i++,memPtr++)
03111         {
03112             memPtr->m_material = m_faces[i].m_material ?  (SoftBodyMaterialData*) serializer->getUniquePointer((void*)m_faces[i].m_material): 0;
03113             m_faces[i].m_normal.serializeFloat( memPtr->m_normal);
03114             for (int j=0;j<3;j++)
03115             {
03116                 memPtr->m_nodeIndices[j] = m_faces[i].m_n[j]? m_faces[i].m_n[j] - &m_nodes[0]: -1;
03117             }
03118             memPtr->m_restArea = m_faces[i].m_ra;
03119         }
03120         serializer->finalizeChunk(chunk,"SoftBodyFaceData",BT_ARRAY_CODE,(void*) &m_faces[0]);
03121     }
03122 
03123 
03124     sbd->m_numTetrahedra = m_tetras.size();
03125     sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*) serializer->getUniquePointer((void*)&m_tetras[0]):0;
03126     if (sbd->m_tetrahedra)
03127     {
03128         int sz = sizeof(SoftBodyTetraData);
03129         int numElem = sbd->m_numTetrahedra;
03130         btChunk* chunk = serializer->allocate(sz,numElem);
03131         SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
03132         for (int i=0;i<numElem;i++,memPtr++)
03133         {
03134             for (int j=0;j<4;j++)
03135             {
03136                 m_tetras[i].m_c0[j].serializeFloat( memPtr->m_c0[j] );
03137                 memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j]? m_tetras[j].m_n[j]-&m_nodes[0] : -1;
03138             }
03139             memPtr->m_c1 = m_tetras[i].m_c1;
03140             memPtr->m_c2 = m_tetras[i].m_c2;
03141             memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*) m_tetras[i].m_material): 0;
03142             memPtr->m_restVolume = m_tetras[i].m_rv;
03143         }
03144         serializer->finalizeChunk(chunk,"SoftBodyTetraData",BT_ARRAY_CODE,(void*) &m_tetras[0]);
03145     }
03146 
03147     sbd->m_numAnchors = m_anchors.size();
03148     sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*) serializer->getUniquePointer((void*)&m_anchors[0]):0;
03149     if (sbd->m_anchors)
03150     {
03151         int sz = sizeof(SoftRigidAnchorData);
03152         int numElem = sbd->m_numAnchors;
03153         btChunk* chunk = serializer->allocate(sz,numElem);
03154         SoftRigidAnchorData* memPtr = (SoftRigidAnchorData*)chunk->m_oldPtr;
03155         for (int i=0;i<numElem;i++,memPtr++)
03156         {
03157             m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
03158             m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
03159             memPtr->m_c2 = m_anchors[i].m_c2;
03160             m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
03161             memPtr->m_nodeIndex = m_anchors[i].m_node? m_anchors[i].m_node-&m_nodes[0]: -1;
03162             
03163             memPtr->m_rigidBody = m_anchors[i].m_body? (btRigidBodyData*)  serializer->getUniquePointer((void*)m_anchors[i].m_body): 0;
03164             btAssert(memPtr->m_nodeIndex < m_nodes.size());
03165         }
03166         serializer->finalizeChunk(chunk,"SoftRigidAnchorData",BT_ARRAY_CODE,(void*) &m_anchors[0]);
03167     }
03168     
03169 
03170     sbd->m_config.m_dynamicFriction = m_cfg.kDF;
03171     sbd->m_config.m_baumgarte = m_cfg.kVCF;
03172     sbd->m_config.m_pressure = m_cfg.kPR;
03173     sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
03174     sbd->m_config.m_lift = m_cfg.kLF;
03175     sbd->m_config.m_drag = m_cfg.kDG;
03176     sbd->m_config.m_positionIterations = m_cfg.piterations;
03177     sbd->m_config.m_driftIterations = m_cfg.diterations;
03178     sbd->m_config.m_clusterIterations = m_cfg.citerations;
03179     sbd->m_config.m_velocityIterations = m_cfg.viterations;
03180     sbd->m_config.m_maxVolume = m_cfg.maxvolume;
03181     sbd->m_config.m_damping = m_cfg.kDP;
03182     sbd->m_config.m_poseMatch = m_cfg.kMT;
03183     sbd->m_config.m_collisionFlags = m_cfg.collisions;
03184     sbd->m_config.m_volume = m_cfg.kVC;
03185     sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
03186     sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
03187     sbd->m_config.m_softContactHardness = m_cfg.kSHR;
03188     sbd->m_config.m_anchorHardness = m_cfg.kAHR;
03189     sbd->m_config.m_timeScale = m_cfg.timescale;
03190     sbd->m_config.m_maxVolume = m_cfg.maxvolume;
03191     sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
03192     sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
03193     sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
03194     sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
03195     sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
03196     sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
03197         
03198     //pose for shape matching
03199     {
03200         sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
03201 
03202         int sz = sizeof(SoftBodyPoseData);
03203         btChunk* chunk = serializer->allocate(sz,1);
03204         SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
03205         
03206         m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
03207         memPtr->m_bframe = m_pose.m_bframe;
03208         memPtr->m_bvolume = m_pose.m_bvolume;
03209         m_pose.m_com.serializeFloat(memPtr->m_com);
03210         
03211         memPtr->m_numPositions = m_pose.m_pos.size();
03212         memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]): 0;
03213         if (memPtr->m_numPositions)
03214         {
03215             int numElem = memPtr->m_numPositions;
03216             int sz = sizeof(btVector3Data);
03217             btChunk* chunk = serializer->allocate(sz,numElem);
03218             btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
03219             for (int i=0;i<numElem;i++,memPtr++)
03220             {
03221                 m_pose.m_pos[i].serializeFloat(*memPtr);
03222             }
03223             serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_pose.m_pos[0]);
03224         }
03225         memPtr->m_restVolume = m_pose.m_volume;
03226         m_pose.m_rot.serializeFloat(memPtr->m_rot);
03227         m_pose.m_scl.serializeFloat(memPtr->m_scale);
03228 
03229         memPtr->m_numWeigts = m_pose.m_wgh.size();
03230         memPtr->m_weights = memPtr->m_numWeigts? (float*) serializer->getUniquePointer((void*) &m_pose.m_wgh[0]) : 0;
03231         if (memPtr->m_numWeigts)
03232         {
03233             
03234             int numElem = memPtr->m_numWeigts;
03235             int sz = sizeof(float);
03236             btChunk* chunk = serializer->allocate(sz,numElem);
03237             float* memPtr = (float*) chunk->m_oldPtr;
03238             for (int i=0;i<numElem;i++,memPtr++)
03239             {
03240                 *memPtr = m_pose.m_wgh[i];
03241             }
03242             serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_pose.m_wgh[0]);
03243         }
03244 
03245         serializer->finalizeChunk(chunk,"SoftBodyPoseData",BT_ARRAY_CODE,(void*)&m_pose);
03246     }
03247 
03248     //clusters for convex-cluster collision detection
03249 
03250     sbd->m_numClusters = m_clusters.size();
03251     sbd->m_clusters = sbd->m_numClusters? (SoftBodyClusterData*) serializer->getUniquePointer((void*)m_clusters[0]) : 0;
03252     if (sbd->m_numClusters)
03253     {
03254         int numElem = sbd->m_numClusters;
03255         int sz = sizeof(SoftBodyClusterData);
03256         btChunk* chunk = serializer->allocate(sz,numElem);
03257         SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
03258         for (int i=0;i<numElem;i++,memPtr++)
03259         {
03260             memPtr->m_adamping= m_clusters[i]->m_adamping;
03261             m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
03262             memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
03263             memPtr->m_collide = m_clusters[i]->m_collide;
03264             m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
03265             memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
03266             m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
03267             m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
03268             m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
03269             memPtr->m_idmass = m_clusters[i]->m_idmass;
03270             memPtr->m_imass = m_clusters[i]->m_imass;
03271             m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
03272             memPtr->m_ldamping = m_clusters[i]->m_ldamping;
03273             m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
03274             m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
03275             memPtr->m_matching = m_clusters[i]->m_matching;
03276             memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
03277             memPtr->m_ndamping = m_clusters[i]->m_ndamping;
03278             memPtr->m_ldamping = m_clusters[i]->m_ldamping;
03279             memPtr->m_adamping = m_clusters[i]->m_adamping;
03280             memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
03281 
03282             memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
03283             memPtr->m_numMasses = m_clusters[i]->m_masses.size();
03284             memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
03285 
03286             memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
03287             m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
03288             m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
03289             memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
03290 
03291             
03292 
03293             memPtr->m_framerefs = memPtr->m_numFrameRefs? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
03294             if (memPtr->m_framerefs)
03295             {
03296                 int numElem = memPtr->m_numFrameRefs;
03297                 int sz = sizeof(btVector3FloatData);
03298                 btChunk* chunk = serializer->allocate(sz,numElem);
03299                 btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
03300                 for (int j=0;j<numElem;j++,memPtr++)
03301                 {
03302                     m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
03303                 }
03304                 serializer->finalizeChunk(chunk,"btVector3FloatData",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_framerefs[0]);
03305             }
03306             
03307             memPtr->m_masses = memPtr->m_numMasses ? (float*) serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]): 0;
03308             if (memPtr->m_masses)
03309             {
03310                 int numElem = memPtr->m_numMasses;
03311                 int sz = sizeof(float);
03312                 btChunk* chunk = serializer->allocate(sz,numElem);
03313                 float* memPtr = (float*) chunk->m_oldPtr;
03314                 for (int j=0;j<numElem;j++,memPtr++)
03315                 {
03316                     *memPtr = m_clusters[i]->m_masses[j];
03317                 }
03318                 serializer->finalizeChunk(chunk,"float",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_masses[0]);
03319             }
03320 
03321             memPtr->m_nodeIndices  = memPtr->m_numNodes ? (int*) serializer->getUniquePointer((void*) &m_clusters[i]->m_nodes) : 0;
03322             if (memPtr->m_nodeIndices )
03323             {
03324                 int numElem = memPtr->m_numMasses;
03325                 int sz = sizeof(int);
03326                 btChunk* chunk = serializer->allocate(sz,numElem);
03327                 int* memPtr = (int*) chunk->m_oldPtr;
03328                 for (int j=0;j<numElem;j++,memPtr++)
03329                 {
03330                     int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
03331                     btAssert(indexPtr);
03332                     *memPtr = *indexPtr;
03333                 }
03334                 serializer->finalizeChunk(chunk,"int",BT_ARRAY_CODE,(void*)&m_clusters[i]->m_nodes);
03335             }
03336         }
03337         serializer->finalizeChunk(chunk,"SoftBodyClusterData",BT_ARRAY_CODE,(void*)m_clusters[0]);
03338 
03339     }
03340     
03341 
03342     
03343     sbd->m_numJoints = m_joints.size();
03344     sbd->m_joints = m_joints.size()? (btSoftBodyJointData*) serializer->getUniquePointer((void*)&m_joints[0]) : 0;
03345 
03346     if (sbd->m_joints)
03347     {
03348         int sz = sizeof(btSoftBodyJointData);
03349         int numElem = m_joints.size();
03350         btChunk* chunk = serializer->allocate(sz,numElem);
03351         btSoftBodyJointData* memPtr = (btSoftBodyJointData*)chunk->m_oldPtr;
03352 
03353         for (int i=0;i<numElem;i++,memPtr++)
03354         {
03355             memPtr->m_jointType = (int)m_joints[i]->Type();
03356             m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
03357             m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
03358             memPtr->m_cfm = m_joints[i]->m_cfm;
03359             memPtr->m_erp = m_joints[i]->m_erp;
03360             memPtr->m_split = m_joints[i]->m_split;
03361             memPtr->m_delete = m_joints[i]->m_delete;
03362             
03363             for (int j=0;j<4;j++)
03364             {
03365                 memPtr->m_relPosition[0].m_floats[j] = 0.f;
03366                 memPtr->m_relPosition[1].m_floats[j] = 0.f;
03367             }
03368             memPtr->m_bodyA = 0;
03369             memPtr->m_bodyB = 0;
03370             if (m_joints[i]->m_bodies[0].m_soft)
03371             {
03372                 memPtr->m_bodyAtype = BT_JOINT_SOFT_BODY_CLUSTER;
03373                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
03374             }
03375             if (m_joints[i]->m_bodies[0].m_collisionObject)
03376             {
03377                 memPtr->m_bodyAtype = BT_JOINT_COLLISION_OBJECT;
03378                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
03379             }
03380             if (m_joints[i]->m_bodies[0].m_rigid)
03381             {
03382                 memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
03383                 memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
03384             }
03385 
03386             if (m_joints[i]->m_bodies[1].m_soft)
03387             {
03388                 memPtr->m_bodyBtype = BT_JOINT_SOFT_BODY_CLUSTER;
03389                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
03390             }
03391             if (m_joints[i]->m_bodies[1].m_collisionObject)
03392             {
03393                 memPtr->m_bodyBtype = BT_JOINT_COLLISION_OBJECT;
03394                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
03395             }
03396             if (m_joints[i]->m_bodies[1].m_rigid)
03397             {
03398                 memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
03399                 memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
03400             }
03401         }
03402         serializer->finalizeChunk(chunk,"btSoftBodyJointData",BT_ARRAY_CODE,(void*) &m_joints[0]);
03403     }
03404 
03405 
03406     return btSoftBodyDataName;
03407 }
03408