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