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