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