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