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