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