Bullet Collision Detection & Physics Library
btSoftBody.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16 
17 #include "btSoftBodyInternals.h"
19 #include "btSoftBodyData.h"
23 
24 
25 //
26 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m)
27 :m_softBodySolver(0),m_worldInfo(worldInfo)
28 {
29  /* Init */
30  initDefaults();
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];
45  ZeroInitialize(n);
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;
50  n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n);
51  n.m_material= pm;
52  }
53  updateBounds();
54 
55 }
56 
58 :m_worldInfo(worldInfo)
59 {
60  initDefaults();
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;
81  m_cfg.kSKHR_CL = (btScalar)1;
82  m_cfg.kSSHR_CL = (btScalar)0.5;
83  m_cfg.kSR_SPLT_CL = (btScalar)0.5;
84  m_cfg.kSK_SPLT_CL = (btScalar)0.5;
85  m_cfg.kSS_SPLT_CL = (btScalar)0.5;
87  m_cfg.timescale = 1;
88  m_cfg.viterations = 0;
89  m_cfg.piterations = 1;
90  m_cfg.diterations = 0;
91  m_cfg.citerations = 4;
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 */
110  m_collisionShape->setMargin(0.25f);
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 
125  releaseClusters();
126  for(i=0;i<m_materials.size();++i)
128  for(i=0;i<m_joints.size();++i)
130 }
131 
132 //
133 bool btSoftBody::checkLink(int node0,int node1) const
134 {
135  return(checkLink(&m_nodes[node0],&m_nodes[node1]));
136 }
137 
138 //
139 bool 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 //
155 bool 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 //
188 void 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;
197  ZeroInitialize(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;
209  m_notes.push_back(n);
210 }
211 
212 //
213 void 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 //
221 void 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 //
231 void 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 {
244  if(m_nodes.capacity()==m_nodes.size())
245  {
247  m_nodes.reserve(m_nodes.size()*2+1);
249  }
250  const btScalar margin=getCollisionShape()->getMargin();
252  Node& n=m_nodes[m_nodes.size()-1];
253  ZeroInitialize(n);
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 //
262 void btSoftBody::appendLink(int model,Material* mat)
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]; }
269  m_links.push_back(l);
270 }
271 
272 //
273 void btSoftBody::appendLink( int node0,
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 //
299 void btSoftBody::appendFace(int model,Material* mat)
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]; }
306  m_faces.push_back(f);
307 }
308 
309 //
310 void 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 //
334 void btSoftBody::appendTetra(int model,Material* mat)
335 {
336 Tetra t;
337 if(model>=0)
338  t=m_tetras[model];
339  else
340  { ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
342 }
343 
344 //
345 void btSoftBody::appendTetra(int node0,
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 
363 void 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 //
370 void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence)
371 {
372  if (disableCollisionBetweenLinkedBodies)
373  {
375  {
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;
386  m_anchors.push_back(a);
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;
400  m_joints.push_back(pj);
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;
427  m_joints.push_back(pj);
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 //
443 void btSoftBody::addForce(const btVector3& force)
444 {
445  for(int i=0,ni=m_nodes.size();i<ni;++i) addForce(force,i);
446 }
447 
448 //
449 void 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 
458 void 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 
547 void 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 //
644 void btSoftBody::addVelocity(const btVector3& velocity)
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 */
650 void 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 //
664 void 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 //
674 void 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 //
698 void 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 {
744 ranks.resize(m_nodes.size(),0);
745 int i;
746 
747 for(i=0;i<m_nodes.size();++i)
748  {
749  m_nodes[i].m_im=0;
750  }
751 for(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  }
760 for( 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  }
767 setTotalMass(mass,false);
768 }
769 
770 //
772 {
773 btScalar volume=0;
774 for(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  }
782 setVolumeMass(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  }
801  updateNormals();
802  updateBounds();
803  updateConstants();
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 //
826 void btSoftBody::scale(const btVector3& scl)
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  }
840  updateNormals();
841  updateBounds();
842  updateConstants();
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 //
867 void 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 
914  updateConstants();
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 //
976 void 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 //
986 void 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 //
996 void 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 //
1003 void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse)
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 //
1011 void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse)
1012 {
1013  const btVector3 ai=cluster->m_invwi*impulse;
1014  cluster->m_dimpulses[1]+=ai;
1015  cluster->m_ndimpulses++;
1016 }
1017 
1018 //
1019 void 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 //
1026 void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse)
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  {
1168  btSwap(m_links[i],m_links[NEXTRAND%ni]);
1169  }
1170  for(i=0,ni=m_faces.size();i<ni;++i)
1171  {
1172  btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
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 //
1194 int btSoftBody::generateClusters(int k,int maxiterations)
1195 {
1196  int i;
1197  releaseClusters();
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  {
1351  updateClusters();
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 //
1387 void 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  {
1403  btSwap(m_links[i],m_links[m_links.size()-1]);
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();
1586  btAlignedObjectArray<int> todelete;
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]];
1606  btSwap(m_links[i],m_links[m_links.size()-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 //
1638 bool 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 //
1644 bool 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 //
1699 bool 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;
1750  updateConstants();
1751  m_fdbvt.clear();
1753  {
1754  initializeFaceTree();
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;
1763  m_sst.updmrg = m_sst.radmrg*(btScalar)0.25;
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 */
1793  updateClusters();
1794  /* Bounds */
1795  updateBounds();
1796  /* Nodes */
1798  for(i=0,ni=m_nodes.size();i<ni;++i)
1799  {
1800  Node& n=m_nodes[i];
1802  m_ndbvt.update( n.m_leaf,
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);
1817  m_fdbvt.update( f.m_leaf,
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 */
1840  m_rcontacts.resize(0);
1841  m_scontacts.resize(0);
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];
1869  a.m_c0 = ImpulseMatrix( m_sst.sdt,
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 //
1943 void 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 //
1955 void 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 //
2100 void btSoftBody::indicesToPointers(const int* map)
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 //
2144 int 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 =
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  {
2289  btSoftBody::Face& f=m_faces[i];
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  {
2352  btSoftBody::Pose& pose=m_pose;
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 //
2384 void 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  {
2407  btSoftBody::Face& f=m_faces[i];
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  {
2433  btSoftBody::Face& f=m_faces[i];
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;
2494  c.m_com = btSoftBody::clusterCom(&c);
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
2644  c.m_leaf=m_cdbvt.insert(bounds,&c);
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  {
2663  btAlignedFree(m_joints[i]);
2664  m_joints.remove(m_joints[i--]);
2665  }
2666  }
2667 }
2668 
2669 //
2670 void btSoftBody::prepareClusters(int iterations)
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 //
2773 void btSoftBody::LJoint::Prepare(btScalar dt,int iterations)
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 //
2816 void btSoftBody::AJoint::Prepare(btScalar dt,int iterations)
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 //
2861 void btSoftBody::CJoint::Prepare(btScalar dt,int iterations)
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  {
2972  btSoftBody::Node& n=m_nodes[i];
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 {
3137 BT_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:
3177  return(&btSoftBody::PSolve_Anchors);
3178  case ePSolver::Linear:
3179  return(&btSoftBody::PSolve_Links);
3180  case ePSolver::RContacts:
3182  case ePSolver::SContacts:
3183  return(&btSoftBody::PSolve_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  {
3212  btSoftColliders::CollideSDF_RS docollide;
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 
3316 const 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;
3333  SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)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 
3507  m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
3508  memPtr->m_bframe = m_pose.m_bframe;
3509  memPtr->m_bvolume = m_pose.m_bvolume;
3510  m_pose.m_com.serializeFloat(memPtr->m_com);
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);
3519  btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
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;
3527  m_pose.m_rot.serializeFloat(memPtr->m_rot);
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);
3558  SoftBodyClusterData* memPtr = (SoftBodyClusterData*) chunk->m_oldPtr;
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);
3600  btVector3FloatData* memPtr = (btVector3FloatData*) chunk->m_oldPtr;
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  {
3683  memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
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  {
3699  memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
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 
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1075
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
static void PSolve_RContacts(btSoftBody *psb, btScalar kst, btScalar ti)
btScalar getInvMass() const
Definition: btRigidBody.h:273
RayFromToCaster(const btVector3 &rayFrom, const btVector3 &rayTo, btScalar mxt)
static T sum(const btAlignedObjectArray< T > &items)
const btCollisionObject * m_colObj
Definition: btSoftBody.h:188
btVector3 m_velocity
Definition: btSoftBody.h:358
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don&#39;t store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:203
int generateBendingConstraints(int distance, Material *mat=0)
btMatrix3x3 m_scl
Definition: btSoftBody.h:318
void(* vsolver_t)(btSoftBody *, btScalar)
Definition: btSoftBody.h:634
static void solveClusters(const btAlignedObjectArray< btSoftBody * > &bodies)
#define SIMD_EPSILON
Definition: btScalar.h:521
#define NEXTRAND
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:906
eFeature::_ feature
soft body
Definition: btSoftBody.h:166
Rigid contacts solver.
Definition: btSoftBody.h:106
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:587
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
void push_back(const T &_Val)
void Process(const btDbvtNode *leaf)
int index
feature type
Definition: btSoftBody.h:167
Vertex normals are oriented toward velocity.
Definition: btSoftBody.h:86
SoftBodyMaterialData * m_material
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
void updateConstants()
btScalar m_maxDisplacement
Definition: btSoftBody.h:48
void Prepare(btScalar dt, int iterations)
Config m_cfg
Definition: btSoftBody.h:653
btMatrix3x3 m_c0
Definition: btSoftBody.h:271
static void ZeroInitialize(T &value)
btVector3 m_normal
Definition: btSoftBody.h:189
btAlignedObjectArray< btScalar > scratch_r
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
bool cutLink(int node0, int node1, btScalar position)
btMatrix3x3 m_locii
Definition: btSoftBody.h:330
btVector3FloatData m_relPosition[2]
void integrateMotion()
Vertex normals are flipped to match velocity.
Definition: btSoftBody.h:87
btScalar m_restLengthScale
Definition: btSoftBody.h:682
btVector3FloatData m_com
void releaseClusters()
tVector3Array m_framerefs
Definition: btSoftBody.h:326
void setSolver(eSolverPresets::_ preset)
static btVector3 clusterVelocity(const Cluster *cluster, const btVector3 &rpos)
Definition: btSoftBody.cpp:970
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:159
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:586
void * data
Definition: btDbvt.h:187
int getInternalType() const
reserved for Bullet internal usage
btMatrix3x3 m_c0
Definition: btSoftBody.h:295
const btCollisionObjectWrapper * m_colObj1Wrap
btVector3FloatData m_previousPosition
btVector3FloatData m_localFrame
tJointArray m_joints
Definition: btSoftBody.h:666
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:676
virtual void * getUniquePointer(void *oldPtr)=0
Material * appendMaterial()
Definition: btSoftBody.cpp:176
btScalar m_split
Definition: btSoftBody.h:505
virtual void Prepare(btScalar dt, int iterations)
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
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)
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
void Prepare(btScalar dt, int iterations)
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:141
#define btAssert(x)
Definition: btScalar.h:131
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:325
static void clusterVImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
Definition: btSoftBody.cpp:976
btDispatcher * m_dispatcher
Definition: btSoftBody.h:51
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void * m_tag
Definition: btSoftBody.h:656
btScalar fraction
feature index
Definition: btSoftBody.h:168
void setVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:650
btVector3 m_windVelocity
Definition: btSoftBody.h:680
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:517
btVector3FloatData m_vimpulses[2]
void Solve(btScalar dt, btScalar sor)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
tSContactArray m_scontacts
Definition: btSoftBody.h:665
SDF based rigid vs soft.
Definition: btSoftBody.h:139
btDbvt m_fdbvt
Definition: btSoftBody.h:672
btDbvtNode * m_root
Definition: btDbvt.h:262
void appendAnchor(int node, btRigidBody *body, bool disableCollisionBetweenLinkedBodies=false, btScalar influence=1)
Definition: btSoftBody.cpp:363
#define btVector3Data
Definition: btVector3.h:29
static void clusterImpulse(Cluster *cluster, const btVector3 &rpos, const Impulse &impulse)
Definition: btSoftBody.cpp:996
static bool SameSign(const T &x, const T &y)
const btTransform & xform() const
Definition: btSoftBody.h:413
static btVector3 clusterCom(const Cluster *cluster)
Definition: btSoftBody.cpp:953
btMatrix3x3 m_aqq
Definition: btSoftBody.h:319
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
btVector3FloatData m_dimpulses[2]
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
Vertex normals are taken as it is.
Definition: btSoftBody.h:89
void addForce(const btVector3 &force)
Definition: btSoftBody.cpp:443
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
btVector3FloatData m_accumulatedForce
void setWindVelocity(const btVector3 &velocity)
Set a wind velocity for interaction with the air.
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
#define btRigidBodyData
Definition: btRigidBody.h:36
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
Cluster vs cluster soft vs soft handling.
Definition: btSoftBody.h:144
btTransform m_initialWorldTransform
Definition: btSoftBody.h:678
btTransform m_worldTransform
btSoftBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.cpp:26
btDbvtNode * m_leaf
Definition: btSoftBody.h:254
#define ISLAND_SLEEPING
void appendLink(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:262
void Terminate(btScalar dt)
bool rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Ray casting using rayFrom and rayTo in worldspace, (not direction!)
void applyForces()
btCollisionShape * m_collisionShape
tNodeArray m_nodes
Definition: btSoftBody.h:659
btAlignedObjectArray< btMatrix3x3 > scratch_m
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...
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
tLinkArray m_links
Definition: btSoftBody.h:660
bool hasContactResponse() const
void appendAngularJoint(const AJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:416
Node * m_n[3]
Definition: btSoftBody.h:251
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:213
btVector3FloatData m_normal
btVector3 m_normal
Definition: btSoftBody.h:252
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
void appendFace(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:299
btSoftBodyWorldInfo * m_worldInfo
Definition: btSoftBody.h:657
tTetraArray m_tetras
Definition: btSoftBody.h:662
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:577
btVector3FloatData m_normal
virtual ~btSoftBody()
Definition: btSoftBody.cpp:119
Cluster soft body self collision.
Definition: btSoftBody.h:146
btScalar getTotalMass() const
Definition: btSoftBody.cpp:687
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:53
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
int generateClusters(int k, int maxiterations=8192)
generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle otherwise an ...
#define IDX2PTR(_p_, _b_)
void addVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:644
const char * m_text
Definition: btSoftBody.h:302
void addAeroForceToNode(const btVector3 &windVelocity, int nodeIndex)
Definition: btSoftBody.cpp:458
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:933
Enable debug draw.
Definition: btSoftBody.h:154
void appendLinearJoint(const LJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:390
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:526
void initDefaults()
Definition: btSoftBody.cpp:64
#define BT_SBNODE_CODE
Definition: btSerializer.h:130
btScalar getRestLengthScale()
Definition: btSoftBody.cpp:846
#define SIMD_PI
Definition: btScalar.h:504
btTransform & getWorldTransform()
btVector3FloatData m_refs[2]
Material * m_material
Definition: btSoftBody.h:219
btVector3 m_offset
Definition: btSoftBody.h:303
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:585
btMatrix3x3FloatData m_c0
int m_internalType
m_internalType is reserved to distinguish Bullet&#39;s btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
btMatrix3x3FloatData m_rot
void clear()
Definition: btDbvt.cpp:459
void indicesToPointers(const int *map=0)
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:497
btBroadphaseProxy * getBroadphaseHandle()
void setVolumeMass(btScalar mass)
Definition: btSoftBody.cpp:741
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
void updatePose()
btVector3 m_velocity
Definition: btSoftBody.h:196
int clusterCount() const
Definition: btSoftBody.cpp:947
void randomizeConstraints()
Vertex normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:88
void Prepare(btScalar dt, int iterations)
void updateLinkConstants()
const btVector3 & getWindVelocity()
Return the wind velocity for interaction with the air.
const btScalar & w() const
Return the w value.
Definition: btVector3.h:593
tMaterialArray m_materials
Definition: btSoftBody.h:667
static void clusterDImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
Definition: btSoftBody.cpp:986
SoftBodyMaterialData * m_material
void btSwap(T &a, T &b)
Definition: btScalar.h:621
void refine(ImplicitFn *ifn, btScalar accurary, bool cut)
static void PSolve_SContacts(btSoftBody *psb, btScalar, btScalar ti)
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:425
btMatrix3x3 m_invwi
Definition: btSoftBody.h:331
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
void staticSolve(int iterations)
RayFromToCaster takes a ray from, ray to (instead of direction!)
Definition: btSoftBody.h:609
static psolver_t getSolver(ePSolver::_ solver)
#define btSoftBodyDataName
Definition: btSoftBody.h:35
void releaseCluster(int index)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
void updateNormals()
void updateBounds()
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:575
#define IDX(_x_, _y_)
const btTransform & getWorldTransform() const
btScalar getMass(int node) const
Definition: btSoftBody.cpp:681
void appendTetra(int model, Material *mat)
Definition: btSoftBody.cpp:334
#define btAlignedFree(ptr)
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
void solveConstraints()
void predictMotion(btScalar dt)
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:573
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:262
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
btMatrix3x3FloatData m_invwi
static void clusterVAImpulse(Cluster *cluster, const btVector3 &impulse)
void appendNode(const btVector3 &x, btScalar m)
Definition: btSoftBody.cpp:242
tAnchorArray m_anchors
Definition: btSoftBody.h:663
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
btVector3FloatData m_velocity
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
static void VSolve_Links(btSoftBody *psb, btScalar kst)
btScalar m_influence
Definition: btSoftBody.h:294
virtual void setMargin(btScalar margin)=0
btVector3 m_vimpulses[2]
Definition: btSoftBody.h:333
void setTotalMass(btScalar mass, bool fromfaces=false)
Definition: btSoftBody.cpp:698
Pose m_pose
Definition: btSoftBody.h:655
void initializeClusters()
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
tPSolverArray m_dsequence
Definition: btSoftBody.h:597
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition: btDbvt.h:419
btScalar m_cfm[2]
Definition: btSoftBody.h:286
void initializeFaceTree()
void rotate(const btQuaternion &rot)
Definition: btSoftBody.cpp:817
btMatrix3x3FloatData m_scale
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
btRigidBodyData * m_rigidBody
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1324
void setTotalDensity(btScalar density)
Definition: btSoftBody.cpp:735
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
#define BT_ARRAY_CODE
Definition: btSerializer.h:128
btAlignedObjectArray< btScalar > m_jacobians
const btCollisionShape * getCollisionShape() const
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
virtual int calculateSerializeBufferSize() const
void activate(bool forceActivation=false) const
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:223
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:82
const Value * find(const Key &key) const
Definition: btHashMap.h:422
Rigid versus soft mask.
Definition: btSoftBody.h:142
btScalar m_coords[4]
Definition: btSoftBody.h:306
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btScalar m_offset
Definition: btSoftBody.h:190
btScalar btAcos(btScalar x)
Definition: btScalar.h:479
virtual btScalar Eval(const btVector3 &x)=0
DBVT_INLINE const btVector3 & Maxs() const
Definition: btDbvt.h:137
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
Definition: btMatrix3x3.h:1394
btVector3 m_com
Definition: btSoftBody.h:316
btAlignedObjectArray< btVector3 > scratch_v
#define BT_PROFILE(name)
Definition: btQuickprof.h:216
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 m_refs[2]
Definition: btSoftBody.h:502
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btMatrix3x3FloatData m_aqq
btVector3FloatData m_av
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
btVector3FloatData m_position
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Node * m_nodes[4]
Definition: btSoftBody.h:305
void Solve(btScalar dt, btScalar sor)
btDbvt m_cdbvt
Definition: btSoftBody.h:673
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:966
void Solve(btScalar dt, btScalar sor)
#define btSoftBodyData
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.h:34
tNoteArray m_notes
Definition: btSoftBody.h:658
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 remove(const T &key)
void applyClusters(bool drift)
static T Lerp(const T &a, const T &b, btScalar t)
Face normals are flipped to match velocity.
Definition: btSoftBody.h:90
void transform(const btTransform &trs)
Definition: btSoftBody.cpp:786
void resize(int newsize, const T &fillData=T())
btDbvt m_ndbvt
Definition: btSoftBody.h:671
#define BT_SBMATERIAL_CODE
Definition: btSerializer.h:129
btVector3 m_n
Definition: btSoftBody.h:228
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:550
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
btDbvtVolume volume
Definition: btDbvt.h:180
btVector3 m_local
Definition: btSoftBody.h:292
int findLinearSearch(const T &key) const
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:158
btDbvtNode * m_leaf
Definition: btSoftBody.h:339
btScalar timescale
Definition: btSoftBody.h:589
SolverState m_sst
Definition: btSoftBody.h:654
void Terminate(btScalar dt)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1030
Face normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:91
SoftBodyMaterialData * m_material
Rigid versus soft mask.
Definition: btSoftBody.h:138
#define PTR2IDX(_p_, _b_)
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
tClusterArray m_clusters
Definition: btSoftBody.h:674
virtual btScalar getMargin() const =0
btSoftBody * body
Definition: btSoftBody.h:165
btVector3 m_dimpulses[2]
Definition: btSoftBody.h:334
void(* psolver_t)(btSoftBody *, btScalar, btScalar)
Definition: btSoftBody.h:633
btMatrix3x3 m_rot
Definition: btSoftBody.h:317
static void clusterAImpulse(Cluster *cluster, const Impulse &impulse)
void scale(const btVector3 &scl)
Definition: btSoftBody.cpp:826
void cleanupClusters()
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
bool empty() const
Definition: btDbvt.h:276
btVector3 m_bounds[2]
Definition: btSoftBody.h:669
btScalar m_volume
Definition: btSoftBody.h:313
btVector3 m_v
Definition: btSoftBody.h:226
tVSolverArray m_vsequence
Definition: btSoftBody.h:595
btVector3FloatData * m_framerefs
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
#define btAlignedAlloc(size, alignment)
btScalar m_timeacc
Definition: btSoftBody.h:668
void dampClusters()
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:284
tPSolverArray m_psequence
Definition: btSoftBody.h:596
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btVector3FloatData m_com
static btVector3 NormalizeAny(const btVector3 &v)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:898
btVector3FloatData m_c0[4]
SoftBodyMaterialData * m_material
tScalarArray m_wgh
Definition: btSoftBody.h:315
IControl * m_icontrol
Definition: btSoftBody.h:547
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:50
void translate(const btVector3 &trs)
Definition: btSoftBody.cpp:808
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:738
eAeroModel::_ aeromodel
Definition: btSoftBody.h:569
btScalar air_density
Definition: btSoftBody.h:45
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:334
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
void setRestLengthScale(btScalar restLength)
Definition: btSoftBody.cpp:852
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:903
btVector3 m_q
Definition: btSoftBody.h:225
btAlignedObjectArray< const class btCollisionObject * > m_collisionDisabledObjects
Definition: btSoftBody.h:74
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
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
virtual void * findPointer(void *oldPtr)=0
btVector3 m_f
Definition: btSoftBody.h:227
void prepareClusters(int iterations)
btVector3 m_gravity
Definition: btSoftBody.h:52
static void solveCommonConstraints(btSoftBody **bodies, int count, int iterations)
bool m_bUpdateRtCst
Definition: btSoftBody.h:670
DBVT_INLINE const btVector3 & Mins() const
Definition: btDbvt.h:136
tScalarArray m_masses
Definition: btSoftBody.h:324
void pointersToIndices()
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:71
static void clusterDCImpulse(Cluster *cluster, const btVector3 &impulse)
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:23
btTransformFloatData m_framexform
const btCollisionShape * getCollisionShape() const
btRigidBody * m_body
Definition: btSoftBody.h:293
void setPose(bool bvolume, bool bframe)
Definition: btSoftBody.cpp:867
static void PSolve_Anchors(btSoftBody *psb, btScalar kst, btScalar ti)
void setMass(int node, btScalar mass)
Definition: btSoftBody.cpp:674
void setVolumeDensity(btScalar density)
Definition: btSoftBody.cpp:771
bool checkFace(int node0, int node1, int node2) const
Definition: btSoftBody.cpp:155
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: