00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00016
00017 #include "btSoftBodyInternals.h"
00018
00019
00020 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
00021 :m_worldInfo(worldInfo)
00022 {
00023
00024 m_internalType = CO_SOFT_BODY;
00025 m_cfg.aeromodel = eAeroModel::V_Point;
00026 m_cfg.kVCF = 1;
00027 m_cfg.kDG = 0;
00028 m_cfg.kLF = 0;
00029 m_cfg.kDP = 0;
00030 m_cfg.kPR = 0;
00031 m_cfg.kVC = 0;
00032 m_cfg.kDF = (btScalar)0.2;
00033 m_cfg.kMT = 0;
00034 m_cfg.kCHR = (btScalar)1.0;
00035 m_cfg.kKHR = (btScalar)0.1;
00036 m_cfg.kSHR = (btScalar)1.0;
00037 m_cfg.kAHR = (btScalar)0.7;
00038 m_cfg.kSRHR_CL = (btScalar)0.1;
00039 m_cfg.kSKHR_CL = (btScalar)1;
00040 m_cfg.kSSHR_CL = (btScalar)0.5;
00041 m_cfg.kSR_SPLT_CL = (btScalar)0.5;
00042 m_cfg.kSK_SPLT_CL = (btScalar)0.5;
00043 m_cfg.kSS_SPLT_CL = (btScalar)0.5;
00044 m_cfg.maxvolume = (btScalar)1;
00045 m_cfg.timescale = 1;
00046 m_cfg.viterations = 0;
00047 m_cfg.piterations = 1;
00048 m_cfg.diterations = 0;
00049 m_cfg.citerations = 4;
00050 m_cfg.collisions = fCollision::Default;
00051 m_pose.m_bvolume = false;
00052 m_pose.m_bframe = false;
00053 m_pose.m_volume = 0;
00054 m_pose.m_com = btVector3(0,0,0);
00055 m_pose.m_rot.setIdentity();
00056 m_pose.m_scl.setIdentity();
00057 m_tag = 0;
00058 m_timeacc = 0;
00059 m_bUpdateRtCst = true;
00060 m_bounds[0] = btVector3(0,0,0);
00061 m_bounds[1] = btVector3(0,0,0);
00062 m_worldTransform.setIdentity();
00063 setSolver(eSolverPresets::Positions);
00064
00065 Material* pm=appendMaterial();
00066 pm->m_kLST = 1;
00067 pm->m_kAST = 1;
00068 pm->m_kVST = 1;
00069 pm->m_flags = fMaterial::Default;
00070
00072 m_collisionShape = new btSoftBodyCollisionShape(this);
00073 m_collisionShape->setMargin(0.25);
00074
00075 const btScalar margin=getCollisionShape()->getMargin();
00076 m_nodes.resize(node_count);
00077 for(int i=0,ni=node_count;i<ni;++i)
00078 {
00079 Node& n=m_nodes[i];
00080 ZeroInitialize(n);
00081 n.m_x = x?*x++:btVector3(0,0,0);
00082 n.m_q = n.m_x;
00083 n.m_im = m?*m++:1;
00084 n.m_im = n.m_im>0?1/n.m_im:0;
00085 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
00086 n.m_material= pm;
00087 }
00088 updateBounds();
00089
00090 m_initialWorldTransform.setIdentity();
00091 }
00092
00093
00094 btSoftBody::~btSoftBody()
00095 {
00096
00097 delete m_collisionShape;
00098 int i;
00099
00100 releaseClusters();
00101 for(i=0;i<m_materials.size();++i)
00102 btAlignedFree(m_materials[i]);
00103 for(i=0;i<m_joints.size();++i)
00104 btAlignedFree(m_joints[i]);
00105 }
00106
00107
00108 bool btSoftBody::checkLink(int node0,int node1) const
00109 {
00110 return(checkLink(&m_nodes[node0],&m_nodes[node1]));
00111 }
00112
00113
00114 bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
00115 {
00116 const Node* n[]={node0,node1};
00117 for(int i=0,ni=m_links.size();i<ni;++i)
00118 {
00119 const Link& l=m_links[i];
00120 if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
00121 (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
00122 {
00123 return(true);
00124 }
00125 }
00126 return(false);
00127 }
00128
00129
00130 bool btSoftBody::checkFace(int node0,int node1,int node2) const
00131 {
00132 const Node* n[]={ &m_nodes[node0],
00133 &m_nodes[node1],
00134 &m_nodes[node2]};
00135 for(int i=0,ni=m_faces.size();i<ni;++i)
00136 {
00137 const Face& f=m_faces[i];
00138 int c=0;
00139 for(int j=0;j<3;++j)
00140 {
00141 if( (f.m_n[j]==n[0])||
00142 (f.m_n[j]==n[1])||
00143 (f.m_n[j]==n[2])) c|=1<<j; else break;
00144 }
00145 if(c==7) return(true);
00146 }
00147 return(false);
00148 }
00149
00150
00151 btSoftBody::Material* btSoftBody::appendMaterial()
00152 {
00153 Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
00154 if(m_materials.size()>0)
00155 *pm=*m_materials[0];
00156 else
00157 ZeroInitialize(*pm);
00158 m_materials.push_back(pm);
00159 return(pm);
00160 }
00161
00162
00163 void btSoftBody::appendNote( const char* text,
00164 const btVector3& o,
00165 const btVector4& c,
00166 Node* n0,
00167 Node* n1,
00168 Node* n2,
00169 Node* n3)
00170 {
00171 Note n;
00172 ZeroInitialize(n);
00173 n.m_rank = 0;
00174 n.m_text = text;
00175 n.m_offset = o;
00176 n.m_coords[0] = c.x();
00177 n.m_coords[1] = c.y();
00178 n.m_coords[2] = c.z();
00179 n.m_coords[3] = c.w();
00180 n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
00181 n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
00182 n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
00183 n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
00184 m_notes.push_back(n);
00185 }
00186
00187
00188 void btSoftBody::appendNote( const char* text,
00189 const btVector3& o,
00190 Node* feature)
00191 {
00192 appendNote(text,o,btVector4(1,0,0,0),feature);
00193 }
00194
00195
00196 void btSoftBody::appendNote( const char* text,
00197 const btVector3& o,
00198 Link* feature)
00199 {
00200 static const btScalar w=1/(btScalar)2;
00201 appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
00202 feature->m_n[1]);
00203 }
00204
00205
00206 void btSoftBody::appendNote( const char* text,
00207 const btVector3& o,
00208 Face* feature)
00209 {
00210 static const btScalar w=1/(btScalar)3;
00211 appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
00212 feature->m_n[1],
00213 feature->m_n[2]);
00214 }
00215
00216
00217 void btSoftBody::appendNode( const btVector3& x,btScalar m)
00218 {
00219 if(m_nodes.capacity()==m_nodes.size())
00220 {
00221 pointersToIndices();
00222 m_nodes.reserve(m_nodes.size()*2+1);
00223 indicesToPointers();
00224 }
00225 const btScalar margin=getCollisionShape()->getMargin();
00226 m_nodes.push_back(Node());
00227 Node& n=m_nodes[m_nodes.size()-1];
00228 ZeroInitialize(n);
00229 n.m_x = x;
00230 n.m_q = n.m_x;
00231 n.m_im = m>0?1/m:0;
00232 n.m_material = m_materials[0];
00233 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
00234 }
00235
00236
00237 void btSoftBody::appendLink(int model,Material* mat)
00238 {
00239 Link l;
00240 if(model>=0)
00241 l=m_links[model];
00242 else
00243 { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
00244 m_links.push_back(l);
00245 }
00246
00247
00248 void btSoftBody::appendLink( int node0,
00249 int node1,
00250 Material* mat,
00251 bool bcheckexist)
00252 {
00253 appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
00254 }
00255
00256
00257 void btSoftBody::appendLink( Node* node0,
00258 Node* node1,
00259 Material* mat,
00260 bool bcheckexist)
00261 {
00262 if((!bcheckexist)||(!checkLink(node0,node1)))
00263 {
00264 appendLink(-1,mat);
00265 Link& l=m_links[m_links.size()-1];
00266 l.m_n[0] = node0;
00267 l.m_n[1] = node1;
00268 l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
00269 m_bUpdateRtCst=true;
00270 }
00271 }
00272
00273
00274 void btSoftBody::appendFace(int model,Material* mat)
00275 {
00276 Face f;
00277 if(model>=0)
00278 { f=m_faces[model]; }
00279 else
00280 { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
00281 m_faces.push_back(f);
00282 }
00283
00284
00285 void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
00286 {
00287 if (node0==node1)
00288 return;
00289 if (node1==node2)
00290 return;
00291 if (node2==node0)
00292 return;
00293
00294 appendFace(-1,mat);
00295 Face& f=m_faces[m_faces.size()-1];
00296 btAssert(node0!=node1);
00297 btAssert(node1!=node2);
00298 btAssert(node2!=node0);
00299 f.m_n[0] = &m_nodes[node0];
00300 f.m_n[1] = &m_nodes[node1];
00301 f.m_n[2] = &m_nodes[node2];
00302 f.m_ra = AreaOf( f.m_n[0]->m_x,
00303 f.m_n[1]->m_x,
00304 f.m_n[2]->m_x);
00305 m_bUpdateRtCst=true;
00306 }
00307
00308
00309 void btSoftBody::appendTetra(int model,Material* mat)
00310 {
00311 Tetra t;
00312 if(model>=0)
00313 t=m_tetras[model];
00314 else
00315 { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
00316 m_tetras.push_back(t);
00317 }
00318
00319
00320 void btSoftBody::appendTetra(int node0,
00321 int node1,
00322 int node2,
00323 int node3,
00324 Material* mat)
00325 {
00326 appendTetra(-1,mat);
00327 Tetra& t=m_tetras[m_tetras.size()-1];
00328 t.m_n[0] = &m_nodes[node0];
00329 t.m_n[1] = &m_nodes[node1];
00330 t.m_n[2] = &m_nodes[node2];
00331 t.m_n[3] = &m_nodes[node3];
00332 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);
00333 m_bUpdateRtCst=true;
00334 }
00335
00336
00337 void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies)
00338 {
00339 if (disableCollisionBetweenLinkedBodies)
00340 {
00341 if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
00342 {
00343 m_collisionDisabledObjects.push_back(body);
00344 }
00345 }
00346
00347 Anchor a;
00348 a.m_node = &m_nodes[node];
00349 a.m_body = body;
00350 a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
00351 a.m_node->m_battach = 1;
00352 m_anchors.push_back(a);
00353 }
00354
00355
00356 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
00357 {
00358 LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
00359 pj->m_bodies[0] = body0;
00360 pj->m_bodies[1] = body1;
00361 pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position;
00362 pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position;
00363 pj->m_cfm = specs.cfm;
00364 pj->m_erp = specs.erp;
00365 pj->m_split = specs.split;
00366 m_joints.push_back(pj);
00367 }
00368
00369
00370 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
00371 {
00372 appendLinearJoint(specs,m_clusters[0],body);
00373 }
00374
00375
00376 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
00377 {
00378 appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
00379 }
00380
00381
00382 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
00383 {
00384 AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
00385 pj->m_bodies[0] = body0;
00386 pj->m_bodies[1] = body1;
00387 pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
00388 pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
00389 pj->m_cfm = specs.cfm;
00390 pj->m_erp = specs.erp;
00391 pj->m_split = specs.split;
00392 pj->m_icontrol = specs.icontrol;
00393 m_joints.push_back(pj);
00394 }
00395
00396
00397 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
00398 {
00399 appendAngularJoint(specs,m_clusters[0],body);
00400 }
00401
00402
00403 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
00404 {
00405 appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
00406 }
00407
00408
00409 void btSoftBody::addForce(const btVector3& force)
00410 {
00411 for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
00412 }
00413
00414
00415 void btSoftBody::addForce(const btVector3& force,int node)
00416 {
00417 Node& n=m_nodes[node];
00418 if(n.m_im>0)
00419 {
00420 n.m_f += force;
00421 }
00422 }
00423
00424
00425 void btSoftBody::addVelocity(const btVector3& velocity)
00426 {
00427 for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
00428 }
00429
00430
00431 void btSoftBody::setVelocity( const btVector3& velocity)
00432 {
00433 for(int i=0,ni=m_nodes.size();i<ni;++i)
00434 {
00435 Node& n=m_nodes[i];
00436 if(n.m_im>0)
00437 {
00438 n.m_v = velocity;
00439 }
00440 }
00441 }
00442
00443
00444
00445 void btSoftBody::addVelocity(const btVector3& velocity,int node)
00446 {
00447 Node& n=m_nodes[node];
00448 if(n.m_im>0)
00449 {
00450 n.m_v += velocity;
00451 }
00452 }
00453
00454
00455 void btSoftBody::setMass(int node,btScalar mass)
00456 {
00457 m_nodes[node].m_im=mass>0?1/mass:0;
00458 m_bUpdateRtCst=true;
00459 }
00460
00461
00462 btScalar btSoftBody::getMass(int node) const
00463 {
00464 return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
00465 }
00466
00467
00468 btScalar btSoftBody::getTotalMass() const
00469 {
00470 btScalar mass=0;
00471 for(int i=0;i<m_nodes.size();++i)
00472 {
00473 mass+=getMass(i);
00474 }
00475 return(mass);
00476 }
00477
00478
00479 void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
00480 {
00481 int i;
00482
00483 if(fromfaces)
00484 {
00485
00486 for(i=0;i<m_nodes.size();++i)
00487 {
00488 m_nodes[i].m_im=0;
00489 }
00490 for(i=0;i<m_faces.size();++i)
00491 {
00492 const Face& f=m_faces[i];
00493 const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
00494 f.m_n[1]->m_x,
00495 f.m_n[2]->m_x);
00496 for(int j=0;j<3;++j)
00497 {
00498 f.m_n[j]->m_im+=twicearea;
00499 }
00500 }
00501 for( i=0;i<m_nodes.size();++i)
00502 {
00503 m_nodes[i].m_im=1/m_nodes[i].m_im;
00504 }
00505 }
00506 const btScalar tm=getTotalMass();
00507 const btScalar itm=1/tm;
00508 for( i=0;i<m_nodes.size();++i)
00509 {
00510 m_nodes[i].m_im/=itm*mass;
00511 }
00512 m_bUpdateRtCst=true;
00513 }
00514
00515
00516 void btSoftBody::setTotalDensity(btScalar density)
00517 {
00518 setTotalMass(getVolume()*density,true);
00519 }
00520
00521
00522 void btSoftBody::setVolumeMass(btScalar mass)
00523 {
00524 btAlignedObjectArray<btScalar> ranks;
00525 ranks.resize(m_nodes.size(),0);
00526 int i;
00527
00528 for(i=0;i<m_nodes.size();++i)
00529 {
00530 m_nodes[i].m_im=0;
00531 }
00532 for(i=0;i<m_tetras.size();++i)
00533 {
00534 const Tetra& t=m_tetras[i];
00535 for(int j=0;j<4;++j)
00536 {
00537 t.m_n[j]->m_im+=btFabs(t.m_rv);
00538 ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
00539 }
00540 }
00541 for( i=0;i<m_nodes.size();++i)
00542 {
00543 if(m_nodes[i].m_im>0)
00544 {
00545 m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
00546 }
00547 }
00548 setTotalMass(mass,false);
00549 }
00550
00551
00552 void btSoftBody::setVolumeDensity(btScalar density)
00553 {
00554 btScalar volume=0;
00555 for(int i=0;i<m_tetras.size();++i)
00556 {
00557 const Tetra& t=m_tetras[i];
00558 for(int j=0;j<4;++j)
00559 {
00560 volume+=btFabs(t.m_rv);
00561 }
00562 }
00563 setVolumeMass(volume*density/6);
00564 }
00565
00566
00567 void btSoftBody::transform(const btTransform& trs)
00568 {
00569 const btScalar margin=getCollisionShape()->getMargin();
00570 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
00571
00572 for(int i=0,ni=m_nodes.size();i<ni;++i)
00573 {
00574 Node& n=m_nodes[i];
00575 n.m_x=trs*n.m_x;
00576 n.m_q=trs*n.m_q;
00577 n.m_n=trs.getBasis()*n.m_n;
00578 vol = btDbvtVolume::FromCR(n.m_x,margin);
00579
00580 m_ndbvt.update(n.m_leaf,vol);
00581 }
00582 updateNormals();
00583 updateBounds();
00584 updateConstants();
00585 m_initialWorldTransform = trs;
00586 }
00587
00588
00589 void btSoftBody::translate(const btVector3& trs)
00590 {
00591 btTransform t;
00592 t.setIdentity();
00593 t.setOrigin(trs);
00594 transform(t);
00595 }
00596
00597
00598 void btSoftBody::rotate( const btQuaternion& rot)
00599 {
00600 btTransform t;
00601 t.setIdentity();
00602 t.setRotation(rot);
00603 transform(t);
00604 }
00605
00606
00607 void btSoftBody::scale(const btVector3& scl)
00608 {
00609 const btScalar margin=getCollisionShape()->getMargin();
00610 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
00611
00612 for(int i=0,ni=m_nodes.size();i<ni;++i)
00613 {
00614 Node& n=m_nodes[i];
00615 n.m_x*=scl;
00616 n.m_q*=scl;
00617 vol = btDbvtVolume::FromCR(n.m_x,margin);
00618 m_ndbvt.update(n.m_leaf,vol);
00619 }
00620 updateNormals();
00621 updateBounds();
00622 updateConstants();
00623 }
00624
00625
00626 void btSoftBody::setPose(bool bvolume,bool bframe)
00627 {
00628 m_pose.m_bvolume = bvolume;
00629 m_pose.m_bframe = bframe;
00630 int i,ni;
00631
00632
00633 const btScalar omass=getTotalMass();
00634 const btScalar kmass=omass*m_nodes.size()*1000;
00635 btScalar tmass=omass;
00636 m_pose.m_wgh.resize(m_nodes.size());
00637 for(i=0,ni=m_nodes.size();i<ni;++i)
00638 {
00639 if(m_nodes[i].m_im<=0) tmass+=kmass;
00640 }
00641 for( i=0,ni=m_nodes.size();i<ni;++i)
00642 {
00643 Node& n=m_nodes[i];
00644 m_pose.m_wgh[i]= n.m_im>0 ?
00645 1/(m_nodes[i].m_im*tmass) :
00646 kmass/tmass;
00647 }
00648
00649 const btVector3 com=evaluateCom();
00650 m_pose.m_pos.resize(m_nodes.size());
00651 for( i=0,ni=m_nodes.size();i<ni;++i)
00652 {
00653 m_pose.m_pos[i]=m_nodes[i].m_x-com;
00654 }
00655 m_pose.m_volume = bvolume?getVolume():0;
00656 m_pose.m_com = com;
00657 m_pose.m_rot.setIdentity();
00658 m_pose.m_scl.setIdentity();
00659
00660 m_pose.m_aqq[0] =
00661 m_pose.m_aqq[1] =
00662 m_pose.m_aqq[2] = btVector3(0,0,0);
00663 for( i=0,ni=m_nodes.size();i<ni;++i)
00664 {
00665 const btVector3& q=m_pose.m_pos[i];
00666 const btVector3 mq=m_pose.m_wgh[i]*q;
00667 m_pose.m_aqq[0]+=mq.x()*q;
00668 m_pose.m_aqq[1]+=mq.y()*q;
00669 m_pose.m_aqq[2]+=mq.z()*q;
00670 }
00671 m_pose.m_aqq=m_pose.m_aqq.inverse();
00672 updateConstants();
00673 }
00674
00675
00676 btScalar btSoftBody::getVolume() const
00677 {
00678 btScalar vol=0;
00679 if(m_nodes.size()>0)
00680 {
00681 int i,ni;
00682
00683 const btVector3 org=m_nodes[0].m_x;
00684 for(i=0,ni=m_faces.size();i<ni;++i)
00685 {
00686 const Face& f=m_faces[i];
00687 vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
00688 }
00689 vol/=(btScalar)6;
00690 }
00691 return(vol);
00692 }
00693
00694
00695 int btSoftBody::clusterCount() const
00696 {
00697 return(m_clusters.size());
00698 }
00699
00700
00701 btVector3 btSoftBody::clusterCom(const Cluster* cluster)
00702 {
00703 btVector3 com(0,0,0);
00704 for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
00705 {
00706 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
00707 }
00708 return(com*cluster->m_imass);
00709 }
00710
00711
00712 btVector3 btSoftBody::clusterCom(int cluster) const
00713 {
00714 return(clusterCom(m_clusters[cluster]));
00715 }
00716
00717
00718 btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
00719 {
00720 return(cluster->m_lv+btCross(cluster->m_av,rpos));
00721 }
00722
00723
00724 void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
00725 {
00726 const btVector3 li=cluster->m_imass*impulse;
00727 const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
00728 cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
00729 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
00730 cluster->m_nvimpulses++;
00731 }
00732
00733
00734 void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
00735 {
00736 const btVector3 li=cluster->m_imass*impulse;
00737 const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
00738 cluster->m_dimpulses[0]+=li;
00739 cluster->m_dimpulses[1]+=ai;
00740 cluster->m_ndimpulses++;
00741 }
00742
00743
00744 void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
00745 {
00746 if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity);
00747 if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift);
00748 }
00749
00750
00751 void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
00752 {
00753 const btVector3 ai=cluster->m_invwi*impulse;
00754 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
00755 cluster->m_nvimpulses++;
00756 }
00757
00758
00759 void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
00760 {
00761 const btVector3 ai=cluster->m_invwi*impulse;
00762 cluster->m_dimpulses[1]+=ai;
00763 cluster->m_ndimpulses++;
00764 }
00765
00766
00767 void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
00768 {
00769 if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity);
00770 if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift);
00771 }
00772
00773
00774 void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
00775 {
00776 cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
00777 cluster->m_ndimpulses++;
00778 }
00779
00780 struct NodeLinks
00781 {
00782 btAlignedObjectArray<int> m_links;
00783 };
00784
00785
00786
00787
00788 int btSoftBody::generateBendingConstraints(int distance,Material* mat)
00789 {
00790 int i,j;
00791
00792 if(distance>1)
00793 {
00794
00795 const int n=m_nodes.size();
00796 const unsigned inf=(~(unsigned)0)>>1;
00797 unsigned* adj=new unsigned[n*n];
00798
00799
00800 #define IDX(_x_,_y_) ((_y_)*n+(_x_))
00801 for(j=0;j<n;++j)
00802 {
00803 for(i=0;i<n;++i)
00804 {
00805 if(i!=j)
00806 {
00807 adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
00808 }
00809 else
00810 {
00811 adj[IDX(i,j)]=adj[IDX(j,i)]=0;
00812 }
00813 }
00814 }
00815 for( i=0;i<m_links.size();++i)
00816 {
00817 const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
00818 const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
00819 adj[IDX(ia,ib)]=1;
00820 adj[IDX(ib,ia)]=1;
00821 }
00822
00823
00824
00825 if (distance == 2)
00826 {
00827
00828 btAlignedObjectArray<NodeLinks> nodeLinks;
00829
00830
00831
00832 nodeLinks.resize(m_nodes.size());
00833
00834 for( i=0;i<m_links.size();++i)
00835 {
00836 const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
00837 const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
00838 if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
00839 nodeLinks[ia].m_links.push_back(ib);
00840
00841 if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
00842 nodeLinks[ib].m_links.push_back(ia);
00843 }
00844 for (int ii=0;ii<nodeLinks.size();ii++)
00845 {
00846 int i=ii;
00847
00848 for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
00849 {
00850 int k = nodeLinks[ii].m_links[jj];
00851 for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
00852 {
00853 int j = nodeLinks[k].m_links[kk];
00854 if (i!=j)
00855 {
00856 const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
00857 btAssert(sum==2);
00858 if(adj[IDX(i,j)]>sum)
00859 {
00860 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
00861 }
00862 }
00863
00864 }
00865 }
00866 }
00867 } else
00868 {
00870 for(int k=0;k<n;++k)
00871 {
00872 for(j=0;j<n;++j)
00873 {
00874 for(i=j+1;i<n;++i)
00875 {
00876 const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
00877 if(adj[IDX(i,j)]>sum)
00878 {
00879 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
00880 }
00881 }
00882 }
00883 }
00884 }
00885
00886
00887
00888 int nlinks=0;
00889 for(j=0;j<n;++j)
00890 {
00891 for(i=j+1;i<n;++i)
00892 {
00893 if(adj[IDX(i,j)]==(unsigned)distance)
00894 {
00895 appendLink(i,j,mat);
00896 m_links[m_links.size()-1].m_bbending=1;
00897 ++nlinks;
00898 }
00899 }
00900 }
00901 delete[] adj;
00902 return(nlinks);
00903 }
00904 return(0);
00905 }
00906
00907
00908 void btSoftBody::randomizeConstraints()
00909 {
00910 unsigned long seed=243703;
00911 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
00912 int i,ni;
00913
00914 for(i=0,ni=m_links.size();i<ni;++i)
00915 {
00916 btSwap(m_links[i],m_links[NEXTRAND%ni]);
00917 }
00918 for(i=0,ni=m_faces.size();i<ni;++i)
00919 {
00920 btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
00921 }
00922 #undef NEXTRAND
00923 }
00924
00925
00926 void btSoftBody::releaseCluster(int index)
00927 {
00928 Cluster* c=m_clusters[index];
00929 if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
00930 c->~Cluster();
00931 btAlignedFree(c);
00932 m_clusters.remove(c);
00933 }
00934
00935
00936 void btSoftBody::releaseClusters()
00937 {
00938 while(m_clusters.size()>0) releaseCluster(0);
00939 }
00940
00941
00942 int btSoftBody::generateClusters(int k,int maxiterations)
00943 {
00944 int i;
00945 releaseClusters();
00946 m_clusters.resize(btMin(k,m_nodes.size()));
00947 for(i=0;i<m_clusters.size();++i)
00948 {
00949 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
00950 m_clusters[i]->m_collide= true;
00951 }
00952 k=m_clusters.size();
00953 if(k>0)
00954 {
00955
00956 btAlignedObjectArray<btVector3> centers;
00957 btVector3 cog(0,0,0);
00958 int i;
00959 for(i=0;i<m_nodes.size();++i)
00960 {
00961 cog+=m_nodes[i].m_x;
00962 m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
00963 }
00964 cog/=(btScalar)m_nodes.size();
00965 centers.resize(k,cog);
00966
00967 const btScalar slope=16;
00968 bool changed;
00969 int iterations=0;
00970 do {
00971 const btScalar w=2-btMin<btScalar>(1,iterations/slope);
00972 changed=false;
00973 iterations++;
00974 int i;
00975
00976 for(i=0;i<k;++i)
00977 {
00978 btVector3 c(0,0,0);
00979 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
00980 {
00981 c+=m_clusters[i]->m_nodes[j]->m_x;
00982 }
00983 if(m_clusters[i]->m_nodes.size())
00984 {
00985 c /= (btScalar)m_clusters[i]->m_nodes.size();
00986 c = centers[i]+(c-centers[i])*w;
00987 changed |= ((c-centers[i]).length2()>SIMD_EPSILON);
00988 centers[i] = c;
00989 m_clusters[i]->m_nodes.resize(0);
00990 }
00991 }
00992 for(i=0;i<m_nodes.size();++i)
00993 {
00994 const btVector3 nx=m_nodes[i].m_x;
00995 int kbest=0;
00996 btScalar kdist=ClusterMetric(centers[0],nx);
00997 for(int j=1;j<k;++j)
00998 {
00999 const btScalar d=ClusterMetric(centers[j],nx);
01000 if(d<kdist)
01001 {
01002 kbest=j;
01003 kdist=d;
01004 }
01005 }
01006 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
01007 }
01008 } while(changed&&(iterations<maxiterations));
01009
01010 btAlignedObjectArray<int> cids;
01011 cids.resize(m_nodes.size(),-1);
01012 for(i=0;i<m_clusters.size();++i)
01013 {
01014 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
01015 {
01016 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
01017 }
01018 }
01019 for(i=0;i<m_faces.size();++i)
01020 {
01021 const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
01022 int(m_faces[i].m_n[1]-&m_nodes[0]),
01023 int(m_faces[i].m_n[2]-&m_nodes[0])};
01024 for(int j=0;j<3;++j)
01025 {
01026 const int cid=cids[idx[j]];
01027 for(int q=1;q<3;++q)
01028 {
01029 const int kid=idx[(j+q)%3];
01030 if(cids[kid]!=cid)
01031 {
01032 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
01033 {
01034 m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
01035 }
01036 }
01037 }
01038 }
01039 }
01040
01041 if(m_clusters.size()>1)
01042 {
01043 Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01044 pmaster->m_collide = false;
01045 pmaster->m_nodes.reserve(m_nodes.size());
01046 for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
01047 m_clusters.push_back(pmaster);
01048 btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
01049 }
01050
01051 for(i=0;i<m_clusters.size();++i)
01052 {
01053 if(m_clusters[i]->m_nodes.size()==0)
01054 {
01055 releaseCluster(i--);
01056 }
01057 }
01058 } else
01059 {
01060
01061 if (m_tetras.size())
01062 {
01063 m_clusters.resize(m_tetras.size());
01064 for(i=0;i<m_clusters.size();++i)
01065 {
01066 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01067 m_clusters[i]->m_collide= true;
01068 }
01069 for (i=0;i<m_tetras.size();i++)
01070 {
01071 for (int j=0;j<4;j++)
01072 {
01073 m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
01074 }
01075 }
01076
01077 } else
01078 {
01079 m_clusters.resize(m_faces.size());
01080 for(i=0;i<m_clusters.size();++i)
01081 {
01082 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
01083 m_clusters[i]->m_collide= true;
01084 }
01085
01086 for(i=0;i<m_faces.size();++i)
01087 {
01088 for(int j=0;j<3;++j)
01089 {
01090 m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
01091 }
01092 }
01093 }
01094 }
01095
01096 if (m_clusters.size())
01097 {
01098 initializeClusters();
01099 updateClusters();
01100
01101
01102
01103 m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
01104 {
01105 for (int c0=0;c0<m_clusters.size();c0++)
01106 {
01107 m_clusters[c0]->m_clusterIndex=c0;
01108 for (int c1=0;c1<m_clusters.size();c1++)
01109 {
01110
01111 bool connected=false;
01112 Cluster* cla = m_clusters[c0];
01113 Cluster* clb = m_clusters[c1];
01114 for (int i=0;!connected&&i<cla->m_nodes.size();i++)
01115 {
01116 for (int j=0;j<clb->m_nodes.size();j++)
01117 {
01118 if (cla->m_nodes[i] == clb->m_nodes[j])
01119 {
01120 connected=true;
01121 break;
01122 }
01123 }
01124 }
01125 m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
01126 }
01127 }
01128 }
01129 }
01130
01131 return(m_clusters.size());
01132 }
01133
01134
01135 void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
01136 {
01137 const Node* nbase = &m_nodes[0];
01138 int ncount = m_nodes.size();
01139 btSymMatrix<int> edges(ncount,-2);
01140 int newnodes=0;
01141 int i,j,k,ni;
01142
01143
01144 for(i=0;i<m_links.size();++i)
01145 {
01146 Link& l=m_links[i];
01147 if(l.m_bbending)
01148 {
01149 if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
01150 {
01151 btSwap(m_links[i],m_links[m_links.size()-1]);
01152 m_links.pop_back();--i;
01153 }
01154 }
01155 }
01156
01157 for(i=0;i<m_links.size();++i)
01158 {
01159 Link& l=m_links[i];
01160 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
01161 }
01162 for(i=0;i<m_faces.size();++i)
01163 {
01164 Face& f=m_faces[i];
01165 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
01166 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
01167 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
01168 }
01169
01170 for(i=0;i<ncount;++i)
01171 {
01172 for(j=i+1;j<ncount;++j)
01173 {
01174 if(edges(i,j)==-1)
01175 {
01176 Node& a=m_nodes[i];
01177 Node& b=m_nodes[j];
01178 const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
01179 if(t>0)
01180 {
01181 const btVector3 x=Lerp(a.m_x,b.m_x,t);
01182 const btVector3 v=Lerp(a.m_v,b.m_v,t);
01183 btScalar m=0;
01184 if(a.m_im>0)
01185 {
01186 if(b.m_im>0)
01187 {
01188 const btScalar ma=1/a.m_im;
01189 const btScalar mb=1/b.m_im;
01190 const btScalar mc=Lerp(ma,mb,t);
01191 const btScalar f=(ma+mb)/(ma+mb+mc);
01192 a.m_im=1/(ma*f);
01193 b.m_im=1/(mb*f);
01194 m=mc*f;
01195 }
01196 else
01197 { a.m_im/=0.5;m=1/a.m_im; }
01198 }
01199 else
01200 {
01201 if(b.m_im>0)
01202 { b.m_im/=0.5;m=1/b.m_im; }
01203 else
01204 m=0;
01205 }
01206 appendNode(x,m);
01207 edges(i,j)=m_nodes.size()-1;
01208 m_nodes[edges(i,j)].m_v=v;
01209 ++newnodes;
01210 }
01211 }
01212 }
01213 }
01214 nbase=&m_nodes[0];
01215
01216 for(i=0,ni=m_links.size();i<ni;++i)
01217 {
01218 Link& feat=m_links[i];
01219 const int idx[]={ int(feat.m_n[0]-nbase),
01220 int(feat.m_n[1]-nbase)};
01221 if((idx[0]<ncount)&&(idx[1]<ncount))
01222 {
01223 const int ni=edges(idx[0],idx[1]);
01224 if(ni>0)
01225 {
01226 appendLink(i);
01227 Link* pft[]={ &m_links[i],
01228 &m_links[m_links.size()-1]};
01229 pft[0]->m_n[0]=&m_nodes[idx[0]];
01230 pft[0]->m_n[1]=&m_nodes[ni];
01231 pft[1]->m_n[0]=&m_nodes[ni];
01232 pft[1]->m_n[1]=&m_nodes[idx[1]];
01233 }
01234 }
01235 }
01236
01237 for(i=0;i<m_faces.size();++i)
01238 {
01239 const Face& feat=m_faces[i];
01240 const int idx[]={ int(feat.m_n[0]-nbase),
01241 int(feat.m_n[1]-nbase),
01242 int(feat.m_n[2]-nbase)};
01243 for(j=2,k=0;k<3;j=k++)
01244 {
01245 if((idx[j]<ncount)&&(idx[k]<ncount))
01246 {
01247 const int ni=edges(idx[j],idx[k]);
01248 if(ni>0)
01249 {
01250 appendFace(i);
01251 const int l=(k+1)%3;
01252 Face* pft[]={ &m_faces[i],
01253 &m_faces[m_faces.size()-1]};
01254 pft[0]->m_n[0]=&m_nodes[idx[l]];
01255 pft[0]->m_n[1]=&m_nodes[idx[j]];
01256 pft[0]->m_n[2]=&m_nodes[ni];
01257 pft[1]->m_n[0]=&m_nodes[ni];
01258 pft[1]->m_n[1]=&m_nodes[idx[k]];
01259 pft[1]->m_n[2]=&m_nodes[idx[l]];
01260 appendLink(ni,idx[l],pft[0]->m_material);
01261 --i;break;
01262 }
01263 }
01264 }
01265 }
01266
01267 if(cut)
01268 {
01269 btAlignedObjectArray<int> cnodes;
01270 const int pcount=ncount;
01271 int i;
01272 ncount=m_nodes.size();
01273 cnodes.resize(ncount,0);
01274
01275 for(i=0;i<ncount;++i)
01276 {
01277 const btVector3 x=m_nodes[i].m_x;
01278 if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
01279 {
01280 const btVector3 v=m_nodes[i].m_v;
01281 btScalar m=getMass(i);
01282 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
01283 appendNode(x,m);
01284 cnodes[i]=m_nodes.size()-1;
01285 m_nodes[cnodes[i]].m_v=v;
01286 }
01287 }
01288 nbase=&m_nodes[0];
01289
01290 for(i=0,ni=m_links.size();i<ni;++i)
01291 {
01292 const int id[]={ int(m_links[i].m_n[0]-nbase),
01293 int(m_links[i].m_n[1]-nbase)};
01294 int todetach=0;
01295 if(cnodes[id[0]]&&cnodes[id[1]])
01296 {
01297 appendLink(i);
01298 todetach=m_links.size()-1;
01299 }
01300 else
01301 {
01302 if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
01303 (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
01304 todetach=i;
01305 }
01306 if(todetach)
01307 {
01308 Link& l=m_links[todetach];
01309 for(int j=0;j<2;++j)
01310 {
01311 int cn=cnodes[int(l.m_n[j]-nbase)];
01312 if(cn) l.m_n[j]=&m_nodes[cn];
01313 }
01314 }
01315 }
01316
01317 for(i=0,ni=m_faces.size();i<ni;++i)
01318 {
01319 Node** n= m_faces[i].m_n;
01320 if( (ifn->Eval(n[0]->m_x)<accurary)&&
01321 (ifn->Eval(n[1]->m_x)<accurary)&&
01322 (ifn->Eval(n[2]->m_x)<accurary))
01323 {
01324 for(int j=0;j<3;++j)
01325 {
01326 int cn=cnodes[int(n[j]-nbase)];
01327 if(cn) n[j]=&m_nodes[cn];
01328 }
01329 }
01330 }
01331
01332 int nnodes=m_nodes.size();
01333 btAlignedObjectArray<int> ranks;
01334 btAlignedObjectArray<int> todelete;
01335 ranks.resize(nnodes,0);
01336 for(i=0,ni=m_links.size();i<ni;++i)
01337 {
01338 for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
01339 }
01340 for(i=0,ni=m_faces.size();i<ni;++i)
01341 {
01342 for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
01343 }
01344 for(i=0;i<m_links.size();++i)
01345 {
01346 const int id[]={ int(m_links[i].m_n[0]-nbase),
01347 int(m_links[i].m_n[1]-nbase)};
01348 const bool sg[]={ ranks[id[0]]==1,
01349 ranks[id[1]]==1};
01350 if(sg[0]||sg[1])
01351 {
01352 --ranks[id[0]];
01353 --ranks[id[1]];
01354 btSwap(m_links[i],m_links[m_links.size()-1]);
01355 m_links.pop_back();--i;
01356 }
01357 }
01358 #if 0
01359 for(i=nnodes-1;i>=0;--i)
01360 {
01361 if(!ranks[i]) todelete.push_back(i);
01362 }
01363 if(todelete.size())
01364 {
01365 btAlignedObjectArray<int>& map=ranks;
01366 for(int i=0;i<nnodes;++i) map[i]=i;
01367 PointersToIndices(this);
01368 for(int i=0,ni=todelete.size();i<ni;++i)
01369 {
01370 int j=todelete[i];
01371 int& a=map[j];
01372 int& b=map[--nnodes];
01373 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
01374 btSwap(m_nodes[a],m_nodes[b]);
01375 j=a;a=b;b=j;
01376 }
01377 IndicesToPointers(this,&map[0]);
01378 m_nodes.resize(nnodes);
01379 }
01380 #endif
01381 }
01382 m_bUpdateRtCst=true;
01383 }
01384
01385
01386 bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
01387 {
01388 return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
01389 }
01390
01391
01392 bool btSoftBody::cutLink(int node0,int node1,btScalar position)
01393 {
01394 bool done=false;
01395 int i,ni;
01396 const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
01397 const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
01398 const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
01399 const btScalar m=1;
01400 appendNode(x,m);
01401 appendNode(x,m);
01402 Node* pa=&m_nodes[node0];
01403 Node* pb=&m_nodes[node1];
01404 Node* pn[2]={ &m_nodes[m_nodes.size()-2],
01405 &m_nodes[m_nodes.size()-1]};
01406 pn[0]->m_v=v;
01407 pn[1]->m_v=v;
01408 for(i=0,ni=m_links.size();i<ni;++i)
01409 {
01410 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
01411 if(mtch!=-1)
01412 {
01413 appendLink(i);
01414 Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
01415 pft[0]->m_n[1]=pn[mtch];
01416 pft[1]->m_n[0]=pn[1-mtch];
01417 done=true;
01418 }
01419 }
01420 for(i=0,ni=m_faces.size();i<ni;++i)
01421 {
01422 for(int k=2,l=0;l<3;k=l++)
01423 {
01424 const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
01425 if(mtch!=-1)
01426 {
01427 appendFace(i);
01428 Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
01429 pft[0]->m_n[l]=pn[mtch];
01430 pft[1]->m_n[k]=pn[1-mtch];
01431 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
01432 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
01433 }
01434 }
01435 }
01436 if(!done)
01437 {
01438 m_ndbvt.remove(pn[0]->m_leaf);
01439 m_ndbvt.remove(pn[1]->m_leaf);
01440 m_nodes.pop_back();
01441 m_nodes.pop_back();
01442 }
01443 return(done);
01444 }
01445
01446
01447 bool btSoftBody::rayTest(const btVector3& rayFrom,
01448 const btVector3& rayTo,
01449 sRayCast& results)
01450 {
01451 if(m_faces.size()&&m_fdbvt.empty())
01452 initializeFaceTree();
01453
01454 results.body = this;
01455 results.fraction = 1.f;
01456 results.feature = eFeature::None;
01457 results.index = -1;
01458
01459 return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
01460 }
01461
01462
01463 void btSoftBody::setSolver(eSolverPresets::_ preset)
01464 {
01465 m_cfg.m_vsequence.clear();
01466 m_cfg.m_psequence.clear();
01467 m_cfg.m_dsequence.clear();
01468 switch(preset)
01469 {
01470 case eSolverPresets::Positions:
01471 m_cfg.m_psequence.push_back(ePSolver::Anchors);
01472 m_cfg.m_psequence.push_back(ePSolver::RContacts);
01473 m_cfg.m_psequence.push_back(ePSolver::SContacts);
01474 m_cfg.m_psequence.push_back(ePSolver::Linear);
01475 break;
01476 case eSolverPresets::Velocities:
01477 m_cfg.m_vsequence.push_back(eVSolver::Linear);
01478
01479 m_cfg.m_psequence.push_back(ePSolver::Anchors);
01480 m_cfg.m_psequence.push_back(ePSolver::RContacts);
01481 m_cfg.m_psequence.push_back(ePSolver::SContacts);
01482
01483 m_cfg.m_dsequence.push_back(ePSolver::Linear);
01484 break;
01485 }
01486 }
01487
01488
01489 void btSoftBody::predictMotion(btScalar dt)
01490 {
01491 int i,ni;
01492
01493
01494 if(m_bUpdateRtCst)
01495 {
01496 m_bUpdateRtCst=false;
01497 updateConstants();
01498 m_fdbvt.clear();
01499 if(m_cfg.collisions&fCollision::VF_SS)
01500 {
01501 initializeFaceTree();
01502 }
01503 }
01504
01505
01506 m_sst.sdt = dt*m_cfg.timescale;
01507 m_sst.isdt = 1/m_sst.sdt;
01508 m_sst.velmrg = m_sst.sdt*3;
01509 m_sst.radmrg = getCollisionShape()->getMargin();
01510 m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
01511
01512 addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
01513 applyForces();
01514
01515 for(i=0,ni=m_nodes.size();i<ni;++i)
01516 {
01517 Node& n=m_nodes[i];
01518 n.m_q = n.m_x;
01519 n.m_v += n.m_f*n.m_im*m_sst.sdt;
01520 n.m_x += n.m_v*m_sst.sdt;
01521 n.m_f = btVector3(0,0,0);
01522 }
01523
01524 updateClusters();
01525
01526 updateBounds();
01527
01528 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
01529 for(i=0,ni=m_nodes.size();i<ni;++i)
01530 {
01531 Node& n=m_nodes[i];
01532 vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
01533 m_ndbvt.update( n.m_leaf,
01534 vol,
01535 n.m_v*m_sst.velmrg,
01536 m_sst.updmrg);
01537 }
01538
01539 if(!m_fdbvt.empty())
01540 {
01541 for(int i=0;i<m_faces.size();++i)
01542 {
01543 Face& f=m_faces[i];
01544 const btVector3 v=( f.m_n[0]->m_v+
01545 f.m_n[1]->m_v+
01546 f.m_n[2]->m_v)/3;
01547 vol = VolumeOf(f,m_sst.radmrg);
01548 m_fdbvt.update( f.m_leaf,
01549 vol,
01550 v*m_sst.velmrg,
01551 m_sst.updmrg);
01552 }
01553 }
01554
01555 updatePose();
01556
01557 if(m_pose.m_bframe&&(m_cfg.kMT>0))
01558 {
01559 const btMatrix3x3 posetrs=m_pose.m_rot;
01560 for(int i=0,ni=m_nodes.size();i<ni;++i)
01561 {
01562 Node& n=m_nodes[i];
01563 if(n.m_im>0)
01564 {
01565 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
01566 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
01567 }
01568 }
01569 }
01570
01571 m_rcontacts.resize(0);
01572 m_scontacts.resize(0);
01573
01574 m_ndbvt.optimizeIncremental(1);
01575 m_fdbvt.optimizeIncremental(1);
01576 m_cdbvt.optimizeIncremental(1);
01577 }
01578
01579
01580 void btSoftBody::solveConstraints()
01581 {
01582
01583 applyClusters(false);
01584
01585
01586 int i,ni;
01587
01588 for(i=0,ni=m_links.size();i<ni;++i)
01589 {
01590 Link& l=m_links[i];
01591 l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
01592 l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
01593 }
01594
01595 for(i=0,ni=m_anchors.size();i<ni;++i)
01596 {
01597 Anchor& a=m_anchors[i];
01598 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
01599 a.m_c0 = ImpulseMatrix( m_sst.sdt,
01600 a.m_node->m_im,
01601 a.m_body->getInvMass(),
01602 a.m_body->getInvInertiaTensorWorld(),
01603 ra);
01604 a.m_c1 = ra;
01605 a.m_c2 = m_sst.sdt*a.m_node->m_im;
01606 a.m_body->activate();
01607 }
01608
01609 if(m_cfg.viterations>0)
01610 {
01611
01612 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
01613 {
01614 for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
01615 {
01616 getSolver(m_cfg.m_vsequence[iseq])(this,1);
01617 }
01618 }
01619
01620 for(i=0,ni=m_nodes.size();i<ni;++i)
01621 {
01622 Node& n=m_nodes[i];
01623 n.m_x = n.m_q+n.m_v*m_sst.sdt;
01624 }
01625 }
01626
01627 if(m_cfg.piterations>0)
01628 {
01629 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
01630 {
01631 const btScalar ti=isolve/(btScalar)m_cfg.piterations;
01632 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
01633 {
01634 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
01635 }
01636 }
01637 const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
01638 for(i=0,ni=m_nodes.size();i<ni;++i)
01639 {
01640 Node& n=m_nodes[i];
01641 n.m_v = (n.m_x-n.m_q)*vc;
01642 n.m_f = btVector3(0,0,0);
01643 }
01644 }
01645
01646 if(m_cfg.diterations>0)
01647 {
01648 const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
01649 for(i=0,ni=m_nodes.size();i<ni;++i)
01650 {
01651 Node& n=m_nodes[i];
01652 n.m_q = n.m_x;
01653 }
01654 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
01655 {
01656 for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
01657 {
01658 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
01659 }
01660 }
01661 for(int i=0,ni=m_nodes.size();i<ni;++i)
01662 {
01663 Node& n=m_nodes[i];
01664 n.m_v += (n.m_x-n.m_q)*vcf;
01665 }
01666 }
01667
01668 dampClusters();
01669 applyClusters(true);
01670 }
01671
01672
01673 void btSoftBody::staticSolve(int iterations)
01674 {
01675 for(int isolve=0;isolve<iterations;++isolve)
01676 {
01677 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
01678 {
01679 getSolver(m_cfg.m_psequence[iseq])(this,1,0);
01680 }
01681 }
01682 }
01683
01684
01685 void btSoftBody::solveCommonConstraints(btSoftBody** ,int ,int )
01686 {
01688 }
01689
01690
01691 void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
01692 {
01693 const int nb=bodies.size();
01694 int iterations=0;
01695 int i;
01696
01697 for(i=0;i<nb;++i)
01698 {
01699 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
01700 }
01701 for(i=0;i<nb;++i)
01702 {
01703 bodies[i]->prepareClusters(iterations);
01704 }
01705 for(i=0;i<iterations;++i)
01706 {
01707 const btScalar sor=1;
01708 for(int j=0;j<nb;++j)
01709 {
01710 bodies[j]->solveClusters(sor);
01711 }
01712 }
01713 for(i=0;i<nb;++i)
01714 {
01715 bodies[i]->cleanupClusters();
01716 }
01717 }
01718
01719
01720 void btSoftBody::integrateMotion()
01721 {
01722
01723 updateNormals();
01724 }
01725
01726
01727 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
01728 {
01729 m_rayFrom = rayFrom;
01730 m_rayNormalizedDirection = (rayTo-rayFrom);
01731 m_rayTo = rayTo;
01732 m_mint = mxt;
01733 m_face = 0;
01734 m_tests = 0;
01735 }
01736
01737
01738 void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
01739 {
01740 btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
01741 const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection,
01742 f.m_n[0]->m_x,
01743 f.m_n[1]->m_x,
01744 f.m_n[2]->m_x,
01745 m_mint);
01746 if((t>0)&&(t<m_mint))
01747 {
01748 m_mint=t;m_face=&f;
01749 }
01750 ++m_tests;
01751 }
01752
01753
01754 btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
01755 const btVector3& rayTo,
01756 const btVector3& rayNormalizedDirection,
01757 const btVector3& a,
01758 const btVector3& b,
01759 const btVector3& c,
01760 btScalar maxt)
01761 {
01762 static const btScalar ceps=-SIMD_EPSILON*10;
01763 static const btScalar teps=SIMD_EPSILON*10;
01764
01765 const btVector3 n=btCross(b-a,c-a);
01766 const btScalar d=btDot(a,n);
01767 const btScalar den=btDot(rayNormalizedDirection,n);
01768 if(!btFuzzyZero(den))
01769 {
01770 const btScalar num=btDot(rayFrom,n)-d;
01771 const btScalar t=-num/den;
01772 if((t>teps)&&(t<maxt))
01773 {
01774 const btVector3 hit=rayFrom+rayNormalizedDirection*t;
01775 if( (btDot(n,btCross(a-hit,b-hit))>ceps) &&
01776 (btDot(n,btCross(b-hit,c-hit))>ceps) &&
01777 (btDot(n,btCross(c-hit,a-hit))>ceps))
01778 {
01779 return(t);
01780 }
01781 }
01782 }
01783 return(-1);
01784 }
01785
01786
01787 void btSoftBody::pointersToIndices()
01788 {
01789 #define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
01790 btSoftBody::Node* base=&m_nodes[0];
01791 int i,ni;
01792
01793 for(i=0,ni=m_nodes.size();i<ni;++i)
01794 {
01795 if(m_nodes[i].m_leaf)
01796 {
01797 m_nodes[i].m_leaf->data=*(void**)&i;
01798 }
01799 }
01800 for(i=0,ni=m_links.size();i<ni;++i)
01801 {
01802 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
01803 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
01804 }
01805 for(i=0,ni=m_faces.size();i<ni;++i)
01806 {
01807 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
01808 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
01809 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
01810 if(m_faces[i].m_leaf)
01811 {
01812 m_faces[i].m_leaf->data=*(void**)&i;
01813 }
01814 }
01815 for(i=0,ni=m_anchors.size();i<ni;++i)
01816 {
01817 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
01818 }
01819 for(i=0,ni=m_notes.size();i<ni;++i)
01820 {
01821 for(int j=0;j<m_notes[i].m_rank;++j)
01822 {
01823 m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
01824 }
01825 }
01826 #undef PTR2IDX
01827 }
01828
01829
01830 void btSoftBody::indicesToPointers(const int* map)
01831 {
01832 #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
01833 (&(_b_)[(((char*)_p_)-(char*)0)])
01834 btSoftBody::Node* base=&m_nodes[0];
01835 int i,ni;
01836
01837 for(i=0,ni=m_nodes.size();i<ni;++i)
01838 {
01839 if(m_nodes[i].m_leaf)
01840 {
01841 m_nodes[i].m_leaf->data=&m_nodes[i];
01842 }
01843 }
01844 for(i=0,ni=m_links.size();i<ni;++i)
01845 {
01846 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
01847 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
01848 }
01849 for(i=0,ni=m_faces.size();i<ni;++i)
01850 {
01851 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
01852 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
01853 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
01854 if(m_faces[i].m_leaf)
01855 {
01856 m_faces[i].m_leaf->data=&m_faces[i];
01857 }
01858 }
01859 for(i=0,ni=m_anchors.size();i<ni;++i)
01860 {
01861 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
01862 }
01863 for(i=0,ni=m_notes.size();i<ni;++i)
01864 {
01865 for(int j=0;j<m_notes[i].m_rank;++j)
01866 {
01867 m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
01868 }
01869 }
01870 #undef IDX2PTR
01871 }
01872
01873
01874 int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
01875 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
01876 {
01877 int cnt=0;
01878 if(bcountonly||m_fdbvt.empty())
01879 {
01880 btVector3 dir = rayTo-rayFrom;
01881 dir.normalize();
01882
01883 for(int i=0,ni=m_faces.size();i<ni;++i)
01884 {
01885 const btSoftBody::Face& f=m_faces[i];
01886
01887 const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
01888 f.m_n[0]->m_x,
01889 f.m_n[1]->m_x,
01890 f.m_n[2]->m_x,
01891 mint);
01892 if(t>0)
01893 {
01894 ++cnt;
01895 if(!bcountonly)
01896 {
01897 feature=btSoftBody::eFeature::Face;
01898 index=i;
01899 mint=t;
01900 }
01901 }
01902 }
01903 }
01904 else
01905 {
01906 RayFromToCaster collider(rayFrom,rayTo,mint);
01907
01908 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
01909 if(collider.m_face)
01910 {
01911 mint=collider.m_mint;
01912 feature=btSoftBody::eFeature::Face;
01913 index=(int)(collider.m_face-&m_faces[0]);
01914 cnt=1;
01915 }
01916 }
01917 return(cnt);
01918 }
01919
01920
01921 void btSoftBody::initializeFaceTree()
01922 {
01923 m_fdbvt.clear();
01924 for(int i=0;i<m_faces.size();++i)
01925 {
01926 Face& f=m_faces[i];
01927 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
01928 }
01929 }
01930
01931
01932 btVector3 btSoftBody::evaluateCom() const
01933 {
01934 btVector3 com(0,0,0);
01935 if(m_pose.m_bframe)
01936 {
01937 for(int i=0,ni=m_nodes.size();i<ni;++i)
01938 {
01939 com+=m_nodes[i].m_x*m_pose.m_wgh[i];
01940 }
01941 }
01942 return(com);
01943 }
01944
01945
01946 bool btSoftBody::checkContact( btCollisionObject* colObj,
01947 const btVector3& x,
01948 btScalar margin,
01949 btSoftBody::sCti& cti) const
01950 {
01951 btVector3 nrm;
01952 btCollisionShape* shp=colObj->getCollisionShape();
01953 btRigidBody* tmpRigid = btRigidBody::upcast(colObj);
01954 const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform();
01955 btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
01956 shp,
01957 nrm,
01958 margin);
01959 if(dst<0)
01960 {
01961 cti.m_colObj = colObj;
01962 cti.m_normal = wtr.getBasis()*nrm;
01963 cti.m_offset = -btDot( cti.m_normal,
01964 x-cti.m_normal*dst);
01965 return(true);
01966 }
01967 return(false);
01968 }
01969
01970
01971 void btSoftBody::updateNormals()
01972 {
01973 const btVector3 zv(0,0,0);
01974 int i,ni;
01975
01976 for(i=0,ni=m_nodes.size();i<ni;++i)
01977 {
01978 m_nodes[i].m_n=zv;
01979 }
01980 for(i=0,ni=m_faces.size();i<ni;++i)
01981 {
01982 btSoftBody::Face& f=m_faces[i];
01983 const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
01984 f.m_n[2]->m_x-f.m_n[0]->m_x);
01985 f.m_normal=n.normalized();
01986 f.m_n[0]->m_n+=n;
01987 f.m_n[1]->m_n+=n;
01988 f.m_n[2]->m_n+=n;
01989 }
01990 for(i=0,ni=m_nodes.size();i<ni;++i)
01991 {
01992 btScalar len = m_nodes[i].m_n.length();
01993 if (len>SIMD_EPSILON)
01994 m_nodes[i].m_n /= len;
01995 }
01996 }
01997
01998
01999 void btSoftBody::updateBounds()
02000 {
02001 if(m_ndbvt.m_root)
02002 {
02003 const btVector3& mins=m_ndbvt.m_root->volume.Mins();
02004 const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
02005 const btScalar csm=getCollisionShape()->getMargin();
02006 const btVector3 mrg=btVector3( csm,
02007 csm,
02008 csm)*1;
02009 m_bounds[0]=mins-mrg;
02010 m_bounds[1]=maxs+mrg;
02011 if(0!=getBroadphaseHandle())
02012 {
02013 m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
02014 m_bounds[0],
02015 m_bounds[1],
02016 m_worldInfo->m_dispatcher);
02017 }
02018 }
02019 else
02020 {
02021 m_bounds[0]=
02022 m_bounds[1]=btVector3(0,0,0);
02023 }
02024 }
02025
02026
02027
02028 void btSoftBody::updatePose()
02029 {
02030 if(m_pose.m_bframe)
02031 {
02032 btSoftBody::Pose& pose=m_pose;
02033 const btVector3 com=evaluateCom();
02034
02035 pose.m_com = com;
02036
02037 btMatrix3x3 Apq;
02038 const btScalar eps=SIMD_EPSILON;
02039 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
02040 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
02041 for(int i=0,ni=m_nodes.size();i<ni;++i)
02042 {
02043 const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
02044 const btVector3& b=pose.m_pos[i];
02045 Apq[0]+=a.x()*b;
02046 Apq[1]+=a.y()*b;
02047 Apq[2]+=a.z()*b;
02048 }
02049 btMatrix3x3 r,s;
02050 PolarDecompose(Apq,r,s);
02051 pose.m_rot=r;
02052 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
02053 if(m_cfg.maxvolume>1)
02054 {
02055 const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
02056 1,m_cfg.maxvolume);
02057 pose.m_scl=Mul(pose.m_scl,idet);
02058 }
02059
02060 }
02061 }
02062
02063
02064 void btSoftBody::updateConstants()
02065 {
02066 int i,ni;
02067
02068
02069 for(i=0,ni=m_links.size();i<ni;++i)
02070 {
02071 Link& l=m_links[i];
02072 Material& m=*l.m_material;
02073 l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
02074 l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
02075 l.m_c1 = l.m_rl*l.m_rl;
02076 }
02077
02078 for(i=0,ni=m_faces.size();i<ni;++i)
02079 {
02080 Face& f=m_faces[i];
02081 f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
02082 }
02083
02084 btAlignedObjectArray<int> counts;
02085 counts.resize(m_nodes.size(),0);
02086 for(i=0,ni=m_nodes.size();i<ni;++i)
02087 {
02088 m_nodes[i].m_area = 0;
02089 }
02090 for(i=0,ni=m_faces.size();i<ni;++i)
02091 {
02092 btSoftBody::Face& f=m_faces[i];
02093 for(int j=0;j<3;++j)
02094 {
02095 const int index=(int)(f.m_n[j]-&m_nodes[0]);
02096 counts[index]++;
02097 f.m_n[j]->m_area+=btFabs(f.m_ra);
02098 }
02099 }
02100 for(i=0,ni=m_nodes.size();i<ni;++i)
02101 {
02102 if(counts[i]>0)
02103 m_nodes[i].m_area/=(btScalar)counts[i];
02104 else
02105 m_nodes[i].m_area=0;
02106 }
02107 }
02108
02109
02110 void btSoftBody::initializeClusters()
02111 {
02112 int i;
02113
02114 for( i=0;i<m_clusters.size();++i)
02115 {
02116 Cluster& c=*m_clusters[i];
02117 c.m_imass=0;
02118 c.m_masses.resize(c.m_nodes.size());
02119 for(int j=0;j<c.m_nodes.size();++j)
02120 {
02121 if (c.m_nodes[j]->m_im==0)
02122 {
02123 c.m_containsAnchor = true;
02124 c.m_masses[j] = BT_LARGE_FLOAT;
02125 } else
02126 {
02127 c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im;
02128 }
02129 c.m_imass += c.m_masses[j];
02130 }
02131 c.m_imass = btScalar(1.)/c.m_imass;
02132 c.m_com = btSoftBody::clusterCom(&c);
02133 c.m_lv = btVector3(0,0,0);
02134 c.m_av = btVector3(0,0,0);
02135 c.m_leaf = 0;
02136
02137 btMatrix3x3& ii=c.m_locii;
02138 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
02139 {
02140 int i,ni;
02141
02142 for(i=0,ni=c.m_nodes.size();i<ni;++i)
02143 {
02144 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
02145 const btVector3 q=k*k;
02146 const btScalar m=c.m_masses[i];
02147 ii[0][0] += m*(q[1]+q[2]);
02148 ii[1][1] += m*(q[0]+q[2]);
02149 ii[2][2] += m*(q[0]+q[1]);
02150 ii[0][1] -= m*k[0]*k[1];
02151 ii[0][2] -= m*k[0]*k[2];
02152 ii[1][2] -= m*k[1]*k[2];
02153 }
02154 }
02155 ii[1][0]=ii[0][1];
02156 ii[2][0]=ii[0][2];
02157 ii[2][1]=ii[1][2];
02158
02159 ii = ii.inverse();
02160
02161
02162 c.m_framexform.setIdentity();
02163 c.m_framexform.setOrigin(c.m_com);
02164 c.m_framerefs.resize(c.m_nodes.size());
02165 {
02166 int i;
02167 for(i=0;i<c.m_framerefs.size();++i)
02168 {
02169 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
02170 }
02171 }
02172 }
02173 }
02174
02175
02176 void btSoftBody::updateClusters()
02177 {
02178 BT_PROFILE("UpdateClusters");
02179 int i;
02180
02181 for(i=0;i<m_clusters.size();++i)
02182 {
02183 btSoftBody::Cluster& c=*m_clusters[i];
02184 const int n=c.m_nodes.size();
02185
02186 if(n)
02187 {
02188
02189 const btScalar eps=btScalar(0.0001);
02190 btMatrix3x3 m,r,s;
02191 m[0]=m[1]=m[2]=btVector3(0,0,0);
02192 m[0][0]=eps*1;
02193 m[1][1]=eps*2;
02194 m[2][2]=eps*3;
02195 c.m_com=clusterCom(&c);
02196 for(int i=0;i<c.m_nodes.size();++i)
02197 {
02198 const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
02199 const btVector3& b=c.m_framerefs[i];
02200 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
02201 }
02202 PolarDecompose(m,r,s);
02203 c.m_framexform.setOrigin(c.m_com);
02204 c.m_framexform.setBasis(r);
02205
02206 #if 1
02207 c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
02208 #else
02209 #if 0
02210 const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
02211 const btVector3 inertia(rk,rk,rk);
02212 const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
02213 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
02214 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
02215
02216 c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
02217 #else
02218 c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
02219 for(int i=0;i<n;++i)
02220 {
02221 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
02222 const btVector3 q=k*k;
02223 const btScalar m=1/c.m_nodes[i]->m_im;
02224 c.m_invwi[0][0] += m*(q[1]+q[2]);
02225 c.m_invwi[1][1] += m*(q[0]+q[2]);
02226 c.m_invwi[2][2] += m*(q[0]+q[1]);
02227 c.m_invwi[0][1] -= m*k[0]*k[1];
02228 c.m_invwi[0][2] -= m*k[0]*k[2];
02229 c.m_invwi[1][2] -= m*k[1]*k[2];
02230 }
02231 c.m_invwi[1][0]=c.m_invwi[0][1];
02232 c.m_invwi[2][0]=c.m_invwi[0][2];
02233 c.m_invwi[2][1]=c.m_invwi[1][2];
02234 c.m_invwi=c.m_invwi.inverse();
02235 #endif
02236 #endif
02237
02238 c.m_lv=btVector3(0,0,0);
02239 c.m_av=btVector3(0,0,0);
02240 {
02241 int i;
02242
02243 for(i=0;i<n;++i)
02244 {
02245 const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
02246 c.m_lv += v;
02247 c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v);
02248 }
02249 }
02250 c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
02251 c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
02252 c.m_vimpulses[0] =
02253 c.m_vimpulses[1] = btVector3(0,0,0);
02254 c.m_dimpulses[0] =
02255 c.m_dimpulses[1] = btVector3(0,0,0);
02256 c.m_nvimpulses = 0;
02257 c.m_ndimpulses = 0;
02258
02259 if(c.m_matching>0)
02260 {
02261 for(int j=0;j<c.m_nodes.size();++j)
02262 {
02263 Node& n=*c.m_nodes[j];
02264 const btVector3 x=c.m_framexform*c.m_framerefs[j];
02265 n.m_x=Lerp(n.m_x,x,c.m_matching);
02266 }
02267 }
02268
02269 if(c.m_collide)
02270 {
02271 btVector3 mi=c.m_nodes[0]->m_x;
02272 btVector3 mx=mi;
02273 for(int j=1;j<n;++j)
02274 {
02275 mi.setMin(c.m_nodes[j]->m_x);
02276 mx.setMax(c.m_nodes[j]->m_x);
02277 }
02278 ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
02279 if(c.m_leaf)
02280 m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
02281 else
02282 c.m_leaf=m_cdbvt.insert(bounds,&c);
02283 }
02284 }
02285 }
02286
02287
02288 }
02289
02290
02291
02292
02293
02294 void btSoftBody::cleanupClusters()
02295 {
02296 for(int i=0;i<m_joints.size();++i)
02297 {
02298 m_joints[i]->Terminate(m_sst.sdt);
02299 if(m_joints[i]->m_delete)
02300 {
02301 btAlignedFree(m_joints[i]);
02302 m_joints.remove(m_joints[i--]);
02303 }
02304 }
02305 }
02306
02307
02308 void btSoftBody::prepareClusters(int iterations)
02309 {
02310 for(int i=0;i<m_joints.size();++i)
02311 {
02312 m_joints[i]->Prepare(m_sst.sdt,iterations);
02313 }
02314 }
02315
02316
02317
02318 void btSoftBody::solveClusters(btScalar sor)
02319 {
02320 for(int i=0,ni=m_joints.size();i<ni;++i)
02321 {
02322 m_joints[i]->Solve(m_sst.sdt,sor);
02323 }
02324 }
02325
02326
02327 void btSoftBody::applyClusters(bool drift)
02328 {
02329 BT_PROFILE("ApplyClusters");
02330
02331
02332 btAlignedObjectArray<btVector3> deltas;
02333 btAlignedObjectArray<btScalar> weights;
02334 deltas.resize(m_nodes.size(),btVector3(0,0,0));
02335 weights.resize(m_nodes.size(),0);
02336 int i;
02337
02338 if(drift)
02339 {
02340 for(i=0;i<m_clusters.size();++i)
02341 {
02342 Cluster& c=*m_clusters[i];
02343 if(c.m_ndimpulses)
02344 {
02345 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
02346 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
02347 }
02348 }
02349 }
02350
02351 for(i=0;i<m_clusters.size();++i)
02352 {
02353 Cluster& c=*m_clusters[i];
02354 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
02355 {
02356 const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
02357 const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
02358 for(int j=0;j<c.m_nodes.size();++j)
02359 {
02360 const int idx=int(c.m_nodes[j]-&m_nodes[0]);
02361 const btVector3& x=c.m_nodes[j]->m_x;
02362 const btScalar q=c.m_masses[j];
02363 deltas[idx] += (v+btCross(w,x-c.m_com))*q;
02364 weights[idx] += q;
02365 }
02366 }
02367 }
02368 for(i=0;i<deltas.size();++i)
02369 {
02370 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
02371 }
02372 }
02373
02374
02375 void btSoftBody::dampClusters()
02376 {
02377 int i;
02378
02379 for(i=0;i<m_clusters.size();++i)
02380 {
02381 Cluster& c=*m_clusters[i];
02382 if(c.m_ndamping>0)
02383 {
02384 for(int j=0;j<c.m_nodes.size();++j)
02385 {
02386 Node& n=*c.m_nodes[j];
02387 if(n.m_im>0)
02388 {
02389 const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
02390 if(vx.length2()<=n.m_v.length2())
02391 {
02392 n.m_v += c.m_ndamping*(vx-n.m_v);
02393 }
02394 }
02395 }
02396 }
02397 }
02398 }
02399
02400
02401 void btSoftBody::Joint::Prepare(btScalar dt,int)
02402 {
02403 m_bodies[0].activate();
02404 m_bodies[1].activate();
02405 }
02406
02407
02408 void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
02409 {
02410 static const btScalar maxdrift=4;
02411 Joint::Prepare(dt,iterations);
02412 m_rpos[0] = m_bodies[0].xform()*m_refs[0];
02413 m_rpos[1] = m_bodies[1].xform()*m_refs[1];
02414 m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
02415 m_rpos[0] -= m_bodies[0].xform().getOrigin();
02416 m_rpos[1] -= m_bodies[1].xform().getOrigin();
02417 m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
02418 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
02419 if(m_split>0)
02420 {
02421 m_sdrift = m_massmatrix*(m_drift*m_split);
02422 m_drift *= 1-m_split;
02423 }
02424 m_drift /=(btScalar)iterations;
02425 }
02426
02427
02428 void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
02429 {
02430 const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
02431 const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
02432 const btVector3 vr=va-vb;
02433 btSoftBody::Impulse impulse;
02434 impulse.m_asVelocity = 1;
02435 impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
02436 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
02437 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
02438 }
02439
02440
02441 void btSoftBody::LJoint::Terminate(btScalar dt)
02442 {
02443 if(m_split>0)
02444 {
02445 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
02446 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
02447 }
02448 }
02449
02450
02451 void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
02452 {
02453 static const btScalar maxdrift=SIMD_PI/16;
02454 m_icontrol->Prepare(this);
02455 Joint::Prepare(dt,iterations);
02456 m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
02457 m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
02458 m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0]));
02459 m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
02460 m_drift *= m_erp/dt;
02461 m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
02462 if(m_split>0)
02463 {
02464 m_sdrift = m_massmatrix*(m_drift*m_split);
02465 m_drift *= 1-m_split;
02466 }
02467 m_drift /=(btScalar)iterations;
02468 }
02469
02470
02471 void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
02472 {
02473 const btVector3 va=m_bodies[0].angularVelocity();
02474 const btVector3 vb=m_bodies[1].angularVelocity();
02475 const btVector3 vr=va-vb;
02476 const btScalar sp=btDot(vr,m_axis[0]);
02477 const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
02478 btSoftBody::Impulse impulse;
02479 impulse.m_asVelocity = 1;
02480 impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
02481 m_bodies[0].applyAImpulse(-impulse);
02482 m_bodies[1].applyAImpulse( impulse);
02483 }
02484
02485
02486 void btSoftBody::AJoint::Terminate(btScalar dt)
02487 {
02488 if(m_split>0)
02489 {
02490 m_bodies[0].applyDAImpulse(-m_sdrift);
02491 m_bodies[1].applyDAImpulse( m_sdrift);
02492 }
02493 }
02494
02495
02496 void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
02497 {
02498 Joint::Prepare(dt,iterations);
02499 const bool dodrift=(m_life==0);
02500 m_delete=(++m_life)>m_maxlife;
02501 if(dodrift)
02502 {
02503 m_drift=m_drift*m_erp/dt;
02504 if(m_split>0)
02505 {
02506 m_sdrift = m_massmatrix*(m_drift*m_split);
02507 m_drift *= 1-m_split;
02508 }
02509 m_drift/=(btScalar)iterations;
02510 }
02511 else
02512 {
02513 m_drift=m_sdrift=btVector3(0,0,0);
02514 }
02515 }
02516
02517
02518 void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
02519 {
02520 const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
02521 const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
02522 const btVector3 vrel=va-vb;
02523 const btScalar rvac=btDot(vrel,m_normal);
02524 btSoftBody::Impulse impulse;
02525 impulse.m_asVelocity = 1;
02526 impulse.m_velocity = m_drift;
02527 if(rvac<0)
02528 {
02529 const btVector3 iv=m_normal*rvac;
02530 const btVector3 fv=vrel-iv;
02531 impulse.m_velocity += iv+fv*m_friction;
02532 }
02533 impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
02534
02535 if (m_bodies[0].m_soft==m_bodies[1].m_soft)
02536 {
02537 if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
02538 (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
02539 {
02540 if (impulse.m_asVelocity)
02541 {
02542 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
02543 {
02544
02545 } else
02546 {
02547 m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
02548 m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
02549 }
02550 }
02551 }
02552 } else
02553 {
02554 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
02555 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
02556 }
02557 }
02558
02559
02560 void btSoftBody::CJoint::Terminate(btScalar dt)
02561 {
02562 if(m_split>0)
02563 {
02564 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
02565 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
02566 }
02567 }
02568
02569
02570 void btSoftBody::applyForces()
02571 {
02572
02573 BT_PROFILE("SoftBody applyForces");
02574 const btScalar dt=m_sst.sdt;
02575 const btScalar kLF=m_cfg.kLF;
02576 const btScalar kDG=m_cfg.kDG;
02577 const btScalar kPR=m_cfg.kPR;
02578 const btScalar kVC=m_cfg.kVC;
02579 const bool as_lift=kLF>0;
02580 const bool as_drag=kDG>0;
02581 const bool as_pressure=kPR!=0;
02582 const bool as_volume=kVC>0;
02583 const bool as_aero= as_lift ||
02584 as_drag ;
02585 const bool as_vaero= as_aero &&
02586 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
02587 const bool as_faero= as_aero &&
02588 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
02589 const bool use_medium= as_aero;
02590 const bool use_volume= as_pressure ||
02591 as_volume ;
02592 btScalar volume=0;
02593 btScalar ivolumetp=0;
02594 btScalar dvolumetv=0;
02595 btSoftBody::sMedium medium;
02596 if(use_volume)
02597 {
02598 volume = getVolume();
02599 ivolumetp = 1/btFabs(volume)*kPR;
02600 dvolumetv = (m_pose.m_volume-volume)*kVC;
02601 }
02602
02603 int i,ni;
02604
02605 for(i=0,ni=m_nodes.size();i<ni;++i)
02606 {
02607 btSoftBody::Node& n=m_nodes[i];
02608 if(n.m_im>0)
02609 {
02610 if(use_medium)
02611 {
02612 EvaluateMedium(m_worldInfo,n.m_x,medium);
02613
02614 if(as_vaero)
02615 {
02616 const btVector3 rel_v=n.m_v-medium.m_velocity;
02617 const btScalar rel_v2=rel_v.length2();
02618 if(rel_v2>SIMD_EPSILON)
02619 {
02620 btVector3 nrm=n.m_n;
02621
02622 switch(m_cfg.aeromodel)
02623 {
02624 case btSoftBody::eAeroModel::V_Point:
02625 nrm=NormalizeAny(rel_v);break;
02626 case btSoftBody::eAeroModel::V_TwoSided:
02627 nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
02628 default:
02629 {
02630 }
02631 }
02632 const btScalar dvn=btDot(rel_v,nrm);
02633
02634 if(dvn>0)
02635 {
02636 btVector3 force(0,0,0);
02637 const btScalar c0 = n.m_area*dvn*rel_v2/2;
02638 const btScalar c1 = c0*medium.m_density;
02639 force += nrm*(-c1*kLF);
02640 force += rel_v.normalized()*(-c1*kDG);
02641 ApplyClampedForce(n,force,dt);
02642 }
02643 }
02644 }
02645 }
02646
02647 if(as_pressure)
02648 {
02649 n.m_f += n.m_n*(n.m_area*ivolumetp);
02650 }
02651
02652 if(as_volume)
02653 {
02654 n.m_f += n.m_n*(n.m_area*dvolumetv);
02655 }
02656 }
02657 }
02658
02659 for(i=0,ni=m_faces.size();i<ni;++i)
02660 {
02661 btSoftBody::Face& f=m_faces[i];
02662 if(as_faero)
02663 {
02664 const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
02665 const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
02666 EvaluateMedium(m_worldInfo,x,medium);
02667 const btVector3 rel_v=v-medium.m_velocity;
02668 const btScalar rel_v2=rel_v.length2();
02669 if(rel_v2>SIMD_EPSILON)
02670 {
02671 btVector3 nrm=f.m_normal;
02672
02673 switch(m_cfg.aeromodel)
02674 {
02675 case btSoftBody::eAeroModel::F_TwoSided:
02676 nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
02677 default:
02678 {
02679 }
02680 }
02681 const btScalar dvn=btDot(rel_v,nrm);
02682
02683 if(dvn>0)
02684 {
02685 btVector3 force(0,0,0);
02686 const btScalar c0 = f.m_ra*dvn*rel_v2;
02687 const btScalar c1 = c0*medium.m_density;
02688 force += nrm*(-c1*kLF);
02689 force += rel_v.normalized()*(-c1*kDG);
02690 force /= 3;
02691 for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
02692 }
02693 }
02694 }
02695 }
02696 }
02697
02698
02699 void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
02700 {
02701 const btScalar kAHR=psb->m_cfg.kAHR*kst;
02702 const btScalar dt=psb->m_sst.sdt;
02703 for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
02704 {
02705 const Anchor& a=psb->m_anchors[i];
02706 const btTransform& t=a.m_body->getInterpolationWorldTransform();
02707 Node& n=*a.m_node;
02708 const btVector3 wa=t*a.m_local;
02709 const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
02710 const btVector3 vb=n.m_x-n.m_q;
02711 const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
02712 const btVector3 impulse=a.m_c0*vr;
02713 n.m_x+=impulse*a.m_c2;
02714 a.m_body->applyImpulse(-impulse,a.m_c1);
02715 }
02716 }
02717
02718
02719 void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
02720 {
02721 const btScalar dt=psb->m_sst.sdt;
02722 const btScalar mrg=psb->getCollisionShape()->getMargin();
02723 for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
02724 {
02725 const RContact& c=psb->m_rcontacts[i];
02726 const sCti& cti=c.m_cti;
02727 btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj);
02728
02729 const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
02730 const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
02731 const btVector3 vr=vb-va;
02732 const btScalar dn=btDot(vr,cti.m_normal);
02733 if(dn<=SIMD_EPSILON)
02734 {
02735 const btScalar dp=btMin(btDot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
02736 const btVector3 fv=vr-cti.m_normal*dn;
02737 const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
02738 c.m_node->m_x-=impulse*c.m_c2;
02739 if (tmpRigid)
02740 tmpRigid->applyImpulse(impulse,c.m_c1);
02741 }
02742 }
02743 }
02744
02745
02746 void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
02747 {
02748 for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
02749 {
02750 const SContact& c=psb->m_scontacts[i];
02751 const btVector3& nr=c.m_normal;
02752 Node& n=*c.m_node;
02753 Face& f=*c.m_face;
02754 const btVector3 p=BaryEval( f.m_n[0]->m_x,
02755 f.m_n[1]->m_x,
02756 f.m_n[2]->m_x,
02757 c.m_weights);
02758 const btVector3 q=BaryEval( f.m_n[0]->m_q,
02759 f.m_n[1]->m_q,
02760 f.m_n[2]->m_q,
02761 c.m_weights);
02762 const btVector3 vr=(n.m_x-n.m_q)-(p-q);
02763 btVector3 corr(0,0,0);
02764 btScalar dot = btDot(vr,nr);
02765 if(dot<0)
02766 {
02767 const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
02768 corr+=c.m_normal*j;
02769 }
02770 corr -= ProjectOnPlane(vr,nr)*c.m_friction;
02771 n.m_x += corr*c.m_cfm[0];
02772 f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
02773 f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
02774 f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
02775 }
02776 }
02777
02778
02779 void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
02780 {
02781 for(int i=0,ni=psb->m_links.size();i<ni;++i)
02782 {
02783 Link& l=psb->m_links[i];
02784 if(l.m_c0>0)
02785 {
02786 Node& a=*l.m_n[0];
02787 Node& b=*l.m_n[1];
02788 const btVector3 d