1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 ///btSoftBody implementation by Nathanael Presson
16
17 #include "btSoftBodyInternals.h"
18
19 //
btSoftBody(btSoftBodyWorldInfo * worldInfo,int node_count,const btVector3 * x,const btScalar * m)20 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
21 :m_worldInfo(worldInfo)
22 {
23 /* Init */
24 m_internalType = CO_SOFT_BODY;
25 m_cfg.aeromodel = eAeroModel::V_Point;
26 m_cfg.kVCF = 1;
27 m_cfg.kDG = 0;
28 m_cfg.kLF = 0;
29 m_cfg.kDP = 0;
30 m_cfg.kPR = 0;
31 m_cfg.kVC = 0;
32 m_cfg.kDF = (btScalar)0.2;
33 m_cfg.kMT = 0;
34 m_cfg.kCHR = (btScalar)1.0;
35 m_cfg.kKHR = (btScalar)0.1;
36 m_cfg.kSHR = (btScalar)1.0;
37 m_cfg.kAHR = (btScalar)0.7;
38 m_cfg.kSRHR_CL = (btScalar)0.1;
39 m_cfg.kSKHR_CL = (btScalar)1;
40 m_cfg.kSSHR_CL = (btScalar)0.5;
41 m_cfg.kSR_SPLT_CL = (btScalar)0.5;
42 m_cfg.kSK_SPLT_CL = (btScalar)0.5;
43 m_cfg.kSS_SPLT_CL = (btScalar)0.5;
44 m_cfg.maxvolume = (btScalar)1;
45 m_cfg.timescale = 1;
46 m_cfg.viterations = 0;
47 m_cfg.piterations = 1;
48 m_cfg.diterations = 0;
49 m_cfg.citerations = 4;
50 m_cfg.collisions = fCollision::Default;
51 m_pose.m_bvolume = false;
52 m_pose.m_bframe = false;
53 m_pose.m_volume = 0;
54 m_pose.m_com = btVector3(0,0,0);
55 m_pose.m_rot.setIdentity();
56 m_pose.m_scl.setIdentity();
57 m_tag = 0;
58 m_timeacc = 0;
59 m_bUpdateRtCst = true;
60 m_bounds[0] = btVector3(0,0,0);
61 m_bounds[1] = btVector3(0,0,0);
62 m_worldTransform.setIdentity();
63 setSolver(eSolverPresets::Positions);
64 /* Default material */
65 Material* pm=appendMaterial();
66 pm->m_kLST = 1;
67 pm->m_kAST = 1;
68 pm->m_kVST = 1;
69 pm->m_flags = fMaterial::Default;
70 /* Collision shape */
71 ///for now, create a collision shape internally
72 m_collisionShape = new btSoftBodyCollisionShape(this);
73 m_collisionShape->setMargin(0.25);
74 /* Nodes */
75 const btScalar margin=getCollisionShape()->getMargin();
76 m_nodes.resize(node_count);
77 for(int i=0,ni=node_count;i<ni;++i)
78 {
79 Node& n=m_nodes[i];
80 ZeroInitialize(n);
81 n.m_x = x?*x++:btVector3(0,0,0);
82 n.m_q = n.m_x;
83 n.m_im = m?*m++:1;
84 n.m_im = n.m_im>0?1/n.m_im:0;
85 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
86 n.m_material= pm;
87 }
88 updateBounds();
89
90 m_initialWorldTransform.setIdentity();
91 }
92
93 //
~btSoftBody()94 btSoftBody::~btSoftBody()
95 {
96 //for now, delete the internal shape
97 delete m_collisionShape;
98 int i;
99
100 releaseClusters();
101 for(i=0;i<m_materials.size();++i)
102 btAlignedFree(m_materials[i]);
103 for(i=0;i<m_joints.size();++i)
104 btAlignedFree(m_joints[i]);
105 }
106
107 //
checkLink(int node0,int node1) const108 bool btSoftBody::checkLink(int node0,int node1) const
109 {
110 return(checkLink(&m_nodes[node0],&m_nodes[node1]));
111 }
112
113 //
checkLink(const Node * node0,const Node * node1) const114 bool btSoftBody::checkLink(const Node* node0,const Node* node1) const
115 {
116 const Node* n[]={node0,node1};
117 for(int i=0,ni=m_links.size();i<ni;++i)
118 {
119 const Link& l=m_links[i];
120 if( (l.m_n[0]==n[0]&&l.m_n[1]==n[1])||
121 (l.m_n[0]==n[1]&&l.m_n[1]==n[0]))
122 {
123 return(true);
124 }
125 }
126 return(false);
127 }
128
129 //
checkFace(int node0,int node1,int node2) const130 bool btSoftBody::checkFace(int node0,int node1,int node2) const
131 {
132 const Node* n[]={ &m_nodes[node0],
133 &m_nodes[node1],
134 &m_nodes[node2]};
135 for(int i=0,ni=m_faces.size();i<ni;++i)
136 {
137 const Face& f=m_faces[i];
138 int c=0;
139 for(int j=0;j<3;++j)
140 {
141 if( (f.m_n[j]==n[0])||
142 (f.m_n[j]==n[1])||
143 (f.m_n[j]==n[2])) c|=1<<j; else break;
144 }
145 if(c==7) return(true);
146 }
147 return(false);
148 }
149
150 //
appendMaterial()151 btSoftBody::Material* btSoftBody::appendMaterial()
152 {
153 Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
154 if(m_materials.size()>0)
155 *pm=*m_materials[0];
156 else
157 ZeroInitialize(*pm);
158 m_materials.push_back(pm);
159 return(pm);
160 }
161
162 //
appendNote(const char * text,const btVector3 & o,const btVector4 & c,Node * n0,Node * n1,Node * n2,Node * n3)163 void btSoftBody::appendNote( const char* text,
164 const btVector3& o,
165 const btVector4& c,
166 Node* n0,
167 Node* n1,
168 Node* n2,
169 Node* n3)
170 {
171 Note n;
172 ZeroInitialize(n);
173 n.m_rank = 0;
174 n.m_text = text;
175 n.m_offset = o;
176 n.m_coords[0] = c.x();
177 n.m_coords[1] = c.y();
178 n.m_coords[2] = c.z();
179 n.m_coords[3] = c.w();
180 n.m_nodes[0] = n0;n.m_rank+=n0?1:0;
181 n.m_nodes[1] = n1;n.m_rank+=n1?1:0;
182 n.m_nodes[2] = n2;n.m_rank+=n2?1:0;
183 n.m_nodes[3] = n3;n.m_rank+=n3?1:0;
184 m_notes.push_back(n);
185 }
186
187 //
appendNote(const char * text,const btVector3 & o,Node * feature)188 void btSoftBody::appendNote( const char* text,
189 const btVector3& o,
190 Node* feature)
191 {
192 appendNote(text,o,btVector4(1,0,0,0),feature);
193 }
194
195 //
appendNote(const char * text,const btVector3 & o,Link * feature)196 void btSoftBody::appendNote( const char* text,
197 const btVector3& o,
198 Link* feature)
199 {
200 static const btScalar w=1/(btScalar)2;
201 appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0],
202 feature->m_n[1]);
203 }
204
205 //
appendNote(const char * text,const btVector3 & o,Face * feature)206 void btSoftBody::appendNote( const char* text,
207 const btVector3& o,
208 Face* feature)
209 {
210 static const btScalar w=1/(btScalar)3;
211 appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0],
212 feature->m_n[1],
213 feature->m_n[2]);
214 }
215
216 //
appendNode(const btVector3 & x,btScalar m)217 void btSoftBody::appendNode( const btVector3& x,btScalar m)
218 {
219 if(m_nodes.capacity()==m_nodes.size())
220 {
221 pointersToIndices();
222 m_nodes.reserve(m_nodes.size()*2+1);
223 indicesToPointers();
224 }
225 const btScalar margin=getCollisionShape()->getMargin();
226 m_nodes.push_back(Node());
227 Node& n=m_nodes[m_nodes.size()-1];
228 ZeroInitialize(n);
229 n.m_x = x;
230 n.m_q = n.m_x;
231 n.m_im = m>0?1/m:0;
232 n.m_material = m_materials[0];
233 n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
234 }
235
236 //
appendLink(int model,Material * mat)237 void btSoftBody::appendLink(int model,Material* mat)
238 {
239 Link l;
240 if(model>=0)
241 l=m_links[model];
242 else
243 { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; }
244 m_links.push_back(l);
245 }
246
247 //
appendLink(int node0,int node1,Material * mat,bool bcheckexist)248 void btSoftBody::appendLink( int node0,
249 int node1,
250 Material* mat,
251 bool bcheckexist)
252 {
253 appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist);
254 }
255
256 //
appendLink(Node * node0,Node * node1,Material * mat,bool bcheckexist)257 void btSoftBody::appendLink( Node* node0,
258 Node* node1,
259 Material* mat,
260 bool bcheckexist)
261 {
262 if((!bcheckexist)||(!checkLink(node0,node1)))
263 {
264 appendLink(-1,mat);
265 Link& l=m_links[m_links.size()-1];
266 l.m_n[0] = node0;
267 l.m_n[1] = node1;
268 l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
269 m_bUpdateRtCst=true;
270 }
271 }
272
273 //
appendFace(int model,Material * mat)274 void btSoftBody::appendFace(int model,Material* mat)
275 {
276 Face f;
277 if(model>=0)
278 { f=m_faces[model]; }
279 else
280 { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; }
281 m_faces.push_back(f);
282 }
283
284 //
appendFace(int node0,int node1,int node2,Material * mat)285 void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
286 {
287 if (node0==node1)
288 return;
289 if (node1==node2)
290 return;
291 if (node2==node0)
292 return;
293
294 appendFace(-1,mat);
295 Face& f=m_faces[m_faces.size()-1];
296 btAssert(node0!=node1);
297 btAssert(node1!=node2);
298 btAssert(node2!=node0);
299 f.m_n[0] = &m_nodes[node0];
300 f.m_n[1] = &m_nodes[node1];
301 f.m_n[2] = &m_nodes[node2];
302 f.m_ra = AreaOf( f.m_n[0]->m_x,
303 f.m_n[1]->m_x,
304 f.m_n[2]->m_x);
305 m_bUpdateRtCst=true;
306 }
307
308 //
appendTetra(int model,Material * mat)309 void btSoftBody::appendTetra(int model,Material* mat)
310 {
311 Tetra t;
312 if(model>=0)
313 t=m_tetras[model];
314 else
315 { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
316 m_tetras.push_back(t);
317 }
318
319 //
appendTetra(int node0,int node1,int node2,int node3,Material * mat)320 void btSoftBody::appendTetra(int node0,
321 int node1,
322 int node2,
323 int node3,
324 Material* mat)
325 {
326 appendTetra(-1,mat);
327 Tetra& t=m_tetras[m_tetras.size()-1];
328 t.m_n[0] = &m_nodes[node0];
329 t.m_n[1] = &m_nodes[node1];
330 t.m_n[2] = &m_nodes[node2];
331 t.m_n[3] = &m_nodes[node3];
332 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);
333 m_bUpdateRtCst=true;
334 }
335
336 //
appendAnchor(int node,btRigidBody * body,bool disableCollisionBetweenLinkedBodies)337 void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies)
338 {
339 if (disableCollisionBetweenLinkedBodies)
340 {
341 if (m_collisionDisabledObjects.findLinearSearch(body)==m_collisionDisabledObjects.size())
342 {
343 m_collisionDisabledObjects.push_back(body);
344 }
345 }
346
347 Anchor a;
348 a.m_node = &m_nodes[node];
349 a.m_body = body;
350 a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x;
351 a.m_node->m_battach = 1;
352 m_anchors.push_back(a);
353 }
354
355 //
appendLinearJoint(const LJoint::Specs & specs,Cluster * body0,Body body1)356 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1)
357 {
358 LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint();
359 pj->m_bodies[0] = body0;
360 pj->m_bodies[1] = body1;
361 pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position;
362 pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position;
363 pj->m_cfm = specs.cfm;
364 pj->m_erp = specs.erp;
365 pj->m_split = specs.split;
366 m_joints.push_back(pj);
367 }
368
369 //
appendLinearJoint(const LJoint::Specs & specs,Body body)370 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body)
371 {
372 appendLinearJoint(specs,m_clusters[0],body);
373 }
374
375 //
appendLinearJoint(const LJoint::Specs & specs,btSoftBody * body)376 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body)
377 {
378 appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]);
379 }
380
381 //
appendAngularJoint(const AJoint::Specs & specs,Cluster * body0,Body body1)382 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1)
383 {
384 AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint();
385 pj->m_bodies[0] = body0;
386 pj->m_bodies[1] = body1;
387 pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis;
388 pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis;
389 pj->m_cfm = specs.cfm;
390 pj->m_erp = specs.erp;
391 pj->m_split = specs.split;
392 pj->m_icontrol = specs.icontrol;
393 m_joints.push_back(pj);
394 }
395
396 //
appendAngularJoint(const AJoint::Specs & specs,Body body)397 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body)
398 {
399 appendAngularJoint(specs,m_clusters[0],body);
400 }
401
402 //
appendAngularJoint(const AJoint::Specs & specs,btSoftBody * body)403 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body)
404 {
405 appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]);
406 }
407
408 //
addForce(const btVector3 & force)409 void btSoftBody::addForce(const btVector3& force)
410 {
411 for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
412 }
413
414 //
addForce(const btVector3 & force,int node)415 void btSoftBody::addForce(const btVector3& force,int node)
416 {
417 Node& n=m_nodes[node];
418 if(n.m_im>0)
419 {
420 n.m_f += force;
421 }
422 }
423
424 //
addVelocity(const btVector3 & velocity)425 void btSoftBody::addVelocity(const btVector3& velocity)
426 {
427 for(int i=0,ni=m_nodes.size();i<ni;++i) addVelocity(velocity,i);
428 }
429
430 /* Set velocity for the entire body */
setVelocity(const btVector3 & velocity)431 void btSoftBody::setVelocity( const btVector3& velocity)
432 {
433 for(int i=0,ni=m_nodes.size();i<ni;++i)
434 {
435 Node& n=m_nodes[i];
436 if(n.m_im>0)
437 {
438 n.m_v = velocity;
439 }
440 }
441 }
442
443
444 //
addVelocity(const btVector3 & velocity,int node)445 void btSoftBody::addVelocity(const btVector3& velocity,int node)
446 {
447 Node& n=m_nodes[node];
448 if(n.m_im>0)
449 {
450 n.m_v += velocity;
451 }
452 }
453
454 //
setMass(int node,btScalar mass)455 void btSoftBody::setMass(int node,btScalar mass)
456 {
457 m_nodes[node].m_im=mass>0?1/mass:0;
458 m_bUpdateRtCst=true;
459 }
460
461 //
getMass(int node) const462 btScalar btSoftBody::getMass(int node) const
463 {
464 return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0);
465 }
466
467 //
getTotalMass() const468 btScalar btSoftBody::getTotalMass() const
469 {
470 btScalar mass=0;
471 for(int i=0;i<m_nodes.size();++i)
472 {
473 mass+=getMass(i);
474 }
475 return(mass);
476 }
477
478 //
setTotalMass(btScalar mass,bool fromfaces)479 void btSoftBody::setTotalMass(btScalar mass,bool fromfaces)
480 {
481 int i;
482
483 if(fromfaces)
484 {
485
486 for(i=0;i<m_nodes.size();++i)
487 {
488 m_nodes[i].m_im=0;
489 }
490 for(i=0;i<m_faces.size();++i)
491 {
492 const Face& f=m_faces[i];
493 const btScalar twicearea=AreaOf( f.m_n[0]->m_x,
494 f.m_n[1]->m_x,
495 f.m_n[2]->m_x);
496 for(int j=0;j<3;++j)
497 {
498 f.m_n[j]->m_im+=twicearea;
499 }
500 }
501 for( i=0;i<m_nodes.size();++i)
502 {
503 m_nodes[i].m_im=1/m_nodes[i].m_im;
504 }
505 }
506 const btScalar tm=getTotalMass();
507 const btScalar itm=1/tm;
508 for( i=0;i<m_nodes.size();++i)
509 {
510 m_nodes[i].m_im/=itm*mass;
511 }
512 m_bUpdateRtCst=true;
513 }
514
515 //
setTotalDensity(btScalar density)516 void btSoftBody::setTotalDensity(btScalar density)
517 {
518 setTotalMass(getVolume()*density,true);
519 }
520
521 //
setVolumeMass(btScalar mass)522 void btSoftBody::setVolumeMass(btScalar mass)
523 {
524 btAlignedObjectArray<btScalar> ranks;
525 ranks.resize(m_nodes.size(),0);
526 int i;
527
528 for(i=0;i<m_nodes.size();++i)
529 {
530 m_nodes[i].m_im=0;
531 }
532 for(i=0;i<m_tetras.size();++i)
533 {
534 const Tetra& t=m_tetras[i];
535 for(int j=0;j<4;++j)
536 {
537 t.m_n[j]->m_im+=btFabs(t.m_rv);
538 ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
539 }
540 }
541 for( i=0;i<m_nodes.size();++i)
542 {
543 if(m_nodes[i].m_im>0)
544 {
545 m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
546 }
547 }
548 setTotalMass(mass,false);
549 }
550
551 //
setVolumeDensity(btScalar density)552 void btSoftBody::setVolumeDensity(btScalar density)
553 {
554 btScalar volume=0;
555 for(int i=0;i<m_tetras.size();++i)
556 {
557 const Tetra& t=m_tetras[i];
558 for(int j=0;j<4;++j)
559 {
560 volume+=btFabs(t.m_rv);
561 }
562 }
563 setVolumeMass(volume*density/6);
564 }
565
566 //
transform(const btTransform & trs)567 void btSoftBody::transform(const btTransform& trs)
568 {
569 const btScalar margin=getCollisionShape()->getMargin();
570 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
571
572 for(int i=0,ni=m_nodes.size();i<ni;++i)
573 {
574 Node& n=m_nodes[i];
575 n.m_x=trs*n.m_x;
576 n.m_q=trs*n.m_q;
577 n.m_n=trs.getBasis()*n.m_n;
578 vol = btDbvtVolume::FromCR(n.m_x,margin);
579
580 m_ndbvt.update(n.m_leaf,vol);
581 }
582 updateNormals();
583 updateBounds();
584 updateConstants();
585 m_initialWorldTransform = trs;
586 }
587
588 //
translate(const btVector3 & trs)589 void btSoftBody::translate(const btVector3& trs)
590 {
591 btTransform t;
592 t.setIdentity();
593 t.setOrigin(trs);
594 transform(t);
595 }
596
597 //
rotate(const btQuaternion & rot)598 void btSoftBody::rotate( const btQuaternion& rot)
599 {
600 btTransform t;
601 t.setIdentity();
602 t.setRotation(rot);
603 transform(t);
604 }
605
606 //
scale(const btVector3 & scl)607 void btSoftBody::scale(const btVector3& scl)
608 {
609 const btScalar margin=getCollisionShape()->getMargin();
610 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
611
612 for(int i=0,ni=m_nodes.size();i<ni;++i)
613 {
614 Node& n=m_nodes[i];
615 n.m_x*=scl;
616 n.m_q*=scl;
617 vol = btDbvtVolume::FromCR(n.m_x,margin);
618 m_ndbvt.update(n.m_leaf,vol);
619 }
620 updateNormals();
621 updateBounds();
622 updateConstants();
623 }
624
625 //
setPose(bool bvolume,bool bframe)626 void btSoftBody::setPose(bool bvolume,bool bframe)
627 {
628 m_pose.m_bvolume = bvolume;
629 m_pose.m_bframe = bframe;
630 int i,ni;
631
632 /* Weights */
633 const btScalar omass=getTotalMass();
634 const btScalar kmass=omass*m_nodes.size()*1000;
635 btScalar tmass=omass;
636 m_pose.m_wgh.resize(m_nodes.size());
637 for(i=0,ni=m_nodes.size();i<ni;++i)
638 {
639 if(m_nodes[i].m_im<=0) tmass+=kmass;
640 }
641 for( i=0,ni=m_nodes.size();i<ni;++i)
642 {
643 Node& n=m_nodes[i];
644 m_pose.m_wgh[i]= n.m_im>0 ?
645 1/(m_nodes[i].m_im*tmass) :
646 kmass/tmass;
647 }
648 /* Pos */
649 const btVector3 com=evaluateCom();
650 m_pose.m_pos.resize(m_nodes.size());
651 for( i=0,ni=m_nodes.size();i<ni;++i)
652 {
653 m_pose.m_pos[i]=m_nodes[i].m_x-com;
654 }
655 m_pose.m_volume = bvolume?getVolume():0;
656 m_pose.m_com = com;
657 m_pose.m_rot.setIdentity();
658 m_pose.m_scl.setIdentity();
659 /* Aqq */
660 m_pose.m_aqq[0] =
661 m_pose.m_aqq[1] =
662 m_pose.m_aqq[2] = btVector3(0,0,0);
663 for( i=0,ni=m_nodes.size();i<ni;++i)
664 {
665 const btVector3& q=m_pose.m_pos[i];
666 const btVector3 mq=m_pose.m_wgh[i]*q;
667 m_pose.m_aqq[0]+=mq.x()*q;
668 m_pose.m_aqq[1]+=mq.y()*q;
669 m_pose.m_aqq[2]+=mq.z()*q;
670 }
671 m_pose.m_aqq=m_pose.m_aqq.inverse();
672 updateConstants();
673 }
674
675 //
getVolume() const676 btScalar btSoftBody::getVolume() const
677 {
678 btScalar vol=0;
679 if(m_nodes.size()>0)
680 {
681 int i,ni;
682
683 const btVector3 org=m_nodes[0].m_x;
684 for(i=0,ni=m_faces.size();i<ni;++i)
685 {
686 const Face& f=m_faces[i];
687 vol+=btDot(f.m_n[0]->m_x-org,btCross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org));
688 }
689 vol/=(btScalar)6;
690 }
691 return(vol);
692 }
693
694 //
clusterCount() const695 int btSoftBody::clusterCount() const
696 {
697 return(m_clusters.size());
698 }
699
700 //
clusterCom(const Cluster * cluster)701 btVector3 btSoftBody::clusterCom(const Cluster* cluster)
702 {
703 btVector3 com(0,0,0);
704 for(int i=0,ni=cluster->m_nodes.size();i<ni;++i)
705 {
706 com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i];
707 }
708 return(com*cluster->m_imass);
709 }
710
711 //
clusterCom(int cluster) const712 btVector3 btSoftBody::clusterCom(int cluster) const
713 {
714 return(clusterCom(m_clusters[cluster]));
715 }
716
717 //
clusterVelocity(const Cluster * cluster,const btVector3 & rpos)718 btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos)
719 {
720 return(cluster->m_lv+btCross(cluster->m_av,rpos));
721 }
722
723 //
clusterVImpulse(Cluster * cluster,const btVector3 & rpos,const btVector3 & impulse)724 void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
725 {
726 const btVector3 li=cluster->m_imass*impulse;
727 const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
728 cluster->m_vimpulses[0]+=li;cluster->m_lv+=li;
729 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
730 cluster->m_nvimpulses++;
731 }
732
733 //
clusterDImpulse(Cluster * cluster,const btVector3 & rpos,const btVector3 & impulse)734 void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse)
735 {
736 const btVector3 li=cluster->m_imass*impulse;
737 const btVector3 ai=cluster->m_invwi*btCross(rpos,impulse);
738 cluster->m_dimpulses[0]+=li;
739 cluster->m_dimpulses[1]+=ai;
740 cluster->m_ndimpulses++;
741 }
742
743 //
clusterImpulse(Cluster * cluster,const btVector3 & rpos,const Impulse & impulse)744 void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse)
745 {
746 if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity);
747 if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift);
748 }
749
750 //
clusterVAImpulse(Cluster * cluster,const btVector3 & impulse)751 void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
752 {
753 const btVector3 ai=cluster->m_invwi*impulse;
754 cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai;
755 cluster->m_nvimpulses++;
756 }
757
758 //
clusterDAImpulse(Cluster * cluster,const btVector3 & impulse)759 void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
760 {
761 const btVector3 ai=cluster->m_invwi*impulse;
762 cluster->m_dimpulses[1]+=ai;
763 cluster->m_ndimpulses++;
764 }
765
766 //
clusterAImpulse(Cluster * cluster,const Impulse & impulse)767 void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse)
768 {
769 if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity);
770 if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift);
771 }
772
773 //
clusterDCImpulse(Cluster * cluster,const btVector3 & impulse)774 void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
775 {
776 cluster->m_dimpulses[0]+=impulse*cluster->m_imass;
777 cluster->m_ndimpulses++;
778 }
779
780 struct NodeLinks
781 {
782 btAlignedObjectArray<int> m_links;
783 };
784
785
786
787 //
generateBendingConstraints(int distance,Material * mat)788 int btSoftBody::generateBendingConstraints(int distance,Material* mat)
789 {
790 int i,j;
791
792 if(distance>1)
793 {
794 /* Build graph */
795 const int n=m_nodes.size();
796 const unsigned inf=(~(unsigned)0)>>1;
797 unsigned* adj=new unsigned[n*n];
798
799
800 #define IDX(_x_,_y_) ((_y_)*n+(_x_))
801 for(j=0;j<n;++j)
802 {
803 for(i=0;i<n;++i)
804 {
805 if(i!=j)
806 {
807 adj[IDX(i,j)]=adj[IDX(j,i)]=inf;
808 }
809 else
810 {
811 adj[IDX(i,j)]=adj[IDX(j,i)]=0;
812 }
813 }
814 }
815 for( i=0;i<m_links.size();++i)
816 {
817 const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
818 const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
819 adj[IDX(ia,ib)]=1;
820 adj[IDX(ib,ia)]=1;
821 }
822
823
824 //special optimized case for distance == 2
825 if (distance == 2)
826 {
827
828 btAlignedObjectArray<NodeLinks> nodeLinks;
829
830
831 /* Build node links */
832 nodeLinks.resize(m_nodes.size());
833
834 for( i=0;i<m_links.size();++i)
835 {
836 const int ia=(int)(m_links[i].m_n[0]-&m_nodes[0]);
837 const int ib=(int)(m_links[i].m_n[1]-&m_nodes[0]);
838 if (nodeLinks[ia].m_links.findLinearSearch(ib)==nodeLinks[ia].m_links.size())
839 nodeLinks[ia].m_links.push_back(ib);
840
841 if (nodeLinks[ib].m_links.findLinearSearch(ia)==nodeLinks[ib].m_links.size())
842 nodeLinks[ib].m_links.push_back(ia);
843 }
844 for (int ii=0;ii<nodeLinks.size();ii++)
845 {
846 int i=ii;
847
848 for (int jj=0;jj<nodeLinks[ii].m_links.size();jj++)
849 {
850 int k = nodeLinks[ii].m_links[jj];
851 for (int kk=0;kk<nodeLinks[k].m_links.size();kk++)
852 {
853 int j = nodeLinks[k].m_links[kk];
854 if (i!=j)
855 {
856 const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
857 btAssert(sum==2);
858 if(adj[IDX(i,j)]>sum)
859 {
860 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
861 }
862 }
863
864 }
865 }
866 }
867 } else
868 {
869 ///generic Floyd's algorithm
870 for(int k=0;k<n;++k)
871 {
872 for(j=0;j<n;++j)
873 {
874 for(i=j+1;i<n;++i)
875 {
876 const unsigned sum=adj[IDX(i,k)]+adj[IDX(k,j)];
877 if(adj[IDX(i,j)]>sum)
878 {
879 adj[IDX(i,j)]=adj[IDX(j,i)]=sum;
880 }
881 }
882 }
883 }
884 }
885
886
887 /* Build links */
888 int nlinks=0;
889 for(j=0;j<n;++j)
890 {
891 for(i=j+1;i<n;++i)
892 {
893 if(adj[IDX(i,j)]==(unsigned)distance)
894 {
895 appendLink(i,j,mat);
896 m_links[m_links.size()-1].m_bbending=1;
897 ++nlinks;
898 }
899 }
900 }
901 delete[] adj;
902 return(nlinks);
903 }
904 return(0);
905 }
906
907 //
randomizeConstraints()908 void btSoftBody::randomizeConstraints()
909 {
910 unsigned long seed=243703;
911 #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
912 int i,ni;
913
914 for(i=0,ni=m_links.size();i<ni;++i)
915 {
916 btSwap(m_links[i],m_links[NEXTRAND%ni]);
917 }
918 for(i=0,ni=m_faces.size();i<ni;++i)
919 {
920 btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
921 }
922 #undef NEXTRAND
923 }
924
925 //
releaseCluster(int index)926 void btSoftBody::releaseCluster(int index)
927 {
928 Cluster* c=m_clusters[index];
929 if(c->m_leaf) m_cdbvt.remove(c->m_leaf);
930 c->~Cluster();
931 btAlignedFree(c);
932 m_clusters.remove(c);
933 }
934
935 //
releaseClusters()936 void btSoftBody::releaseClusters()
937 {
938 while(m_clusters.size()>0) releaseCluster(0);
939 }
940
941 //
generateClusters(int k,int maxiterations)942 int btSoftBody::generateClusters(int k,int maxiterations)
943 {
944 int i;
945 releaseClusters();
946 m_clusters.resize(btMin(k,m_nodes.size()));
947 for(i=0;i<m_clusters.size();++i)
948 {
949 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
950 m_clusters[i]->m_collide= true;
951 }
952 k=m_clusters.size();
953 if(k>0)
954 {
955 /* Initialize */
956 btAlignedObjectArray<btVector3> centers;
957 btVector3 cog(0,0,0);
958 int i;
959 for(i=0;i<m_nodes.size();++i)
960 {
961 cog+=m_nodes[i].m_x;
962 m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
963 }
964 cog/=(btScalar)m_nodes.size();
965 centers.resize(k,cog);
966 /* Iterate */
967 const btScalar slope=16;
968 bool changed;
969 int iterations=0;
970 do {
971 const btScalar w=2-btMin<btScalar>(1,iterations/slope);
972 changed=false;
973 iterations++;
974 int i;
975
976 for(i=0;i<k;++i)
977 {
978 btVector3 c(0,0,0);
979 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
980 {
981 c+=m_clusters[i]->m_nodes[j]->m_x;
982 }
983 if(m_clusters[i]->m_nodes.size())
984 {
985 c /= (btScalar)m_clusters[i]->m_nodes.size();
986 c = centers[i]+(c-centers[i])*w;
987 changed |= ((c-centers[i]).length2()>SIMD_EPSILON);
988 centers[i] = c;
989 m_clusters[i]->m_nodes.resize(0);
990 }
991 }
992 for(i=0;i<m_nodes.size();++i)
993 {
994 const btVector3 nx=m_nodes[i].m_x;
995 int kbest=0;
996 btScalar kdist=ClusterMetric(centers[0],nx);
997 for(int j=1;j<k;++j)
998 {
999 const btScalar d=ClusterMetric(centers[j],nx);
1000 if(d<kdist)
1001 {
1002 kbest=j;
1003 kdist=d;
1004 }
1005 }
1006 m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1007 }
1008 } while(changed&&(iterations<maxiterations));
1009 /* Merge */
1010 btAlignedObjectArray<int> cids;
1011 cids.resize(m_nodes.size(),-1);
1012 for(i=0;i<m_clusters.size();++i)
1013 {
1014 for(int j=0;j<m_clusters[i]->m_nodes.size();++j)
1015 {
1016 cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i;
1017 }
1018 }
1019 for(i=0;i<m_faces.size();++i)
1020 {
1021 const int idx[]={ int(m_faces[i].m_n[0]-&m_nodes[0]),
1022 int(m_faces[i].m_n[1]-&m_nodes[0]),
1023 int(m_faces[i].m_n[2]-&m_nodes[0])};
1024 for(int j=0;j<3;++j)
1025 {
1026 const int cid=cids[idx[j]];
1027 for(int q=1;q<3;++q)
1028 {
1029 const int kid=idx[(j+q)%3];
1030 if(cids[kid]!=cid)
1031 {
1032 if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size())
1033 {
1034 m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1035 }
1036 }
1037 }
1038 }
1039 }
1040 /* Master */
1041 if(m_clusters.size()>1)
1042 {
1043 Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1044 pmaster->m_collide = false;
1045 pmaster->m_nodes.reserve(m_nodes.size());
1046 for(int i=0;i<m_nodes.size();++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1047 m_clusters.push_back(pmaster);
1048 btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]);
1049 }
1050 /* Terminate */
1051 for(i=0;i<m_clusters.size();++i)
1052 {
1053 if(m_clusters[i]->m_nodes.size()==0)
1054 {
1055 releaseCluster(i--);
1056 }
1057 }
1058 } else
1059 {
1060 //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1061 if (m_tetras.size())
1062 {
1063 m_clusters.resize(m_tetras.size());
1064 for(i=0;i<m_clusters.size();++i)
1065 {
1066 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1067 m_clusters[i]->m_collide= true;
1068 }
1069 for (i=0;i<m_tetras.size();i++)
1070 {
1071 for (int j=0;j<4;j++)
1072 {
1073 m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1074 }
1075 }
1076
1077 } else
1078 {
1079 m_clusters.resize(m_faces.size());
1080 for(i=0;i<m_clusters.size();++i)
1081 {
1082 m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster();
1083 m_clusters[i]->m_collide= true;
1084 }
1085
1086 for(i=0;i<m_faces.size();++i)
1087 {
1088 for(int j=0;j<3;++j)
1089 {
1090 m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1091 }
1092 }
1093 }
1094 }
1095
1096 if (m_clusters.size())
1097 {
1098 initializeClusters();
1099 updateClusters();
1100
1101
1102 //for self-collision
1103 m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
1104 {
1105 for (int c0=0;c0<m_clusters.size();c0++)
1106 {
1107 m_clusters[c0]->m_clusterIndex=c0;
1108 for (int c1=0;c1<m_clusters.size();c1++)
1109 {
1110
1111 bool connected=false;
1112 Cluster* cla = m_clusters[c0];
1113 Cluster* clb = m_clusters[c1];
1114 for (int i=0;!connected&&i<cla->m_nodes.size();i++)
1115 {
1116 for (int j=0;j<clb->m_nodes.size();j++)
1117 {
1118 if (cla->m_nodes[i] == clb->m_nodes[j])
1119 {
1120 connected=true;
1121 break;
1122 }
1123 }
1124 }
1125 m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
1126 }
1127 }
1128 }
1129 }
1130
1131 return(m_clusters.size());
1132 }
1133
1134 //
refine(ImplicitFn * ifn,btScalar accurary,bool cut)1135 void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut)
1136 {
1137 const Node* nbase = &m_nodes[0];
1138 int ncount = m_nodes.size();
1139 btSymMatrix<int> edges(ncount,-2);
1140 int newnodes=0;
1141 int i,j,k,ni;
1142
1143 /* Filter out */
1144 for(i=0;i<m_links.size();++i)
1145 {
1146 Link& l=m_links[i];
1147 if(l.m_bbending)
1148 {
1149 if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
1150 {
1151 btSwap(m_links[i],m_links[m_links.size()-1]);
1152 m_links.pop_back();--i;
1153 }
1154 }
1155 }
1156 /* Fill edges */
1157 for(i=0;i<m_links.size();++i)
1158 {
1159 Link& l=m_links[i];
1160 edges(int(l.m_n[0]-nbase),int(l.m_n[1]-nbase))=-1;
1161 }
1162 for(i=0;i<m_faces.size();++i)
1163 {
1164 Face& f=m_faces[i];
1165 edges(int(f.m_n[0]-nbase),int(f.m_n[1]-nbase))=-1;
1166 edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
1167 edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
1168 }
1169 /* Intersect */
1170 for(i=0;i<ncount;++i)
1171 {
1172 for(j=i+1;j<ncount;++j)
1173 {
1174 if(edges(i,j)==-1)
1175 {
1176 Node& a=m_nodes[i];
1177 Node& b=m_nodes[j];
1178 const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary);
1179 if(t>0)
1180 {
1181 const btVector3 x=Lerp(a.m_x,b.m_x,t);
1182 const btVector3 v=Lerp(a.m_v,b.m_v,t);
1183 btScalar m=0;
1184 if(a.m_im>0)
1185 {
1186 if(b.m_im>0)
1187 {
1188 const btScalar ma=1/a.m_im;
1189 const btScalar mb=1/b.m_im;
1190 const btScalar mc=Lerp(ma,mb,t);
1191 const btScalar f=(ma+mb)/(ma+mb+mc);
1192 a.m_im=1/(ma*f);
1193 b.m_im=1/(mb*f);
1194 m=mc*f;
1195 }
1196 else
1197 { a.m_im/=0.5;m=1/a.m_im; }
1198 }
1199 else
1200 {
1201 if(b.m_im>0)
1202 { b.m_im/=0.5;m=1/b.m_im; }
1203 else
1204 m=0;
1205 }
1206 appendNode(x,m);
1207 edges(i,j)=m_nodes.size()-1;
1208 m_nodes[edges(i,j)].m_v=v;
1209 ++newnodes;
1210 }
1211 }
1212 }
1213 }
1214 nbase=&m_nodes[0];
1215 /* Refine links */
1216 for(i=0,ni=m_links.size();i<ni;++i)
1217 {
1218 Link& feat=m_links[i];
1219 const int idx[]={ int(feat.m_n[0]-nbase),
1220 int(feat.m_n[1]-nbase)};
1221 if((idx[0]<ncount)&&(idx[1]<ncount))
1222 {
1223 const int ni=edges(idx[0],idx[1]);
1224 if(ni>0)
1225 {
1226 appendLink(i);
1227 Link* pft[]={ &m_links[i],
1228 &m_links[m_links.size()-1]};
1229 pft[0]->m_n[0]=&m_nodes[idx[0]];
1230 pft[0]->m_n[1]=&m_nodes[ni];
1231 pft[1]->m_n[0]=&m_nodes[ni];
1232 pft[1]->m_n[1]=&m_nodes[idx[1]];
1233 }
1234 }
1235 }
1236 /* Refine faces */
1237 for(i=0;i<m_faces.size();++i)
1238 {
1239 const Face& feat=m_faces[i];
1240 const int idx[]={ int(feat.m_n[0]-nbase),
1241 int(feat.m_n[1]-nbase),
1242 int(feat.m_n[2]-nbase)};
1243 for(j=2,k=0;k<3;j=k++)
1244 {
1245 if((idx[j]<ncount)&&(idx[k]<ncount))
1246 {
1247 const int ni=edges(idx[j],idx[k]);
1248 if(ni>0)
1249 {
1250 appendFace(i);
1251 const int l=(k+1)%3;
1252 Face* pft[]={ &m_faces[i],
1253 &m_faces[m_faces.size()-1]};
1254 pft[0]->m_n[0]=&m_nodes[idx[l]];
1255 pft[0]->m_n[1]=&m_nodes[idx[j]];
1256 pft[0]->m_n[2]=&m_nodes[ni];
1257 pft[1]->m_n[0]=&m_nodes[ni];
1258 pft[1]->m_n[1]=&m_nodes[idx[k]];
1259 pft[1]->m_n[2]=&m_nodes[idx[l]];
1260 appendLink(ni,idx[l],pft[0]->m_material);
1261 --i;break;
1262 }
1263 }
1264 }
1265 }
1266 /* Cut */
1267 if(cut)
1268 {
1269 btAlignedObjectArray<int> cnodes;
1270 const int pcount=ncount;
1271 int i;
1272 ncount=m_nodes.size();
1273 cnodes.resize(ncount,0);
1274 /* Nodes */
1275 for(i=0;i<ncount;++i)
1276 {
1277 const btVector3 x=m_nodes[i].m_x;
1278 if((i>=pcount)||(btFabs(ifn->Eval(x))<accurary))
1279 {
1280 const btVector3 v=m_nodes[i].m_v;
1281 btScalar m=getMass(i);
1282 if(m>0) { m*=0.5;m_nodes[i].m_im/=0.5; }
1283 appendNode(x,m);
1284 cnodes[i]=m_nodes.size()-1;
1285 m_nodes[cnodes[i]].m_v=v;
1286 }
1287 }
1288 nbase=&m_nodes[0];
1289 /* Links */
1290 for(i=0,ni=m_links.size();i<ni;++i)
1291 {
1292 const int id[]={ int(m_links[i].m_n[0]-nbase),
1293 int(m_links[i].m_n[1]-nbase)};
1294 int todetach=0;
1295 if(cnodes[id[0]]&&cnodes[id[1]])
1296 {
1297 appendLink(i);
1298 todetach=m_links.size()-1;
1299 }
1300 else
1301 {
1302 if(( (ifn->Eval(m_nodes[id[0]].m_x)<accurary)&&
1303 (ifn->Eval(m_nodes[id[1]].m_x)<accurary)))
1304 todetach=i;
1305 }
1306 if(todetach)
1307 {
1308 Link& l=m_links[todetach];
1309 for(int j=0;j<2;++j)
1310 {
1311 int cn=cnodes[int(l.m_n[j]-nbase)];
1312 if(cn) l.m_n[j]=&m_nodes[cn];
1313 }
1314 }
1315 }
1316 /* Faces */
1317 for(i=0,ni=m_faces.size();i<ni;++i)
1318 {
1319 Node** n= m_faces[i].m_n;
1320 if( (ifn->Eval(n[0]->m_x)<accurary)&&
1321 (ifn->Eval(n[1]->m_x)<accurary)&&
1322 (ifn->Eval(n[2]->m_x)<accurary))
1323 {
1324 for(int j=0;j<3;++j)
1325 {
1326 int cn=cnodes[int(n[j]-nbase)];
1327 if(cn) n[j]=&m_nodes[cn];
1328 }
1329 }
1330 }
1331 /* Clean orphans */
1332 int nnodes=m_nodes.size();
1333 btAlignedObjectArray<int> ranks;
1334 btAlignedObjectArray<int> todelete;
1335 ranks.resize(nnodes,0);
1336 for(i=0,ni=m_links.size();i<ni;++i)
1337 {
1338 for(int j=0;j<2;++j) ranks[int(m_links[i].m_n[j]-nbase)]++;
1339 }
1340 for(i=0,ni=m_faces.size();i<ni;++i)
1341 {
1342 for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
1343 }
1344 for(i=0;i<m_links.size();++i)
1345 {
1346 const int id[]={ int(m_links[i].m_n[0]-nbase),
1347 int(m_links[i].m_n[1]-nbase)};
1348 const bool sg[]={ ranks[id[0]]==1,
1349 ranks[id[1]]==1};
1350 if(sg[0]||sg[1])
1351 {
1352 --ranks[id[0]];
1353 --ranks[id[1]];
1354 btSwap(m_links[i],m_links[m_links.size()-1]);
1355 m_links.pop_back();--i;
1356 }
1357 }
1358 #if 0
1359 for(i=nnodes-1;i>=0;--i)
1360 {
1361 if(!ranks[i]) todelete.push_back(i);
1362 }
1363 if(todelete.size())
1364 {
1365 btAlignedObjectArray<int>& map=ranks;
1366 for(int i=0;i<nnodes;++i) map[i]=i;
1367 PointersToIndices(this);
1368 for(int i=0,ni=todelete.size();i<ni;++i)
1369 {
1370 int j=todelete[i];
1371 int& a=map[j];
1372 int& b=map[--nnodes];
1373 m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1374 btSwap(m_nodes[a],m_nodes[b]);
1375 j=a;a=b;b=j;
1376 }
1377 IndicesToPointers(this,&map[0]);
1378 m_nodes.resize(nnodes);
1379 }
1380 #endif
1381 }
1382 m_bUpdateRtCst=true;
1383 }
1384
1385 //
cutLink(const Node * node0,const Node * node1,btScalar position)1386 bool btSoftBody::cutLink(const Node* node0,const Node* node1,btScalar position)
1387 {
1388 return(cutLink(int(node0-&m_nodes[0]),int(node1-&m_nodes[0]),position));
1389 }
1390
1391 //
cutLink(int node0,int node1,btScalar position)1392 bool btSoftBody::cutLink(int node0,int node1,btScalar position)
1393 {
1394 bool done=false;
1395 int i,ni;
1396 const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1397 const btVector3 x=Lerp(m_nodes[node0].m_x,m_nodes[node1].m_x,position);
1398 const btVector3 v=Lerp(m_nodes[node0].m_v,m_nodes[node1].m_v,position);
1399 const btScalar m=1;
1400 appendNode(x,m);
1401 appendNode(x,m);
1402 Node* pa=&m_nodes[node0];
1403 Node* pb=&m_nodes[node1];
1404 Node* pn[2]={ &m_nodes[m_nodes.size()-2],
1405 &m_nodes[m_nodes.size()-1]};
1406 pn[0]->m_v=v;
1407 pn[1]->m_v=v;
1408 for(i=0,ni=m_links.size();i<ni;++i)
1409 {
1410 const int mtch=MatchEdge(m_links[i].m_n[0],m_links[i].m_n[1],pa,pb);
1411 if(mtch!=-1)
1412 {
1413 appendLink(i);
1414 Link* pft[]={&m_links[i],&m_links[m_links.size()-1]};
1415 pft[0]->m_n[1]=pn[mtch];
1416 pft[1]->m_n[0]=pn[1-mtch];
1417 done=true;
1418 }
1419 }
1420 for(i=0,ni=m_faces.size();i<ni;++i)
1421 {
1422 for(int k=2,l=0;l<3;k=l++)
1423 {
1424 const int mtch=MatchEdge(m_faces[i].m_n[k],m_faces[i].m_n[l],pa,pb);
1425 if(mtch!=-1)
1426 {
1427 appendFace(i);
1428 Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]};
1429 pft[0]->m_n[l]=pn[mtch];
1430 pft[1]->m_n[k]=pn[1-mtch];
1431 appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1432 appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true);
1433 }
1434 }
1435 }
1436 if(!done)
1437 {
1438 m_ndbvt.remove(pn[0]->m_leaf);
1439 m_ndbvt.remove(pn[1]->m_leaf);
1440 m_nodes.pop_back();
1441 m_nodes.pop_back();
1442 }
1443 return(done);
1444 }
1445
1446 //
rayTest(const btVector3 & rayFrom,const btVector3 & rayTo,sRayCast & results)1447 bool btSoftBody::rayTest(const btVector3& rayFrom,
1448 const btVector3& rayTo,
1449 sRayCast& results)
1450 {
1451 if(m_faces.size()&&m_fdbvt.empty())
1452 initializeFaceTree();
1453
1454 results.body = this;
1455 results.fraction = 1.f;
1456 results.feature = eFeature::None;
1457 results.index = -1;
1458
1459 return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0);
1460 }
1461
1462 //
setSolver(eSolverPresets::_ preset)1463 void btSoftBody::setSolver(eSolverPresets::_ preset)
1464 {
1465 m_cfg.m_vsequence.clear();
1466 m_cfg.m_psequence.clear();
1467 m_cfg.m_dsequence.clear();
1468 switch(preset)
1469 {
1470 case eSolverPresets::Positions:
1471 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1472 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1473 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1474 m_cfg.m_psequence.push_back(ePSolver::Linear);
1475 break;
1476 case eSolverPresets::Velocities:
1477 m_cfg.m_vsequence.push_back(eVSolver::Linear);
1478
1479 m_cfg.m_psequence.push_back(ePSolver::Anchors);
1480 m_cfg.m_psequence.push_back(ePSolver::RContacts);
1481 m_cfg.m_psequence.push_back(ePSolver::SContacts);
1482
1483 m_cfg.m_dsequence.push_back(ePSolver::Linear);
1484 break;
1485 }
1486 }
1487
1488 //
predictMotion(btScalar dt)1489 void btSoftBody::predictMotion(btScalar dt)
1490 {
1491 int i,ni;
1492
1493 /* Update */
1494 if(m_bUpdateRtCst)
1495 {
1496 m_bUpdateRtCst=false;
1497 updateConstants();
1498 m_fdbvt.clear();
1499 if(m_cfg.collisions&fCollision::VF_SS)
1500 {
1501 initializeFaceTree();
1502 }
1503 }
1504
1505 /* Prepare */
1506 m_sst.sdt = dt*m_cfg.timescale;
1507 m_sst.isdt = 1/m_sst.sdt;
1508 m_sst.velmrg = m_sst.sdt*3;
1509 m_sst.radmrg = getCollisionShape()->getMargin();
1510 m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
1511 /* Forces */
1512 addVelocity(m_worldInfo->m_gravity*m_sst.sdt);
1513 applyForces();
1514 /* Integrate */
1515 for(i=0,ni=m_nodes.size();i<ni;++i)
1516 {
1517 Node& n=m_nodes[i];
1518 n.m_q = n.m_x;
1519 n.m_v += n.m_f*n.m_im*m_sst.sdt;
1520 n.m_x += n.m_v*m_sst.sdt;
1521 n.m_f = btVector3(0,0,0);
1522 }
1523 /* Clusters */
1524 updateClusters();
1525 /* Bounds */
1526 updateBounds();
1527 /* Nodes */
1528 ATTRIBUTE_ALIGNED16(btDbvtVolume) vol;
1529 for(i=0,ni=m_nodes.size();i<ni;++i)
1530 {
1531 Node& n=m_nodes[i];
1532 vol = btDbvtVolume::FromCR(n.m_x,m_sst.radmrg);
1533 m_ndbvt.update( n.m_leaf,
1534 vol,
1535 n.m_v*m_sst.velmrg,
1536 m_sst.updmrg);
1537 }
1538 /* Faces */
1539 if(!m_fdbvt.empty())
1540 {
1541 for(int i=0;i<m_faces.size();++i)
1542 {
1543 Face& f=m_faces[i];
1544 const btVector3 v=( f.m_n[0]->m_v+
1545 f.m_n[1]->m_v+
1546 f.m_n[2]->m_v)/3;
1547 vol = VolumeOf(f,m_sst.radmrg);
1548 m_fdbvt.update( f.m_leaf,
1549 vol,
1550 v*m_sst.velmrg,
1551 m_sst.updmrg);
1552 }
1553 }
1554 /* Pose */
1555 updatePose();
1556 /* Match */
1557 if(m_pose.m_bframe&&(m_cfg.kMT>0))
1558 {
1559 const btMatrix3x3 posetrs=m_pose.m_rot;
1560 for(int i=0,ni=m_nodes.size();i<ni;++i)
1561 {
1562 Node& n=m_nodes[i];
1563 if(n.m_im>0)
1564 {
1565 const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com;
1566 n.m_x=Lerp(n.m_x,x,m_cfg.kMT);
1567 }
1568 }
1569 }
1570 /* Clear contacts */
1571 m_rcontacts.resize(0);
1572 m_scontacts.resize(0);
1573 /* Optimize dbvt's */
1574 m_ndbvt.optimizeIncremental(1);
1575 m_fdbvt.optimizeIncremental(1);
1576 m_cdbvt.optimizeIncremental(1);
1577 }
1578
1579 //
solveConstraints()1580 void btSoftBody::solveConstraints()
1581 {
1582 /* Apply clusters */
1583 applyClusters(false);
1584 /* Prepare links */
1585
1586 int i,ni;
1587
1588 for(i=0,ni=m_links.size();i<ni;++i)
1589 {
1590 Link& l=m_links[i];
1591 l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q;
1592 l.m_c2 = 1/(l.m_c3.length2()*l.m_c0);
1593 }
1594 /* Prepare anchors */
1595 for(i=0,ni=m_anchors.size();i<ni;++i)
1596 {
1597 Anchor& a=m_anchors[i];
1598 const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local;
1599 a.m_c0 = ImpulseMatrix( m_sst.sdt,
1600 a.m_node->m_im,
1601 a.m_body->getInvMass(),
1602 a.m_body->getInvInertiaTensorWorld(),
1603 ra);
1604 a.m_c1 = ra;
1605 a.m_c2 = m_sst.sdt*a.m_node->m_im;
1606 a.m_body->activate();
1607 }
1608 /* Solve velocities */
1609 if(m_cfg.viterations>0)
1610 {
1611 /* Solve */
1612 for(int isolve=0;isolve<m_cfg.viterations;++isolve)
1613 {
1614 for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
1615 {
1616 getSolver(m_cfg.m_vsequence[iseq])(this,1);
1617 }
1618 }
1619 /* Update */
1620 for(i=0,ni=m_nodes.size();i<ni;++i)
1621 {
1622 Node& n=m_nodes[i];
1623 n.m_x = n.m_q+n.m_v*m_sst.sdt;
1624 }
1625 }
1626 /* Solve positions */
1627 if(m_cfg.piterations>0)
1628 {
1629 for(int isolve=0;isolve<m_cfg.piterations;++isolve)
1630 {
1631 const btScalar ti=isolve/(btScalar)m_cfg.piterations;
1632 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1633 {
1634 getSolver(m_cfg.m_psequence[iseq])(this,1,ti);
1635 }
1636 }
1637 const btScalar vc=m_sst.isdt*(1-m_cfg.kDP);
1638 for(i=0,ni=m_nodes.size();i<ni;++i)
1639 {
1640 Node& n=m_nodes[i];
1641 n.m_v = (n.m_x-n.m_q)*vc;
1642 n.m_f = btVector3(0,0,0);
1643 }
1644 }
1645 /* Solve drift */
1646 if(m_cfg.diterations>0)
1647 {
1648 const btScalar vcf=m_cfg.kVCF*m_sst.isdt;
1649 for(i=0,ni=m_nodes.size();i<ni;++i)
1650 {
1651 Node& n=m_nodes[i];
1652 n.m_q = n.m_x;
1653 }
1654 for(int idrift=0;idrift<m_cfg.diterations;++idrift)
1655 {
1656 for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
1657 {
1658 getSolver(m_cfg.m_dsequence[iseq])(this,1,0);
1659 }
1660 }
1661 for(int i=0,ni=m_nodes.size();i<ni;++i)
1662 {
1663 Node& n=m_nodes[i];
1664 n.m_v += (n.m_x-n.m_q)*vcf;
1665 }
1666 }
1667 /* Apply clusters */
1668 dampClusters();
1669 applyClusters(true);
1670 }
1671
1672 //
staticSolve(int iterations)1673 void btSoftBody::staticSolve(int iterations)
1674 {
1675 for(int isolve=0;isolve<iterations;++isolve)
1676 {
1677 for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
1678 {
1679 getSolver(m_cfg.m_psequence[iseq])(this,1,0);
1680 }
1681 }
1682 }
1683
1684 //
solveCommonConstraints(btSoftBody **,int,int)1685 void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/,int /*count*/,int /*iterations*/)
1686 {
1687 /// placeholder
1688 }
1689
1690 //
solveClusters(const btAlignedObjectArray<btSoftBody * > & bodies)1691 void btSoftBody::solveClusters(const btAlignedObjectArray<btSoftBody*>& bodies)
1692 {
1693 const int nb=bodies.size();
1694 int iterations=0;
1695 int i;
1696
1697 for(i=0;i<nb;++i)
1698 {
1699 iterations=btMax(iterations,bodies[i]->m_cfg.citerations);
1700 }
1701 for(i=0;i<nb;++i)
1702 {
1703 bodies[i]->prepareClusters(iterations);
1704 }
1705 for(i=0;i<iterations;++i)
1706 {
1707 const btScalar sor=1;
1708 for(int j=0;j<nb;++j)
1709 {
1710 bodies[j]->solveClusters(sor);
1711 }
1712 }
1713 for(i=0;i<nb;++i)
1714 {
1715 bodies[i]->cleanupClusters();
1716 }
1717 }
1718
1719 //
integrateMotion()1720 void btSoftBody::integrateMotion()
1721 {
1722 /* Update */
1723 updateNormals();
1724 }
1725
1726 //
RayFromToCaster(const btVector3 & rayFrom,const btVector3 & rayTo,btScalar mxt)1727 btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt)
1728 {
1729 m_rayFrom = rayFrom;
1730 m_rayNormalizedDirection = (rayTo-rayFrom);
1731 m_rayTo = rayTo;
1732 m_mint = mxt;
1733 m_face = 0;
1734 m_tests = 0;
1735 }
1736
1737 //
Process(const btDbvtNode * leaf)1738 void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf)
1739 {
1740 btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data;
1741 const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection,
1742 f.m_n[0]->m_x,
1743 f.m_n[1]->m_x,
1744 f.m_n[2]->m_x,
1745 m_mint);
1746 if((t>0)&&(t<m_mint))
1747 {
1748 m_mint=t;m_face=&f;
1749 }
1750 ++m_tests;
1751 }
1752
1753 //
rayFromToTriangle(const btVector3 & rayFrom,const btVector3 & rayTo,const btVector3 & rayNormalizedDirection,const btVector3 & a,const btVector3 & b,const btVector3 & c,btScalar maxt)1754 btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom,
1755 const btVector3& rayTo,
1756 const btVector3& rayNormalizedDirection,
1757 const btVector3& a,
1758 const btVector3& b,
1759 const btVector3& c,
1760 btScalar maxt)
1761 {
1762 static const btScalar ceps=-SIMD_EPSILON*10;
1763 static const btScalar teps=SIMD_EPSILON*10;
1764
1765 const btVector3 n=btCross(b-a,c-a);
1766 const btScalar d=btDot(a,n);
1767 const btScalar den=btDot(rayNormalizedDirection,n);
1768 if(!btFuzzyZero(den))
1769 {
1770 const btScalar num=btDot(rayFrom,n)-d;
1771 const btScalar t=-num/den;
1772 if((t>teps)&&(t<maxt))
1773 {
1774 const btVector3 hit=rayFrom+rayNormalizedDirection*t;
1775 if( (btDot(n,btCross(a-hit,b-hit))>ceps) &&
1776 (btDot(n,btCross(b-hit,c-hit))>ceps) &&
1777 (btDot(n,btCross(c-hit,a-hit))>ceps))
1778 {
1779 return(t);
1780 }
1781 }
1782 }
1783 return(-1);
1784 }
1785
1786 //
pointersToIndices()1787 void btSoftBody::pointersToIndices()
1788 {
1789 #define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_))
1790 btSoftBody::Node* base=&m_nodes[0];
1791 int i,ni;
1792
1793 for(i=0,ni=m_nodes.size();i<ni;++i)
1794 {
1795 if(m_nodes[i].m_leaf)
1796 {
1797 m_nodes[i].m_leaf->data=*(void**)&i;
1798 }
1799 }
1800 for(i=0,ni=m_links.size();i<ni;++i)
1801 {
1802 m_links[i].m_n[0]=PTR2IDX(m_links[i].m_n[0],base);
1803 m_links[i].m_n[1]=PTR2IDX(m_links[i].m_n[1],base);
1804 }
1805 for(i=0,ni=m_faces.size();i<ni;++i)
1806 {
1807 m_faces[i].m_n[0]=PTR2IDX(m_faces[i].m_n[0],base);
1808 m_faces[i].m_n[1]=PTR2IDX(m_faces[i].m_n[1],base);
1809 m_faces[i].m_n[2]=PTR2IDX(m_faces[i].m_n[2],base);
1810 if(m_faces[i].m_leaf)
1811 {
1812 m_faces[i].m_leaf->data=*(void**)&i;
1813 }
1814 }
1815 for(i=0,ni=m_anchors.size();i<ni;++i)
1816 {
1817 m_anchors[i].m_node=PTR2IDX(m_anchors[i].m_node,base);
1818 }
1819 for(i=0,ni=m_notes.size();i<ni;++i)
1820 {
1821 for(int j=0;j<m_notes[i].m_rank;++j)
1822 {
1823 m_notes[i].m_nodes[j]=PTR2IDX(m_notes[i].m_nodes[j],base);
1824 }
1825 }
1826 #undef PTR2IDX
1827 }
1828
1829 //
indicesToPointers(const int * map)1830 void btSoftBody::indicesToPointers(const int* map)
1831 {
1832 #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \
1833 (&(_b_)[(((char*)_p_)-(char*)0)])
1834 btSoftBody::Node* base=&m_nodes[0];
1835 int i,ni;
1836
1837 for(i=0,ni=m_nodes.size();i<ni;++i)
1838 {
1839 if(m_nodes[i].m_leaf)
1840 {
1841 m_nodes[i].m_leaf->data=&m_nodes[i];
1842 }
1843 }
1844 for(i=0,ni=m_links.size();i<ni;++i)
1845 {
1846 m_links[i].m_n[0]=IDX2PTR(m_links[i].m_n[0],base);
1847 m_links[i].m_n[1]=IDX2PTR(m_links[i].m_n[1],base);
1848 }
1849 for(i=0,ni=m_faces.size();i<ni;++i)
1850 {
1851 m_faces[i].m_n[0]=IDX2PTR(m_faces[i].m_n[0],base);
1852 m_faces[i].m_n[1]=IDX2PTR(m_faces[i].m_n[1],base);
1853 m_faces[i].m_n[2]=IDX2PTR(m_faces[i].m_n[2],base);
1854 if(m_faces[i].m_leaf)
1855 {
1856 m_faces[i].m_leaf->data=&m_faces[i];
1857 }
1858 }
1859 for(i=0,ni=m_anchors.size();i<ni;++i)
1860 {
1861 m_anchors[i].m_node=IDX2PTR(m_anchors[i].m_node,base);
1862 }
1863 for(i=0,ni=m_notes.size();i<ni;++i)
1864 {
1865 for(int j=0;j<m_notes[i].m_rank;++j)
1866 {
1867 m_notes[i].m_nodes[j]=IDX2PTR(m_notes[i].m_nodes[j],base);
1868 }
1869 }
1870 #undef IDX2PTR
1871 }
1872
1873 //
rayTest(const btVector3 & rayFrom,const btVector3 & rayTo,btScalar & mint,eFeature::_ & feature,int & index,bool bcountonly) const1874 int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo,
1875 btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const
1876 {
1877 int cnt=0;
1878 if(bcountonly||m_fdbvt.empty())
1879 {/* Full search */
1880 btVector3 dir = rayTo-rayFrom;
1881 dir.normalize();
1882
1883 for(int i=0,ni=m_faces.size();i<ni;++i)
1884 {
1885 const btSoftBody::Face& f=m_faces[i];
1886
1887 const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir,
1888 f.m_n[0]->m_x,
1889 f.m_n[1]->m_x,
1890 f.m_n[2]->m_x,
1891 mint);
1892 if(t>0)
1893 {
1894 ++cnt;
1895 if(!bcountonly)
1896 {
1897 feature=btSoftBody::eFeature::Face;
1898 index=i;
1899 mint=t;
1900 }
1901 }
1902 }
1903 }
1904 else
1905 {/* Use dbvt */
1906 RayFromToCaster collider(rayFrom,rayTo,mint);
1907
1908 btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider);
1909 if(collider.m_face)
1910 {
1911 mint=collider.m_mint;
1912 feature=btSoftBody::eFeature::Face;
1913 index=(int)(collider.m_face-&m_faces[0]);
1914 cnt=1;
1915 }
1916 }
1917 return(cnt);
1918 }
1919
1920 //
initializeFaceTree()1921 void btSoftBody::initializeFaceTree()
1922 {
1923 m_fdbvt.clear();
1924 for(int i=0;i<m_faces.size();++i)
1925 {
1926 Face& f=m_faces[i];
1927 f.m_leaf=m_fdbvt.insert(VolumeOf(f,0),&f);
1928 }
1929 }
1930
1931 //
evaluateCom() const1932 btVector3 btSoftBody::evaluateCom() const
1933 {
1934 btVector3 com(0,0,0);
1935 if(m_pose.m_bframe)
1936 {
1937 for(int i=0,ni=m_nodes.size();i<ni;++i)
1938 {
1939 com+=m_nodes[i].m_x*m_pose.m_wgh[i];
1940 }
1941 }
1942 return(com);
1943 }
1944
1945 //
checkContact(btCollisionObject * colObj,const btVector3 & x,btScalar margin,btSoftBody::sCti & cti) const1946 bool btSoftBody::checkContact( btCollisionObject* colObj,
1947 const btVector3& x,
1948 btScalar margin,
1949 btSoftBody::sCti& cti) const
1950 {
1951 btVector3 nrm;
1952 btCollisionShape* shp=colObj->getCollisionShape();
1953 btRigidBody* tmpRigid = btRigidBody::upcast(colObj);
1954 const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform();
1955 btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x),
1956 shp,
1957 nrm,
1958 margin);
1959 if(dst<0)
1960 {
1961 cti.m_colObj = colObj;
1962 cti.m_normal = wtr.getBasis()*nrm;
1963 cti.m_offset = -btDot( cti.m_normal,
1964 x-cti.m_normal*dst);
1965 return(true);
1966 }
1967 return(false);
1968 }
1969
1970 //
updateNormals()1971 void btSoftBody::updateNormals()
1972 {
1973 const btVector3 zv(0,0,0);
1974 int i,ni;
1975
1976 for(i=0,ni=m_nodes.size();i<ni;++i)
1977 {
1978 m_nodes[i].m_n=zv;
1979 }
1980 for(i=0,ni=m_faces.size();i<ni;++i)
1981 {
1982 btSoftBody::Face& f=m_faces[i];
1983 const btVector3 n=btCross(f.m_n[1]->m_x-f.m_n[0]->m_x,
1984 f.m_n[2]->m_x-f.m_n[0]->m_x);
1985 f.m_normal=n.normalized();
1986 f.m_n[0]->m_n+=n;
1987 f.m_n[1]->m_n+=n;
1988 f.m_n[2]->m_n+=n;
1989 }
1990 for(i=0,ni=m_nodes.size();i<ni;++i)
1991 {
1992 btScalar len = m_nodes[i].m_n.length();
1993 if (len>SIMD_EPSILON)
1994 m_nodes[i].m_n /= len;
1995 }
1996 }
1997
1998 //
updateBounds()1999 void btSoftBody::updateBounds()
2000 {
2001 if(m_ndbvt.m_root)
2002 {
2003 const btVector3& mins=m_ndbvt.m_root->volume.Mins();
2004 const btVector3& maxs=m_ndbvt.m_root->volume.Maxs();
2005 const btScalar csm=getCollisionShape()->getMargin();
2006 const btVector3 mrg=btVector3( csm,
2007 csm,
2008 csm)*1; // ??? to investigate...
2009 m_bounds[0]=mins-mrg;
2010 m_bounds[1]=maxs+mrg;
2011 if(0!=getBroadphaseHandle())
2012 {
2013 m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(),
2014 m_bounds[0],
2015 m_bounds[1],
2016 m_worldInfo->m_dispatcher);
2017 }
2018 }
2019 else
2020 {
2021 m_bounds[0]=
2022 m_bounds[1]=btVector3(0,0,0);
2023 }
2024 }
2025
2026
2027 //
updatePose()2028 void btSoftBody::updatePose()
2029 {
2030 if(m_pose.m_bframe)
2031 {
2032 btSoftBody::Pose& pose=m_pose;
2033 const btVector3 com=evaluateCom();
2034 /* Com */
2035 pose.m_com = com;
2036 /* Rotation */
2037 btMatrix3x3 Apq;
2038 const btScalar eps=SIMD_EPSILON;
2039 Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0);
2040 Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3);
2041 for(int i=0,ni=m_nodes.size();i<ni;++i)
2042 {
2043 const btVector3 a=pose.m_wgh[i]*(m_nodes[i].m_x-com);
2044 const btVector3& b=pose.m_pos[i];
2045 Apq[0]+=a.x()*b;
2046 Apq[1]+=a.y()*b;
2047 Apq[2]+=a.z()*b;
2048 }
2049 btMatrix3x3 r,s;
2050 PolarDecompose(Apq,r,s);
2051 pose.m_rot=r;
2052 pose.m_scl=pose.m_aqq*r.transpose()*Apq;
2053 if(m_cfg.maxvolume>1)
2054 {
2055 const btScalar idet=Clamp<btScalar>( 1/pose.m_scl.determinant(),
2056 1,m_cfg.maxvolume);
2057 pose.m_scl=Mul(pose.m_scl,idet);
2058 }
2059
2060 }
2061 }
2062
2063 //
updateConstants()2064 void btSoftBody::updateConstants()
2065 {
2066 int i,ni;
2067
2068 /* Links */
2069 for(i=0,ni=m_links.size();i<ni;++i)
2070 {
2071 Link& l=m_links[i];
2072 Material& m=*l.m_material;
2073 l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length();
2074 l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST;
2075 l.m_c1 = l.m_rl*l.m_rl;
2076 }
2077 /* Faces */
2078 for(i=0,ni=m_faces.size();i<ni;++i)
2079 {
2080 Face& f=m_faces[i];
2081 f.m_ra = AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
2082 }
2083 /* Area's */
2084 btAlignedObjectArray<int> counts;
2085 counts.resize(m_nodes.size(),0);
2086 for(i=0,ni=m_nodes.size();i<ni;++i)
2087 {
2088 m_nodes[i].m_area = 0;
2089 }
2090 for(i=0,ni=m_faces.size();i<ni;++i)
2091 {
2092 btSoftBody::Face& f=m_faces[i];
2093 for(int j=0;j<3;++j)
2094 {
2095 const int index=(int)(f.m_n[j]-&m_nodes[0]);
2096 counts[index]++;
2097 f.m_n[j]->m_area+=btFabs(f.m_ra);
2098 }
2099 }
2100 for(i=0,ni=m_nodes.size();i<ni;++i)
2101 {
2102 if(counts[i]>0)
2103 m_nodes[i].m_area/=(btScalar)counts[i];
2104 else
2105 m_nodes[i].m_area=0;
2106 }
2107 }
2108
2109 //
initializeClusters()2110 void btSoftBody::initializeClusters()
2111 {
2112 int i;
2113
2114 for( i=0;i<m_clusters.size();++i)
2115 {
2116 Cluster& c=*m_clusters[i];
2117 c.m_imass=0;
2118 c.m_masses.resize(c.m_nodes.size());
2119 for(int j=0;j<c.m_nodes.size();++j)
2120 {
2121 if (c.m_nodes[j]->m_im==0)
2122 {
2123 c.m_containsAnchor = true;
2124 c.m_masses[j] = BT_LARGE_FLOAT;
2125 } else
2126 {
2127 c.m_masses[j] = btScalar(1.)/c.m_nodes[j]->m_im;
2128 }
2129 c.m_imass += c.m_masses[j];
2130 }
2131 c.m_imass = btScalar(1.)/c.m_imass;
2132 c.m_com = btSoftBody::clusterCom(&c);
2133 c.m_lv = btVector3(0,0,0);
2134 c.m_av = btVector3(0,0,0);
2135 c.m_leaf = 0;
2136 /* Inertia */
2137 btMatrix3x3& ii=c.m_locii;
2138 ii[0]=ii[1]=ii[2]=btVector3(0,0,0);
2139 {
2140 int i,ni;
2141
2142 for(i=0,ni=c.m_nodes.size();i<ni;++i)
2143 {
2144 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2145 const btVector3 q=k*k;
2146 const btScalar m=c.m_masses[i];
2147 ii[0][0] += m*(q[1]+q[2]);
2148 ii[1][1] += m*(q[0]+q[2]);
2149 ii[2][2] += m*(q[0]+q[1]);
2150 ii[0][1] -= m*k[0]*k[1];
2151 ii[0][2] -= m*k[0]*k[2];
2152 ii[1][2] -= m*k[1]*k[2];
2153 }
2154 }
2155 ii[1][0]=ii[0][1];
2156 ii[2][0]=ii[0][2];
2157 ii[2][1]=ii[1][2];
2158
2159 ii = ii.inverse();
2160
2161 /* Frame */
2162 c.m_framexform.setIdentity();
2163 c.m_framexform.setOrigin(c.m_com);
2164 c.m_framerefs.resize(c.m_nodes.size());
2165 {
2166 int i;
2167 for(i=0;i<c.m_framerefs.size();++i)
2168 {
2169 c.m_framerefs[i]=c.m_nodes[i]->m_x-c.m_com;
2170 }
2171 }
2172 }
2173 }
2174
2175 //
updateClusters()2176 void btSoftBody::updateClusters()
2177 {
2178 BT_PROFILE("UpdateClusters");
2179 int i;
2180
2181 for(i=0;i<m_clusters.size();++i)
2182 {
2183 btSoftBody::Cluster& c=*m_clusters[i];
2184 const int n=c.m_nodes.size();
2185 //const btScalar invn=1/(btScalar)n;
2186 if(n)
2187 {
2188 /* Frame */
2189 const btScalar eps=btScalar(0.0001);
2190 btMatrix3x3 m,r,s;
2191 m[0]=m[1]=m[2]=btVector3(0,0,0);
2192 m[0][0]=eps*1;
2193 m[1][1]=eps*2;
2194 m[2][2]=eps*3;
2195 c.m_com=clusterCom(&c);
2196 for(int i=0;i<c.m_nodes.size();++i)
2197 {
2198 const btVector3 a=c.m_nodes[i]->m_x-c.m_com;
2199 const btVector3& b=c.m_framerefs[i];
2200 m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b;
2201 }
2202 PolarDecompose(m,r,s);
2203 c.m_framexform.setOrigin(c.m_com);
2204 c.m_framexform.setBasis(r);
2205 /* Inertia */
2206 #if 1/* Constant */
2207 c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose();
2208 #else
2209 #if 0/* Sphere */
2210 const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
2211 const btVector3 inertia(rk,rk,rk);
2212 const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2213 btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2214 btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2215
2216 c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2217 #else/* Actual */
2218 c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0);
2219 for(int i=0;i<n;++i)
2220 {
2221 const btVector3 k=c.m_nodes[i]->m_x-c.m_com;
2222 const btVector3 q=k*k;
2223 const btScalar m=1/c.m_nodes[i]->m_im;
2224 c.m_invwi[0][0] += m*(q[1]+q[2]);
2225 c.m_invwi[1][1] += m*(q[0]+q[2]);
2226 c.m_invwi[2][2] += m*(q[0]+q[1]);
2227 c.m_invwi[0][1] -= m*k[0]*k[1];
2228 c.m_invwi[0][2] -= m*k[0]*k[2];
2229 c.m_invwi[1][2] -= m*k[1]*k[2];
2230 }
2231 c.m_invwi[1][0]=c.m_invwi[0][1];
2232 c.m_invwi[2][0]=c.m_invwi[0][2];
2233 c.m_invwi[2][1]=c.m_invwi[1][2];
2234 c.m_invwi=c.m_invwi.inverse();
2235 #endif
2236 #endif
2237 /* Velocities */
2238 c.m_lv=btVector3(0,0,0);
2239 c.m_av=btVector3(0,0,0);
2240 {
2241 int i;
2242
2243 for(i=0;i<n;++i)
2244 {
2245 const btVector3 v=c.m_nodes[i]->m_v*c.m_masses[i];
2246 c.m_lv += v;
2247 c.m_av += btCross(c.m_nodes[i]->m_x-c.m_com,v);
2248 }
2249 }
2250 c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping);
2251 c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping);
2252 c.m_vimpulses[0] =
2253 c.m_vimpulses[1] = btVector3(0,0,0);
2254 c.m_dimpulses[0] =
2255 c.m_dimpulses[1] = btVector3(0,0,0);
2256 c.m_nvimpulses = 0;
2257 c.m_ndimpulses = 0;
2258 /* Matching */
2259 if(c.m_matching>0)
2260 {
2261 for(int j=0;j<c.m_nodes.size();++j)
2262 {
2263 Node& n=*c.m_nodes[j];
2264 const btVector3 x=c.m_framexform*c.m_framerefs[j];
2265 n.m_x=Lerp(n.m_x,x,c.m_matching);
2266 }
2267 }
2268 /* Dbvt */
2269 if(c.m_collide)
2270 {
2271 btVector3 mi=c.m_nodes[0]->m_x;
2272 btVector3 mx=mi;
2273 for(int j=1;j<n;++j)
2274 {
2275 mi.setMin(c.m_nodes[j]->m_x);
2276 mx.setMax(c.m_nodes[j]->m_x);
2277 }
2278 ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx);
2279 if(c.m_leaf)
2280 m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg);
2281 else
2282 c.m_leaf=m_cdbvt.insert(bounds,&c);
2283 }
2284 }
2285 }
2286
2287
2288 }
2289
2290
2291
2292
2293 //
cleanupClusters()2294 void btSoftBody::cleanupClusters()
2295 {
2296 for(int i=0;i<m_joints.size();++i)
2297 {
2298 m_joints[i]->Terminate(m_sst.sdt);
2299 if(m_joints[i]->m_delete)
2300 {
2301 btAlignedFree(m_joints[i]);
2302 m_joints.remove(m_joints[i--]);
2303 }
2304 }
2305 }
2306
2307 //
prepareClusters(int iterations)2308 void btSoftBody::prepareClusters(int iterations)
2309 {
2310 for(int i=0;i<m_joints.size();++i)
2311 {
2312 m_joints[i]->Prepare(m_sst.sdt,iterations);
2313 }
2314 }
2315
2316
2317 //
solveClusters(btScalar sor)2318 void btSoftBody::solveClusters(btScalar sor)
2319 {
2320 for(int i=0,ni=m_joints.size();i<ni;++i)
2321 {
2322 m_joints[i]->Solve(m_sst.sdt,sor);
2323 }
2324 }
2325
2326 //
applyClusters(bool drift)2327 void btSoftBody::applyClusters(bool drift)
2328 {
2329 BT_PROFILE("ApplyClusters");
2330 // const btScalar f0=m_sst.sdt;
2331 //const btScalar f1=f0/2;
2332 btAlignedObjectArray<btVector3> deltas;
2333 btAlignedObjectArray<btScalar> weights;
2334 deltas.resize(m_nodes.size(),btVector3(0,0,0));
2335 weights.resize(m_nodes.size(),0);
2336 int i;
2337
2338 if(drift)
2339 {
2340 for(i=0;i<m_clusters.size();++i)
2341 {
2342 Cluster& c=*m_clusters[i];
2343 if(c.m_ndimpulses)
2344 {
2345 c.m_dimpulses[0]/=(btScalar)c.m_ndimpulses;
2346 c.m_dimpulses[1]/=(btScalar)c.m_ndimpulses;
2347 }
2348 }
2349 }
2350
2351 for(i=0;i<m_clusters.size();++i)
2352 {
2353 Cluster& c=*m_clusters[i];
2354 if(0<(drift?c.m_ndimpulses:c.m_nvimpulses))
2355 {
2356 const btVector3 v=(drift?c.m_dimpulses[0]:c.m_vimpulses[0])*m_sst.sdt;
2357 const btVector3 w=(drift?c.m_dimpulses[1]:c.m_vimpulses[1])*m_sst.sdt;
2358 for(int j=0;j<c.m_nodes.size();++j)
2359 {
2360 const int idx=int(c.m_nodes[j]-&m_nodes[0]);
2361 const btVector3& x=c.m_nodes[j]->m_x;
2362 const btScalar q=c.m_masses[j];
2363 deltas[idx] += (v+btCross(w,x-c.m_com))*q;
2364 weights[idx] += q;
2365 }
2366 }
2367 }
2368 for(i=0;i<deltas.size();++i)
2369 {
2370 if(weights[i]>0) m_nodes[i].m_x+=deltas[i]/weights[i];
2371 }
2372 }
2373
2374 //
dampClusters()2375 void btSoftBody::dampClusters()
2376 {
2377 int i;
2378
2379 for(i=0;i<m_clusters.size();++i)
2380 {
2381 Cluster& c=*m_clusters[i];
2382 if(c.m_ndamping>0)
2383 {
2384 for(int j=0;j<c.m_nodes.size();++j)
2385 {
2386 Node& n=*c.m_nodes[j];
2387 if(n.m_im>0)
2388 {
2389 const btVector3 vx=c.m_lv+btCross(c.m_av,c.m_nodes[j]->m_q-c.m_com);
2390 if(vx.length2()<=n.m_v.length2())
2391 {
2392 n.m_v += c.m_ndamping*(vx-n.m_v);
2393 }
2394 }
2395 }
2396 }
2397 }
2398 }
2399
2400 //
Prepare(btScalar dt,int)2401 void btSoftBody::Joint::Prepare(btScalar dt,int)
2402 {
2403 m_bodies[0].activate();
2404 m_bodies[1].activate();
2405 }
2406
2407 //
Prepare(btScalar dt,int iterations)2408 void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
2409 {
2410 static const btScalar maxdrift=4;
2411 Joint::Prepare(dt,iterations);
2412 m_rpos[0] = m_bodies[0].xform()*m_refs[0];
2413 m_rpos[1] = m_bodies[1].xform()*m_refs[1];
2414 m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt;
2415 m_rpos[0] -= m_bodies[0].xform().getOrigin();
2416 m_rpos[1] -= m_bodies[1].xform().getOrigin();
2417 m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0],
2418 m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]);
2419 if(m_split>0)
2420 {
2421 m_sdrift = m_massmatrix*(m_drift*m_split);
2422 m_drift *= 1-m_split;
2423 }
2424 m_drift /=(btScalar)iterations;
2425 }
2426
2427 //
Solve(btScalar dt,btScalar sor)2428 void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor)
2429 {
2430 const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2431 const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2432 const btVector3 vr=va-vb;
2433 btSoftBody::Impulse impulse;
2434 impulse.m_asVelocity = 1;
2435 impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor;
2436 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2437 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2438 }
2439
2440 //
Terminate(btScalar dt)2441 void btSoftBody::LJoint::Terminate(btScalar dt)
2442 {
2443 if(m_split>0)
2444 {
2445 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2446 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2447 }
2448 }
2449
2450 //
Prepare(btScalar dt,int iterations)2451 void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
2452 {
2453 static const btScalar maxdrift=SIMD_PI/16;
2454 m_icontrol->Prepare(this);
2455 Joint::Prepare(dt,iterations);
2456 m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0];
2457 m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1];
2458 m_drift = NormalizeAny(btCross(m_axis[1],m_axis[0]));
2459 m_drift *= btMin(maxdrift,btAcos(Clamp<btScalar>(btDot(m_axis[0],m_axis[1]),-1,+1)));
2460 m_drift *= m_erp/dt;
2461 m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia());
2462 if(m_split>0)
2463 {
2464 m_sdrift = m_massmatrix*(m_drift*m_split);
2465 m_drift *= 1-m_split;
2466 }
2467 m_drift /=(btScalar)iterations;
2468 }
2469
2470 //
Solve(btScalar dt,btScalar sor)2471 void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor)
2472 {
2473 const btVector3 va=m_bodies[0].angularVelocity();
2474 const btVector3 vb=m_bodies[1].angularVelocity();
2475 const btVector3 vr=va-vb;
2476 const btScalar sp=btDot(vr,m_axis[0]);
2477 const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp);
2478 btSoftBody::Impulse impulse;
2479 impulse.m_asVelocity = 1;
2480 impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor;
2481 m_bodies[0].applyAImpulse(-impulse);
2482 m_bodies[1].applyAImpulse( impulse);
2483 }
2484
2485 //
Terminate(btScalar dt)2486 void btSoftBody::AJoint::Terminate(btScalar dt)
2487 {
2488 if(m_split>0)
2489 {
2490 m_bodies[0].applyDAImpulse(-m_sdrift);
2491 m_bodies[1].applyDAImpulse( m_sdrift);
2492 }
2493 }
2494
2495 //
Prepare(btScalar dt,int iterations)2496 void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
2497 {
2498 Joint::Prepare(dt,iterations);
2499 const bool dodrift=(m_life==0);
2500 m_delete=(++m_life)>m_maxlife;
2501 if(dodrift)
2502 {
2503 m_drift=m_drift*m_erp/dt;
2504 if(m_split>0)
2505 {
2506 m_sdrift = m_massmatrix*(m_drift*m_split);
2507 m_drift *= 1-m_split;
2508 }
2509 m_drift/=(btScalar)iterations;
2510 }
2511 else
2512 {
2513 m_drift=m_sdrift=btVector3(0,0,0);
2514 }
2515 }
2516
2517 //
Solve(btScalar dt,btScalar sor)2518 void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor)
2519 {
2520 const btVector3 va=m_bodies[0].velocity(m_rpos[0]);
2521 const btVector3 vb=m_bodies[1].velocity(m_rpos[1]);
2522 const btVector3 vrel=va-vb;
2523 const btScalar rvac=btDot(vrel,m_normal);
2524 btSoftBody::Impulse impulse;
2525 impulse.m_asVelocity = 1;
2526 impulse.m_velocity = m_drift;
2527 if(rvac<0)
2528 {
2529 const btVector3 iv=m_normal*rvac;
2530 const btVector3 fv=vrel-iv;
2531 impulse.m_velocity += iv+fv*m_friction;
2532 }
2533 impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor;
2534
2535 if (m_bodies[0].m_soft==m_bodies[1].m_soft)
2536 {
2537 if ((impulse.m_velocity.getX() ==impulse.m_velocity.getX())&&(impulse.m_velocity.getY() ==impulse.m_velocity.getY())&&
2538 (impulse.m_velocity.getZ() ==impulse.m_velocity.getZ()))
2539 {
2540 if (impulse.m_asVelocity)
2541 {
2542 if (impulse.m_velocity.length() <m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
2543 {
2544
2545 } else
2546 {
2547 m_bodies[0].applyImpulse(-impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[0]);
2548 m_bodies[1].applyImpulse( impulse*m_bodies[0].m_soft->m_selfCollisionImpulseFactor,m_rpos[1]);
2549 }
2550 }
2551 }
2552 } else
2553 {
2554 m_bodies[0].applyImpulse(-impulse,m_rpos[0]);
2555 m_bodies[1].applyImpulse( impulse,m_rpos[1]);
2556 }
2557 }
2558
2559 //
Terminate(btScalar dt)2560 void btSoftBody::CJoint::Terminate(btScalar dt)
2561 {
2562 if(m_split>0)
2563 {
2564 m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]);
2565 m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]);
2566 }
2567 }
2568
2569 //
applyForces()2570 void btSoftBody::applyForces()
2571 {
2572
2573 BT_PROFILE("SoftBody applyForces");
2574 const btScalar dt=m_sst.sdt;
2575 const btScalar kLF=m_cfg.kLF;
2576 const btScalar kDG=m_cfg.kDG;
2577 const btScalar kPR=m_cfg.kPR;
2578 const btScalar kVC=m_cfg.kVC;
2579 const bool as_lift=kLF>0;
2580 const bool as_drag=kDG>0;
2581 const bool as_pressure=kPR!=0;
2582 const bool as_volume=kVC>0;
2583 const bool as_aero= as_lift ||
2584 as_drag ;
2585 const bool as_vaero= as_aero &&
2586 (m_cfg.aeromodel<btSoftBody::eAeroModel::F_TwoSided);
2587 const bool as_faero= as_aero &&
2588 (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided);
2589 const bool use_medium= as_aero;
2590 const bool use_volume= as_pressure ||
2591 as_volume ;
2592 btScalar volume=0;
2593 btScalar ivolumetp=0;
2594 btScalar dvolumetv=0;
2595 btSoftBody::sMedium medium;
2596 if(use_volume)
2597 {
2598 volume = getVolume();
2599 ivolumetp = 1/btFabs(volume)*kPR;
2600 dvolumetv = (m_pose.m_volume-volume)*kVC;
2601 }
2602 /* Per vertex forces */
2603 int i,ni;
2604
2605 for(i=0,ni=m_nodes.size();i<ni;++i)
2606 {
2607 btSoftBody::Node& n=m_nodes[i];
2608 if(n.m_im>0)
2609 {
2610 if(use_medium)
2611 {
2612 EvaluateMedium(m_worldInfo,n.m_x,medium);
2613 /* Aerodynamics */
2614 if(as_vaero)
2615 {
2616 const btVector3 rel_v=n.m_v-medium.m_velocity;
2617 const btScalar rel_v2=rel_v.length2();
2618 if(rel_v2>SIMD_EPSILON)
2619 {
2620 btVector3 nrm=n.m_n;
2621 /* Setup normal */
2622 switch(m_cfg.aeromodel)
2623 {
2624 case btSoftBody::eAeroModel::V_Point:
2625 nrm=NormalizeAny(rel_v);break;
2626 case btSoftBody::eAeroModel::V_TwoSided:
2627 nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
2628 default:
2629 {
2630 }
2631 }
2632 const btScalar dvn=btDot(rel_v,nrm);
2633 /* Compute forces */
2634 if(dvn>0)
2635 {
2636 btVector3 force(0,0,0);
2637 const btScalar c0 = n.m_area*dvn*rel_v2/2;
2638 const btScalar c1 = c0*medium.m_density;
2639 force += nrm*(-c1*kLF);
2640 force += rel_v.normalized()*(-c1*kDG);
2641 ApplyClampedForce(n,force,dt);
2642 }
2643 }
2644 }
2645 }
2646 /* Pressure */
2647 if(as_pressure)
2648 {
2649 n.m_f += n.m_n*(n.m_area*ivolumetp);
2650 }
2651 /* Volume */
2652 if(as_volume)
2653 {
2654 n.m_f += n.m_n*(n.m_area*dvolumetv);
2655 }
2656 }
2657 }
2658 /* Per face forces */
2659 for(i=0,ni=m_faces.size();i<ni;++i)
2660 {
2661 btSoftBody::Face& f=m_faces[i];
2662 if(as_faero)
2663 {
2664 const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3;
2665 const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3;
2666 EvaluateMedium(m_worldInfo,x,medium);
2667 const btVector3 rel_v=v-medium.m_velocity;
2668 const btScalar rel_v2=rel_v.length2();
2669 if(rel_v2>SIMD_EPSILON)
2670 {
2671 btVector3 nrm=f.m_normal;
2672 /* Setup normal */
2673 switch(m_cfg.aeromodel)
2674 {
2675 case btSoftBody::eAeroModel::F_TwoSided:
2676 nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break;
2677 default:
2678 {
2679 }
2680 }
2681 const btScalar dvn=btDot(rel_v,nrm);
2682 /* Compute forces */
2683 if(dvn>0)
2684 {
2685 btVector3 force(0,0,0);
2686 const btScalar c0 = f.m_ra*dvn*rel_v2;
2687 const btScalar c1 = c0*medium.m_density;
2688 force += nrm*(-c1*kLF);
2689 force += rel_v.normalized()*(-c1*kDG);
2690 force /= 3;
2691 for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt);
2692 }
2693 }
2694 }
2695 }
2696 }
2697
2698 //
PSolve_Anchors(btSoftBody * psb,btScalar kst,btScalar ti)2699 void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti)
2700 {
2701 const btScalar kAHR=psb->m_cfg.kAHR*kst;
2702 const btScalar dt=psb->m_sst.sdt;
2703 for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
2704 {
2705 const Anchor& a=psb->m_anchors[i];
2706 const btTransform& t=a.m_body->getInterpolationWorldTransform();
2707 Node& n=*a.m_node;
2708 const btVector3 wa=t*a.m_local;
2709 const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt;
2710 const btVector3 vb=n.m_x-n.m_q;
2711 const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR;
2712 const btVector3 impulse=a.m_c0*vr;
2713 n.m_x+=impulse*a.m_c2;
2714 a.m_body->applyImpulse(-impulse,a.m_c1);
2715 }
2716 }
2717
2718 //
PSolve_RContacts(btSoftBody * psb,btScalar kst,btScalar ti)2719 void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti)
2720 {
2721 const btScalar dt=psb->m_sst.sdt;
2722 const btScalar mrg=psb->getCollisionShape()->getMargin();
2723 for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
2724 {
2725 const RContact& c=psb->m_rcontacts[i];
2726 const sCti& cti=c.m_cti;
2727 btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj);
2728
2729 const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
2730 const btVector3 vb=c.m_node->m_x-c.m_node->m_q;
2731 const btVector3 vr=vb-va;
2732 const btScalar dn=btDot(vr,cti.m_normal);
2733 if(dn<=SIMD_EPSILON)
2734 {
2735 const btScalar dp=btMin(btDot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
2736 const btVector3 fv=vr-cti.m_normal*dn;
2737 const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
2738 c.m_node->m_x-=impulse*c.m_c2;
2739 if (tmpRigid)
2740 tmpRigid->applyImpulse(impulse,c.m_c1);
2741 }
2742 }
2743 }
2744
2745 //
PSolve_SContacts(btSoftBody * psb,btScalar,btScalar ti)2746 void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti)
2747 {
2748 for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
2749 {
2750 const SContact& c=psb->m_scontacts[i];
2751 const btVector3& nr=c.m_normal;
2752 Node& n=*c.m_node;
2753 Face& f=*c.m_face;
2754 const btVector3 p=BaryEval( f.m_n[0]->m_x,
2755 f.m_n[1]->m_x,
2756 f.m_n[2]->m_x,
2757 c.m_weights);
2758 const btVector3 q=BaryEval( f.m_n[0]->m_q,
2759 f.m_n[1]->m_q,
2760 f.m_n[2]->m_q,
2761 c.m_weights);
2762 const btVector3 vr=(n.m_x-n.m_q)-(p-q);
2763 btVector3 corr(0,0,0);
2764 btScalar dot = btDot(vr,nr);
2765 if(dot<0)
2766 {
2767 const btScalar j=c.m_margin-(btDot(nr,n.m_x)-btDot(nr,p));
2768 corr+=c.m_normal*j;
2769 }
2770 corr -= ProjectOnPlane(vr,nr)*c.m_friction;
2771 n.m_x += corr*c.m_cfm[0];
2772 f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x());
2773 f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y());
2774 f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z());
2775 }
2776 }
2777
2778 //
PSolve_Links(btSoftBody * psb,btScalar kst,btScalar ti)2779 void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti)
2780 {
2781 for(int i=0,ni=psb->m_links.size();i<ni;++i)
2782 {
2783 Link& l=psb->m_links[i];
2784 if(l.m_c0>0)
2785 {
2786 Node& a=*l.m_n[0];
2787 Node& b=*l.m_n[1];
2788 const btVector3 del=b.m_x-a.m_x;
2789 const btScalar len=del.length2();
2790 const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
2791 //const btScalar t=k*a.m_im;
2792 a.m_x-=del*(k*a.m_im);
2793 b.m_x+=del*(k*b.m_im);
2794 }
2795 }
2796 }
2797
2798 //
VSolve_Links(btSoftBody * psb,btScalar kst)2799 void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst)
2800 {
2801 for(int i=0,ni=psb->m_links.size();i<ni;++i)
2802 {
2803 Link& l=psb->m_links[i];
2804 Node** n=l.m_n;
2805 const btScalar j=-btDot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
2806 n[0]->m_v+= l.m_c3*(j*n[0]->m_im);
2807 n[1]->m_v-= l.m_c3*(j*n[1]->m_im);
2808 }
2809 }
2810
2811 //
getSolver(ePSolver::_ solver)2812 btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver)
2813 {
2814 switch(solver)
2815 {
2816 case ePSolver::Anchors:
2817 return(&btSoftBody::PSolve_Anchors);
2818 case ePSolver::Linear:
2819 return(&btSoftBody::PSolve_Links);
2820 case ePSolver::RContacts:
2821 return(&btSoftBody::PSolve_RContacts);
2822 case ePSolver::SContacts:
2823 return(&btSoftBody::PSolve_SContacts);
2824 default:
2825 {
2826 }
2827 }
2828 return(0);
2829 }
2830
2831 //
getSolver(eVSolver::_ solver)2832 btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
2833 {
2834 switch(solver)
2835 {
2836 case eVSolver::Linear: return(&btSoftBody::VSolve_Links);
2837 default:
2838 {
2839 }
2840 }
2841 return(0);
2842 }
2843
2844 //
defaultCollisionHandler(btCollisionObject * pco)2845 void btSoftBody::defaultCollisionHandler(btCollisionObject* pco)
2846 {
2847 switch(m_cfg.collisions&fCollision::RVSmask)
2848 {
2849 case fCollision::SDF_RS:
2850 {
2851 btSoftColliders::CollideSDF_RS docollide;
2852 btRigidBody* prb1=btRigidBody::upcast(pco);
2853 btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform();
2854
2855 const btTransform ctr=pco->getWorldTransform();
2856 const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length();
2857 const btScalar basemargin=getCollisionShape()->getMargin();
2858 btVector3 mins;
2859 btVector3 maxs;
2860 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume;
2861 pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(),
2862 mins,
2863 maxs);
2864 volume=btDbvtVolume::FromMM(mins,maxs);
2865 volume.Expand(btVector3(basemargin,basemargin,basemargin));
2866 docollide.psb = this;
2867 docollide.m_colObj1 = pco;
2868 docollide.m_rigidBody = prb1;
2869
2870 docollide.dynmargin = basemargin+timemargin;
2871 docollide.stamargin = basemargin;
2872 m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide);
2873 }
2874 break;
2875 case fCollision::CL_RS:
2876 {
2877 btSoftColliders::CollideCL_RS collider;
2878 collider.Process(this,pco);
2879 }
2880 break;
2881 }
2882 }
2883
2884 //
defaultCollisionHandler(btSoftBody * psb)2885 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
2886 {
2887 const int cf=m_cfg.collisions&psb->m_cfg.collisions;
2888 switch(cf&fCollision::SVSmask)
2889 {
2890 case fCollision::CL_SS:
2891 {
2892
2893 //support self-collision if CL_SELF flag set
2894 if (this!=psb || psb->m_cfg.collisions&fCollision::CL_SELF)
2895 {
2896 btSoftColliders::CollideCL_SS docollide;
2897 docollide.Process(this,psb);
2898 }
2899
2900 }
2901 break;
2902 case fCollision::VF_SS:
2903 {
2904 //only self-collision for Cluster, not Vertex-Face yet
2905 if (this!=psb)
2906 {
2907 btSoftColliders::CollideVF_SS docollide;
2908 /* common */
2909 docollide.mrg= getCollisionShape()->getMargin()+
2910 psb->getCollisionShape()->getMargin();
2911 /* psb0 nodes vs psb1 faces */
2912 docollide.psb[0]=this;
2913 docollide.psb[1]=psb;
2914 docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
2915 docollide.psb[1]->m_fdbvt.m_root,
2916 docollide);
2917 /* psb1 nodes vs psb0 faces */
2918 docollide.psb[0]=psb;
2919 docollide.psb[1]=this;
2920 docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root,
2921 docollide.psb[1]->m_fdbvt.m_root,
2922 docollide);
2923 }
2924 }
2925 break;
2926 default:
2927 {
2928
2929 }
2930 }
2931 }
2932