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"
26 #include <iostream>
27 //
28 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
29  : m_softBodySolver(0), m_worldInfo(worldInfo)
30 {
31  /* Init */
32  initDefaults();
33 
34  /* Default material */
35  Material* pm = appendMaterial();
36  pm->m_kLST = 1;
37  pm->m_kAST = 1;
38  pm->m_kVST = 1;
40 
41  /* Nodes */
42  const btScalar margin = getCollisionShape()->getMargin();
43  m_nodes.resize(node_count);
44  for (int i = 0, ni = node_count; i < ni; ++i)
45  {
46  Node& n = m_nodes[i];
47  ZeroInitialize(n);
48  n.m_x = x ? *x++ : btVector3(0, 0, 0);
49  n.m_q = n.m_x;
50  n.m_im = m ? *m++ : 1;
51  n.m_im = n.m_im > 0 ? 1 / n.m_im : 0;
52  n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
53  n.m_material = pm;
54  }
55  updateBounds();
56 }
57 
59  : m_worldInfo(worldInfo)
60 {
61  initDefaults();
62 }
63 
65 {
68  m_cfg.kVCF = 1;
69  m_cfg.kDG = 0;
70  m_cfg.kLF = 0;
71  m_cfg.kDP = 0;
72  m_cfg.kPR = 0;
73  m_cfg.kVC = 0;
74  m_cfg.kDF = (btScalar)0.2;
75  m_cfg.kMT = 0;
76  m_cfg.kCHR = (btScalar)1.0;
77  m_cfg.kKHR = (btScalar)0.1;
78  m_cfg.kSHR = (btScalar)1.0;
79  m_cfg.kAHR = (btScalar)0.7;
80  m_cfg.kSRHR_CL = (btScalar)0.1;
81  m_cfg.kSKHR_CL = (btScalar)1;
82  m_cfg.kSSHR_CL = (btScalar)0.5;
83  m_cfg.kSR_SPLT_CL = (btScalar)0.5;
84  m_cfg.kSK_SPLT_CL = (btScalar)0.5;
85  m_cfg.kSS_SPLT_CL = (btScalar)0.5;
87  m_cfg.timescale = 1;
88  m_cfg.viterations = 0;
89  m_cfg.piterations = 1;
90  m_cfg.diterations = 0;
91  m_cfg.citerations = 4;
92  m_cfg.drag = 0;
93  m_cfg.m_maxStress = 0;
95  m_pose.m_bvolume = false;
96  m_pose.m_bframe = false;
97  m_pose.m_volume = 0;
98  m_pose.m_com = btVector3(0, 0, 0);
101  m_tag = 0;
102  m_timeacc = 0;
103  m_bUpdateRtCst = true;
104  m_bounds[0] = btVector3(0, 0, 0);
105  m_bounds[1] = btVector3(0, 0, 0);
108 
109  /* Collision shape */
112  m_collisionShape->setMargin(0.25f);
113 
115 
116  m_windVelocity = btVector3(0, 0, 0);
119  m_sleepingThreshold = 0.1;
120  m_useFaceContact = true;
121  m_useSelfCollision = false;
122  m_collisionFlags = 0;
123 }
124 
125 //
127 {
128  //for now, delete the internal shape
129  delete m_collisionShape;
130  int i;
131 
132  releaseClusters();
133  for (i = 0; i < m_materials.size(); ++i)
135  for (i = 0; i < m_joints.size(); ++i)
137 }
138 
139 //
140 bool btSoftBody::checkLink(int node0, int node1) const
141 {
142  return (checkLink(&m_nodes[node0], &m_nodes[node1]));
143 }
144 
145 //
146 bool btSoftBody::checkLink(const Node* node0, const Node* node1) const
147 {
148  const Node* n[] = {node0, node1};
149  for (int i = 0, ni = m_links.size(); i < ni; ++i)
150  {
151  const Link& l = m_links[i];
152  if ((l.m_n[0] == n[0] && l.m_n[1] == n[1]) ||
153  (l.m_n[0] == n[1] && l.m_n[1] == n[0]))
154  {
155  return (true);
156  }
157  }
158  return (false);
159 }
160 
161 //
162 bool btSoftBody::checkFace(int node0, int node1, int node2) const
163 {
164  const Node* n[] = {&m_nodes[node0],
165  &m_nodes[node1],
166  &m_nodes[node2]};
167  for (int i = 0, ni = m_faces.size(); i < ni; ++i)
168  {
169  const Face& f = m_faces[i];
170  int c = 0;
171  for (int j = 0; j < 3; ++j)
172  {
173  if ((f.m_n[j] == n[0]) ||
174  (f.m_n[j] == n[1]) ||
175  (f.m_n[j] == n[2]))
176  c |= 1 << j;
177  else
178  break;
179  }
180  if (c == 7) return (true);
181  }
182  return (false);
183 }
184 
185 //
187 {
188  Material* pm = new (btAlignedAlloc(sizeof(Material), 16)) Material();
189  if (m_materials.size() > 0)
190  *pm = *m_materials[0];
191  else
192  ZeroInitialize(*pm);
194  return (pm);
195 }
196 
197 //
198 void btSoftBody::appendNote(const char* text,
199  const btVector3& o,
200  const btVector4& c,
201  Node* n0,
202  Node* n1,
203  Node* n2,
204  Node* n3)
205 {
206  Note n;
207  ZeroInitialize(n);
208  n.m_rank = 0;
209  n.m_text = text;
210  n.m_offset = o;
211  n.m_coords[0] = c.x();
212  n.m_coords[1] = c.y();
213  n.m_coords[2] = c.z();
214  n.m_coords[3] = c.w();
215  n.m_nodes[0] = n0;
216  n.m_rank += n0 ? 1 : 0;
217  n.m_nodes[1] = n1;
218  n.m_rank += n1 ? 1 : 0;
219  n.m_nodes[2] = n2;
220  n.m_rank += n2 ? 1 : 0;
221  n.m_nodes[3] = n3;
222  n.m_rank += n3 ? 1 : 0;
223  m_notes.push_back(n);
224 }
225 
226 //
227 void btSoftBody::appendNote(const char* text,
228  const btVector3& o,
229  Node* feature)
230 {
231  appendNote(text, o, btVector4(1, 0, 0, 0), feature);
232 }
233 
234 //
235 void btSoftBody::appendNote(const char* text,
236  const btVector3& o,
237  Link* feature)
238 {
239  static const btScalar w = 1 / (btScalar)2;
240  appendNote(text, o, btVector4(w, w, 0, 0), feature->m_n[0],
241  feature->m_n[1]);
242 }
243 
244 //
245 void btSoftBody::appendNote(const char* text,
246  const btVector3& o,
247  Face* feature)
248 {
249  static const btScalar w = 1 / (btScalar)3;
250  appendNote(text, o, btVector4(w, w, w, 0), feature->m_n[0],
251  feature->m_n[1],
252  feature->m_n[2]);
253 }
254 
255 //
257 {
258  if (m_nodes.capacity() == m_nodes.size())
259  {
261  m_nodes.reserve(m_nodes.size() * 2 + 1);
263  }
264  const btScalar margin = getCollisionShape()->getMargin();
266  Node& n = m_nodes[m_nodes.size() - 1];
267  ZeroInitialize(n);
268  n.m_x = x;
269  n.m_q = n.m_x;
270  n.m_im = m > 0 ? 1 / m : 0;
271  n.m_material = m_materials[0];
272  n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x, margin), &n);
273 }
274 
275 //
276 void btSoftBody::appendLink(int model, Material* mat)
277 {
278  Link l;
279  if (model >= 0)
280  l = m_links[model];
281  else
282  {
283  ZeroInitialize(l);
284  l.m_material = mat ? mat : m_materials[0];
285  }
286  m_links.push_back(l);
287 }
288 
289 //
290 void btSoftBody::appendLink(int node0,
291  int node1,
292  Material* mat,
293  bool bcheckexist)
294 {
295  appendLink(&m_nodes[node0], &m_nodes[node1], mat, bcheckexist);
296 }
297 
298 //
300  Node* node1,
301  Material* mat,
302  bool bcheckexist)
303 {
304  if ((!bcheckexist) || (!checkLink(node0, node1)))
305  {
306  appendLink(-1, mat);
307  Link& l = m_links[m_links.size() - 1];
308  l.m_n[0] = node0;
309  l.m_n[1] = node1;
310  l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
311  m_bUpdateRtCst = true;
312  }
313 }
314 
315 //
316 void btSoftBody::appendFace(int model, Material* mat)
317 {
318  Face f;
319  if (model >= 0)
320  {
321  f = m_faces[model];
322  }
323  else
324  {
325  ZeroInitialize(f);
326  f.m_material = mat ? mat : m_materials[0];
327  }
328  m_faces.push_back(f);
329 }
330 
331 //
332 void btSoftBody::appendFace(int node0, int node1, int node2, Material* mat)
333 {
334  if (node0 == node1)
335  return;
336  if (node1 == node2)
337  return;
338  if (node2 == node0)
339  return;
340 
341  appendFace(-1, mat);
342  Face& f = m_faces[m_faces.size() - 1];
343  btAssert(node0 != node1);
344  btAssert(node1 != node2);
345  btAssert(node2 != node0);
346  f.m_n[0] = &m_nodes[node0];
347  f.m_n[1] = &m_nodes[node1];
348  f.m_n[2] = &m_nodes[node2];
349  f.m_ra = AreaOf(f.m_n[0]->m_x,
350  f.m_n[1]->m_x,
351  f.m_n[2]->m_x);
352  m_bUpdateRtCst = true;
353 }
354 
355 //
356 void btSoftBody::appendTetra(int model, Material* mat)
357 {
358  Tetra t;
359  if (model >= 0)
360  t = m_tetras[model];
361  else
362  {
363  ZeroInitialize(t);
364  t.m_material = mat ? mat : m_materials[0];
365  }
366  m_tetras.push_back(t);
367 }
368 
369 //
370 void btSoftBody::appendTetra(int node0,
371  int node1,
372  int node2,
373  int node3,
374  Material* mat)
375 {
376  appendTetra(-1, mat);
377  Tetra& t = m_tetras[m_tetras.size() - 1];
378  t.m_n[0] = &m_nodes[node0];
379  t.m_n[1] = &m_nodes[node1];
380  t.m_n[2] = &m_nodes[node2];
381  t.m_n[3] = &m_nodes[node3];
382  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);
383  m_bUpdateRtCst = true;
384 }
385 
386 //
387 
388 void btSoftBody::appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies, btScalar influence)
389 {
390  btVector3 local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
391  appendAnchor(node, body, local, disableCollisionBetweenLinkedBodies, influence);
392 }
393 
394 //
395 void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies, btScalar influence)
396 {
397  if (disableCollisionBetweenLinkedBodies)
398  {
400  {
402  }
403  }
404 
405  Anchor a;
406  a.m_node = &m_nodes[node];
407  a.m_body = body;
408  a.m_local = localPivot;
409  a.m_node->m_battach = 1;
410  a.m_influence = influence;
411  m_anchors.push_back(a);
412 }
413 
414 //
416 {
418  btSoftBody::Node& n = m_nodes[node];
419  const btScalar ima = n.m_im;
420  const btScalar imb = body->getInvMass();
421  btVector3 nrm;
422  const btCollisionShape* shp = body->getCollisionShape();
423  const btTransform& wtr = body->getWorldTransform();
424  btScalar dst =
426  wtr.invXform(m_nodes[node].m_x),
427  shp,
428  nrm,
429  0);
430 
431  c.m_cti.m_colObj = body;
432  c.m_cti.m_normal = wtr.getBasis() * nrm;
433  c.m_cti.m_offset = dst;
434  c.m_node = &m_nodes[node];
435  const btScalar fc = m_cfg.kDF * body->getFriction();
436  c.m_c2 = ima;
437  c.m_c3 = fc;
439  static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
440  const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
441  const btVector3 ra = n.m_x - wtr.getOrigin();
442 
443  c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
444  c.m_c1 = ra;
445  c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
446  c.m_node->m_battach = 1;
447  m_deformableAnchors.push_back(c);
448 }
449 
450 //
452 {
454  btSoftBody::Node& n = m_nodes[node];
455  const btScalar ima = n.m_im;
456  btVector3 nrm;
457  const btCollisionShape* shp = link->getCollisionShape();
458  const btTransform& wtr = link->getWorldTransform();
459  btScalar dst =
461  wtr.invXform(m_nodes[node].m_x),
462  shp,
463  nrm,
464  0);
465  c.m_cti.m_colObj = link;
466  c.m_cti.m_normal = wtr.getBasis() * nrm;
467  c.m_cti.m_offset = dst;
468  c.m_node = &m_nodes[node];
469  const btScalar fc = m_cfg.kDF * link->getFriction();
470  c.m_c2 = ima;
471  c.m_c3 = fc;
473  btVector3 normal = c.m_cti.m_normal;
475  btVector3 t2 = btCross(normal, t1);
476  btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
477  findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
478  findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
479  findJacobian(link, jacobianData_t2, c.m_node->m_x, t2);
480 
481  btScalar* J_n = &jacobianData_normal.m_jacobians[0];
482  btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
483  btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
484 
485  btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
486  btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
487  btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
488 
489  btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
490  t1.getX(), t1.getY(), t1.getZ(),
491  t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
492  const int ndof = link->m_multiBody->getNumDofs() + 6;
493  btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
494  c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
495  c.jacobianData_normal = jacobianData_normal;
496  c.jacobianData_t1 = jacobianData_t1;
497  c.jacobianData_t2 = jacobianData_t2;
498  c.t1 = t1;
499  c.t2 = t2;
500  const btVector3 ra = n.m_x - wtr.getOrigin();
501  c.m_c1 = ra;
502  c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
503  c.m_node->m_battach = 1;
504  m_deformableAnchors.push_back(c);
505 }
506 //
507 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
508 {
509  LJoint* pj = new (btAlignedAlloc(sizeof(LJoint), 16)) LJoint();
510  pj->m_bodies[0] = body0;
511  pj->m_bodies[1] = body1;
512  pj->m_refs[0] = pj->m_bodies[0].xform().inverse() * specs.position;
513  pj->m_refs[1] = pj->m_bodies[1].xform().inverse() * specs.position;
514  pj->m_cfm = specs.cfm;
515  pj->m_erp = specs.erp;
516  pj->m_split = specs.split;
517  m_joints.push_back(pj);
518 }
519 
520 //
522 {
523  appendLinearJoint(specs, m_clusters[0], body);
524 }
525 
526 //
528 {
529  appendLinearJoint(specs, m_clusters[0], body->m_clusters[0]);
530 }
531 
532 //
533 void btSoftBody::appendAngularJoint(const AJoint::Specs& specs, Cluster* body0, Body body1)
534 {
535  AJoint* pj = new (btAlignedAlloc(sizeof(AJoint), 16)) AJoint();
536  pj->m_bodies[0] = body0;
537  pj->m_bodies[1] = body1;
538  pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis() * specs.axis;
539  pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis() * specs.axis;
540  pj->m_cfm = specs.cfm;
541  pj->m_erp = specs.erp;
542  pj->m_split = specs.split;
543  pj->m_icontrol = specs.icontrol;
544  m_joints.push_back(pj);
545 }
546 
547 //
549 {
550  appendAngularJoint(specs, m_clusters[0], body);
551 }
552 
553 //
555 {
556  appendAngularJoint(specs, m_clusters[0], body->m_clusters[0]);
557 }
558 
559 //
560 void btSoftBody::addForce(const btVector3& force)
561 {
562  for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addForce(force, i);
563 }
564 
565 //
566 void btSoftBody::addForce(const btVector3& force, int node)
567 {
568  Node& n = m_nodes[node];
569  if (n.m_im > 0)
570  {
571  n.m_f += force;
572  }
573 }
574 
575 void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex)
576 {
577  btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size());
578 
579  const btScalar dt = m_sst.sdt;
580  const btScalar kLF = m_cfg.kLF;
581  const btScalar kDG = m_cfg.kDG;
582  //const btScalar kPR = m_cfg.kPR;
583  //const btScalar kVC = m_cfg.kVC;
584  const bool as_lift = kLF > 0;
585  const bool as_drag = kDG > 0;
586  const bool as_aero = as_lift || as_drag;
587  const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
588 
589  Node& n = m_nodes[nodeIndex];
590 
591  if (n.m_im > 0)
592  {
593  btSoftBody::sMedium medium;
594 
595  EvaluateMedium(m_worldInfo, n.m_x, medium);
596  medium.m_velocity = windVelocity;
598 
599  /* Aerodynamics */
600  if (as_vaero)
601  {
602  const btVector3 rel_v = n.m_v - medium.m_velocity;
603  const btScalar rel_v_len = rel_v.length();
604  const btScalar rel_v2 = rel_v.length2();
605 
606  if (rel_v2 > SIMD_EPSILON)
607  {
608  const btVector3 rel_v_nrm = rel_v.normalized();
609  btVector3 nrm = n.m_n;
610 
612  {
613  nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
614  btVector3 fDrag(0, 0, 0);
615  btVector3 fLift(0, 0, 0);
616 
617  btScalar n_dot_v = nrm.dot(rel_v_nrm);
618  btScalar tri_area = 0.5f * n.m_area;
619 
620  fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
621 
622  // Check angle of attack
623  // cos(10º) = 0.98480
624  if (0 < n_dot_v && n_dot_v < 0.98480f)
625  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));
626 
627  // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
628  btVector3 del_v_by_fDrag = fDrag * n.m_im * m_sst.sdt;
629  btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
630  btScalar v_len2 = n.m_v.length2();
631 
632  if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
633  {
634  btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
635  btScalar v_len = n.m_v.length();
636  fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
637  }
638 
639  n.m_f += fDrag;
640  n.m_f += fLift;
641  }
643  {
645  nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
646 
647  const btScalar dvn = btDot(rel_v, nrm);
648  /* Compute forces */
649  if (dvn > 0)
650  {
651  btVector3 force(0, 0, 0);
652  const btScalar c0 = n.m_area * dvn * rel_v2 / 2;
653  const btScalar c1 = c0 * medium.m_density;
654  force += nrm * (-c1 * kLF);
655  force += rel_v.normalized() * (-c1 * kDG);
656  ApplyClampedForce(n, force, dt);
657  }
658  }
659  }
660  }
661  }
662 }
663 
664 void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex)
665 {
666  const btScalar dt = m_sst.sdt;
667  const btScalar kLF = m_cfg.kLF;
668  const btScalar kDG = m_cfg.kDG;
669  // const btScalar kPR = m_cfg.kPR;
670  // const btScalar kVC = m_cfg.kVC;
671  const bool as_lift = kLF > 0;
672  const bool as_drag = kDG > 0;
673  const bool as_aero = as_lift || as_drag;
674  const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
675 
676  if (as_faero)
677  {
678  btSoftBody::Face& f = m_faces[faceIndex];
679 
680  btSoftBody::sMedium medium;
681 
682  const btVector3 v = (f.m_n[0]->m_v + f.m_n[1]->m_v + f.m_n[2]->m_v) / 3;
683  const btVector3 x = (f.m_n[0]->m_x + f.m_n[1]->m_x + f.m_n[2]->m_x) / 3;
684  EvaluateMedium(m_worldInfo, x, medium);
685  medium.m_velocity = windVelocity;
687  const btVector3 rel_v = v - medium.m_velocity;
688  const btScalar rel_v_len = rel_v.length();
689  const btScalar rel_v2 = rel_v.length2();
690 
691  if (rel_v2 > SIMD_EPSILON)
692  {
693  const btVector3 rel_v_nrm = rel_v.normalized();
694  btVector3 nrm = f.m_normal;
695 
697  {
698  nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
699 
700  btVector3 fDrag(0, 0, 0);
701  btVector3 fLift(0, 0, 0);
702 
703  btScalar n_dot_v = nrm.dot(rel_v_nrm);
704  btScalar tri_area = 0.5f * f.m_ra;
705 
706  fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm);
707 
708  // Check angle of attack
709  // cos(10º) = 0.98480
710  if (0 < n_dot_v && n_dot_v < 0.98480f)
711  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));
712 
713  fDrag /= 3;
714  fLift /= 3;
715 
716  for (int j = 0; j < 3; ++j)
717  {
718  if (f.m_n[j]->m_im > 0)
719  {
720  // Check if the velocity change resulted by aero drag force exceeds the current velocity of the node.
721  btVector3 del_v_by_fDrag = fDrag * f.m_n[j]->m_im * m_sst.sdt;
722  btScalar del_v_by_fDrag_len2 = del_v_by_fDrag.length2();
723  btScalar v_len2 = f.m_n[j]->m_v.length2();
724 
725  if (del_v_by_fDrag_len2 >= v_len2 && del_v_by_fDrag_len2 > 0)
726  {
727  btScalar del_v_by_fDrag_len = del_v_by_fDrag.length();
728  btScalar v_len = f.m_n[j]->m_v.length();
729  fDrag *= btScalar(0.8) * (v_len / del_v_by_fDrag_len);
730  }
731 
732  f.m_n[j]->m_f += fDrag;
733  f.m_n[j]->m_f += fLift;
734  }
735  }
736  }
738  {
740  nrm *= (btScalar)((btDot(nrm, rel_v) < 0) ? -1 : +1);
741 
742  const btScalar dvn = btDot(rel_v, nrm);
743  /* Compute forces */
744  if (dvn > 0)
745  {
746  btVector3 force(0, 0, 0);
747  const btScalar c0 = f.m_ra * dvn * rel_v2;
748  const btScalar c1 = c0 * medium.m_density;
749  force += nrm * (-c1 * kLF);
750  force += rel_v.normalized() * (-c1 * kDG);
751  force /= 3;
752  for (int j = 0; j < 3; ++j) ApplyClampedForce(*f.m_n[j], force, dt);
753  }
754  }
755  }
756  }
757 }
758 
759 //
760 void btSoftBody::addVelocity(const btVector3& velocity)
761 {
762  for (int i = 0, ni = m_nodes.size(); i < ni; ++i) addVelocity(velocity, i);
763 }
764 
765 /* Set velocity for the entire body */
766 void btSoftBody::setVelocity(const btVector3& velocity)
767 {
768  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
769  {
770  Node& n = m_nodes[i];
771  if (n.m_im > 0)
772  {
773  n.m_v = velocity;
774  }
775  }
776 }
777 
778 //
779 void btSoftBody::addVelocity(const btVector3& velocity, int node)
780 {
781  Node& n = m_nodes[node];
782  if (n.m_im > 0)
783  {
784  n.m_v += velocity;
785  }
786 }
787 
788 //
789 void btSoftBody::setMass(int node, btScalar mass)
790 {
791  m_nodes[node].m_im = mass > 0 ? 1 / mass : 0;
792  m_bUpdateRtCst = true;
793 }
794 
795 //
797 {
798  return (m_nodes[node].m_im > 0 ? 1 / m_nodes[node].m_im : 0);
799 }
800 
801 //
803 {
804  btScalar mass = 0;
805  for (int i = 0; i < m_nodes.size(); ++i)
806  {
807  mass += getMass(i);
808  }
809  return (mass);
810 }
811 
812 //
813 void btSoftBody::setTotalMass(btScalar mass, bool fromfaces)
814 {
815  int i;
816 
817  if (fromfaces)
818  {
819  for (i = 0; i < m_nodes.size(); ++i)
820  {
821  m_nodes[i].m_im = 0;
822  }
823  for (i = 0; i < m_faces.size(); ++i)
824  {
825  const Face& f = m_faces[i];
826  const btScalar twicearea = AreaOf(f.m_n[0]->m_x,
827  f.m_n[1]->m_x,
828  f.m_n[2]->m_x);
829  for (int j = 0; j < 3; ++j)
830  {
831  f.m_n[j]->m_im += twicearea;
832  }
833  }
834  for (i = 0; i < m_nodes.size(); ++i)
835  {
836  m_nodes[i].m_im = 1 / m_nodes[i].m_im;
837  }
838  }
839  const btScalar tm = getTotalMass();
840  const btScalar itm = 1 / tm;
841  for (i = 0; i < m_nodes.size(); ++i)
842  {
843  m_nodes[i].m_im /= itm * mass;
844  }
845  m_bUpdateRtCst = true;
846 }
847 
848 //
850 {
851  setTotalMass(getVolume() * density, true);
852 }
853 
854 //
856 {
858  ranks.resize(m_nodes.size(), 0);
859  int i;
860 
861  for (i = 0; i < m_nodes.size(); ++i)
862  {
863  m_nodes[i].m_im = 0;
864  }
865  for (i = 0; i < m_tetras.size(); ++i)
866  {
867  const Tetra& t = m_tetras[i];
868  for (int j = 0; j < 4; ++j)
869  {
870  t.m_n[j]->m_im += btFabs(t.m_rv);
871  ranks[int(t.m_n[j] - &m_nodes[0])] += 1;
872  }
873  }
874  for (i = 0; i < m_nodes.size(); ++i)
875  {
876  if (m_nodes[i].m_im > 0)
877  {
878  m_nodes[i].m_im = ranks[i] / m_nodes[i].m_im;
879  }
880  }
881  setTotalMass(mass, false);
882 }
883 
884 //
886 {
887  btScalar volume = 0;
888  for (int i = 0; i < m_tetras.size(); ++i)
889  {
890  const Tetra& t = m_tetras[i];
891  for (int j = 0; j < 4; ++j)
892  {
893  volume += btFabs(t.m_rv);
894  }
895  }
896  setVolumeMass(volume * density / 6);
897 }
898 
899 //
901 {
902  const btScalar margin = getCollisionShape()->getMargin();
904  vol;
905 
906  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
907  {
908  Node& n = m_nodes[i];
909  n.m_x = trs * n.m_x;
910  n.m_q = trs * n.m_q;
911  n.m_n = trs.getBasis() * n.m_n;
912  vol = btDbvtVolume::FromCR(n.m_x, margin);
913 
914  m_ndbvt.update(n.m_leaf, vol);
915  }
916  updateNormals();
917  updateBounds();
918  updateConstants();
920 }
921 
922 //
924 {
925  btTransform t;
926  t.setIdentity();
927  t.setOrigin(trs);
928  transform(t);
929 }
930 
931 //
933 {
934  btTransform t;
935  t.setIdentity();
936  t.setRotation(rot);
937  transform(t);
938 }
939 
940 //
941 void btSoftBody::scale(const btVector3& scl)
942 {
943  const btScalar margin = getCollisionShape()->getMargin();
945  vol;
946 
947  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
948  {
949  Node& n = m_nodes[i];
950  n.m_x *= scl;
951  n.m_q *= scl;
952  vol = btDbvtVolume::FromCR(n.m_x, margin);
953  m_ndbvt.update(n.m_leaf, vol);
954  }
955  updateNormals();
956  updateBounds();
957  updateConstants();
959 }
960 
961 //
963 {
964  return m_restLengthScale;
965 }
966 
967 //
969 {
970  for (int i = 0, ni = m_links.size(); i < ni; ++i)
971  {
972  Link& l = m_links[i];
973  l.m_rl = l.m_rl / m_restLengthScale * restLengthScale;
974  l.m_c1 = l.m_rl * l.m_rl;
975  }
976  m_restLengthScale = restLengthScale;
977 
979  activate();
980 }
981 
982 //
983 void btSoftBody::setPose(bool bvolume, bool bframe)
984 {
985  m_pose.m_bvolume = bvolume;
986  m_pose.m_bframe = bframe;
987  int i, ni;
988 
989  /* Weights */
990  const btScalar omass = getTotalMass();
991  const btScalar kmass = omass * m_nodes.size() * 1000;
992  btScalar tmass = omass;
994  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
995  {
996  if (m_nodes[i].m_im <= 0) tmass += kmass;
997  }
998  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
999  {
1000  Node& n = m_nodes[i];
1001  m_pose.m_wgh[i] = n.m_im > 0 ? 1 / (m_nodes[i].m_im * tmass) : kmass / tmass;
1002  }
1003  /* Pos */
1004  const btVector3 com = evaluateCom();
1006  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1007  {
1008  m_pose.m_pos[i] = m_nodes[i].m_x - com;
1009  }
1010  m_pose.m_volume = bvolume ? getVolume() : 0;
1011  m_pose.m_com = com;
1014  /* Aqq */
1015  m_pose.m_aqq[0] =
1016  m_pose.m_aqq[1] =
1017  m_pose.m_aqq[2] = btVector3(0, 0, 0);
1018  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1019  {
1020  const btVector3& q = m_pose.m_pos[i];
1021  const btVector3 mq = m_pose.m_wgh[i] * q;
1022  m_pose.m_aqq[0] += mq.x() * q;
1023  m_pose.m_aqq[1] += mq.y() * q;
1024  m_pose.m_aqq[2] += mq.z() * q;
1025  }
1027 
1028  updateConstants();
1029 }
1030 
1032 {
1033  for (int i = 0, ni = m_links.size(); i < ni; ++i)
1034  {
1035  Link& l = m_links[i];
1036  l.m_rl = (l.m_n[0]->m_x - l.m_n[1]->m_x).length();
1037  l.m_c1 = l.m_rl * l.m_rl;
1038  }
1039 }
1040 
1041 //
1043 {
1044  btScalar vol = 0;
1045  if (m_nodes.size() > 0)
1046  {
1047  int i, ni;
1048 
1049  const btVector3 org = m_nodes[0].m_x;
1050  for (i = 0, ni = m_faces.size(); i < ni; ++i)
1051  {
1052  const Face& f = m_faces[i];
1053  vol += btDot(f.m_n[0]->m_x - org, btCross(f.m_n[1]->m_x - org, f.m_n[2]->m_x - org));
1054  }
1055  vol /= (btScalar)6;
1056  }
1057  return (vol);
1058 }
1059 
1060 //
1062 {
1063  return (m_clusters.size());
1064 }
1065 
1066 //
1068 {
1069  btVector3 com(0, 0, 0);
1070  for (int i = 0, ni = cluster->m_nodes.size(); i < ni; ++i)
1071  {
1072  com += cluster->m_nodes[i]->m_x * cluster->m_masses[i];
1073  }
1074  return (com * cluster->m_imass);
1075 }
1076 
1077 //
1079 {
1080  return (clusterCom(m_clusters[cluster]));
1081 }
1082 
1083 //
1085 {
1086  return (cluster->m_lv + btCross(cluster->m_av, rpos));
1087 }
1088 
1089 //
1090 void btSoftBody::clusterVImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1091 {
1092  const btVector3 li = cluster->m_imass * impulse;
1093  const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1094  cluster->m_vimpulses[0] += li;
1095  cluster->m_lv += li;
1096  cluster->m_vimpulses[1] += ai;
1097  cluster->m_av += ai;
1098  cluster->m_nvimpulses++;
1099 }
1100 
1101 //
1102 void btSoftBody::clusterDImpulse(Cluster* cluster, const btVector3& rpos, const btVector3& impulse)
1103 {
1104  const btVector3 li = cluster->m_imass * impulse;
1105  const btVector3 ai = cluster->m_invwi * btCross(rpos, impulse);
1106  cluster->m_dimpulses[0] += li;
1107  cluster->m_dimpulses[1] += ai;
1108  cluster->m_ndimpulses++;
1109 }
1110 
1111 //
1112 void btSoftBody::clusterImpulse(Cluster* cluster, const btVector3& rpos, const Impulse& impulse)
1113 {
1114  if (impulse.m_asVelocity) clusterVImpulse(cluster, rpos, impulse.m_velocity);
1115  if (impulse.m_asDrift) clusterDImpulse(cluster, rpos, impulse.m_drift);
1116 }
1117 
1118 //
1119 void btSoftBody::clusterVAImpulse(Cluster* cluster, const btVector3& impulse)
1120 {
1121  const btVector3 ai = cluster->m_invwi * impulse;
1122  cluster->m_vimpulses[1] += ai;
1123  cluster->m_av += ai;
1124  cluster->m_nvimpulses++;
1125 }
1126 
1127 //
1128 void btSoftBody::clusterDAImpulse(Cluster* cluster, const btVector3& impulse)
1129 {
1130  const btVector3 ai = cluster->m_invwi * impulse;
1131  cluster->m_dimpulses[1] += ai;
1132  cluster->m_ndimpulses++;
1133 }
1134 
1135 //
1136 void btSoftBody::clusterAImpulse(Cluster* cluster, const Impulse& impulse)
1137 {
1138  if (impulse.m_asVelocity) clusterVAImpulse(cluster, impulse.m_velocity);
1139  if (impulse.m_asDrift) clusterDAImpulse(cluster, impulse.m_drift);
1140 }
1141 
1142 //
1143 void btSoftBody::clusterDCImpulse(Cluster* cluster, const btVector3& impulse)
1144 {
1145  cluster->m_dimpulses[0] += impulse * cluster->m_imass;
1146  cluster->m_ndimpulses++;
1147 }
1148 
1150 {
1152 };
1153 
1154 //
1156 {
1157  int i, j;
1158 
1159  if (distance > 1)
1160  {
1161  /* Build graph */
1162  const int n = m_nodes.size();
1163  const unsigned inf = (~(unsigned)0) >> 1;
1164  unsigned* adj = new unsigned[n * n];
1165 
1166 #define IDX(_x_, _y_) ((_y_)*n + (_x_))
1167  for (j = 0; j < n; ++j)
1168  {
1169  for (i = 0; i < n; ++i)
1170  {
1171  if (i != j)
1172  {
1173  adj[IDX(i, j)] = adj[IDX(j, i)] = inf;
1174  }
1175  else
1176  {
1177  adj[IDX(i, j)] = adj[IDX(j, i)] = 0;
1178  }
1179  }
1180  }
1181  for (i = 0; i < m_links.size(); ++i)
1182  {
1183  const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1184  const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1185  adj[IDX(ia, ib)] = 1;
1186  adj[IDX(ib, ia)] = 1;
1187  }
1188 
1189  //special optimized case for distance == 2
1190  if (distance == 2)
1191  {
1193 
1194  /* Build node links */
1195  nodeLinks.resize(m_nodes.size());
1196 
1197  for (i = 0; i < m_links.size(); ++i)
1198  {
1199  const int ia = (int)(m_links[i].m_n[0] - &m_nodes[0]);
1200  const int ib = (int)(m_links[i].m_n[1] - &m_nodes[0]);
1201  if (nodeLinks[ia].m_links.findLinearSearch(ib) == nodeLinks[ia].m_links.size())
1202  nodeLinks[ia].m_links.push_back(ib);
1203 
1204  if (nodeLinks[ib].m_links.findLinearSearch(ia) == nodeLinks[ib].m_links.size())
1205  nodeLinks[ib].m_links.push_back(ia);
1206  }
1207  for (int ii = 0; ii < nodeLinks.size(); ii++)
1208  {
1209  int i = ii;
1210 
1211  for (int jj = 0; jj < nodeLinks[ii].m_links.size(); jj++)
1212  {
1213  int k = nodeLinks[ii].m_links[jj];
1214  for (int kk = 0; kk < nodeLinks[k].m_links.size(); kk++)
1215  {
1216  int j = nodeLinks[k].m_links[kk];
1217  if (i != j)
1218  {
1219  const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1220  btAssert(sum == 2);
1221  if (adj[IDX(i, j)] > sum)
1222  {
1223  adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1224  }
1225  }
1226  }
1227  }
1228  }
1229  }
1230  else
1231  {
1233  for (int k = 0; k < n; ++k)
1234  {
1235  for (j = 0; j < n; ++j)
1236  {
1237  for (i = j + 1; i < n; ++i)
1238  {
1239  const unsigned sum = adj[IDX(i, k)] + adj[IDX(k, j)];
1240  if (adj[IDX(i, j)] > sum)
1241  {
1242  adj[IDX(i, j)] = adj[IDX(j, i)] = sum;
1243  }
1244  }
1245  }
1246  }
1247  }
1248 
1249  /* Build links */
1250  int nlinks = 0;
1251  for (j = 0; j < n; ++j)
1252  {
1253  for (i = j + 1; i < n; ++i)
1254  {
1255  if (adj[IDX(i, j)] == (unsigned)distance)
1256  {
1257  appendLink(i, j, mat);
1258  m_links[m_links.size() - 1].m_bbending = 1;
1259  ++nlinks;
1260  }
1261  }
1262  }
1263  delete[] adj;
1264  return (nlinks);
1265  }
1266  return (0);
1267 }
1268 
1269 //
1271 {
1272  unsigned long seed = 243703;
1273 #define NEXTRAND (seed = (1664525L * seed + 1013904223L) & 0xffffffff)
1274  int i, ni;
1275 
1276  for (i = 0, ni = m_links.size(); i < ni; ++i)
1277  {
1278  btSwap(m_links[i], m_links[NEXTRAND % ni]);
1279  }
1280  for (i = 0, ni = m_faces.size(); i < ni; ++i)
1281  {
1282  btSwap(m_faces[i], m_faces[NEXTRAND % ni]);
1283  }
1284 #undef NEXTRAND
1285 }
1286 
1287 //
1289 {
1290  Cluster* c = m_clusters[index];
1291  if (c->m_leaf) m_cdbvt.remove(c->m_leaf);
1292  c->~Cluster();
1293  btAlignedFree(c);
1294  m_clusters.remove(c);
1295 }
1296 
1297 //
1299 {
1300  while (m_clusters.size() > 0) releaseCluster(0);
1301 }
1302 
1303 //
1304 int btSoftBody::generateClusters(int k, int maxiterations)
1305 {
1306  int i;
1307  releaseClusters();
1309  for (i = 0; i < m_clusters.size(); ++i)
1310  {
1311  m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1312  m_clusters[i]->m_collide = true;
1313  }
1314  k = m_clusters.size();
1315  if (k > 0)
1316  {
1317  /* Initialize */
1319  btVector3 cog(0, 0, 0);
1320  int i;
1321  for (i = 0; i < m_nodes.size(); ++i)
1322  {
1323  cog += m_nodes[i].m_x;
1324  m_clusters[(i * 29873) % m_clusters.size()]->m_nodes.push_back(&m_nodes[i]);
1325  }
1326  cog /= (btScalar)m_nodes.size();
1327  centers.resize(k, cog);
1328  /* Iterate */
1329  const btScalar slope = 16;
1330  bool changed;
1331  int iterations = 0;
1332  do
1333  {
1334  const btScalar w = 2 - btMin<btScalar>(1, iterations / slope);
1335  changed = false;
1336  iterations++;
1337  int i;
1338 
1339  for (i = 0; i < k; ++i)
1340  {
1341  btVector3 c(0, 0, 0);
1342  for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1343  {
1344  c += m_clusters[i]->m_nodes[j]->m_x;
1345  }
1346  if (m_clusters[i]->m_nodes.size())
1347  {
1348  c /= (btScalar)m_clusters[i]->m_nodes.size();
1349  c = centers[i] + (c - centers[i]) * w;
1350  changed |= ((c - centers[i]).length2() > SIMD_EPSILON);
1351  centers[i] = c;
1352  m_clusters[i]->m_nodes.resize(0);
1353  }
1354  }
1355  for (i = 0; i < m_nodes.size(); ++i)
1356  {
1357  const btVector3 nx = m_nodes[i].m_x;
1358  int kbest = 0;
1359  btScalar kdist = ClusterMetric(centers[0], nx);
1360  for (int j = 1; j < k; ++j)
1361  {
1362  const btScalar d = ClusterMetric(centers[j], nx);
1363  if (d < kdist)
1364  {
1365  kbest = j;
1366  kdist = d;
1367  }
1368  }
1369  m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]);
1370  }
1371  } while (changed && (iterations < maxiterations));
1372  /* Merge */
1374  cids.resize(m_nodes.size(), -1);
1375  for (i = 0; i < m_clusters.size(); ++i)
1376  {
1377  for (int j = 0; j < m_clusters[i]->m_nodes.size(); ++j)
1378  {
1379  cids[int(m_clusters[i]->m_nodes[j] - &m_nodes[0])] = i;
1380  }
1381  }
1382  for (i = 0; i < m_faces.size(); ++i)
1383  {
1384  const int idx[] = {int(m_faces[i].m_n[0] - &m_nodes[0]),
1385  int(m_faces[i].m_n[1] - &m_nodes[0]),
1386  int(m_faces[i].m_n[2] - &m_nodes[0])};
1387  for (int j = 0; j < 3; ++j)
1388  {
1389  const int cid = cids[idx[j]];
1390  for (int q = 1; q < 3; ++q)
1391  {
1392  const int kid = idx[(j + q) % 3];
1393  if (cids[kid] != cid)
1394  {
1395  if (m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid]) == m_clusters[cid]->m_nodes.size())
1396  {
1397  m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]);
1398  }
1399  }
1400  }
1401  }
1402  }
1403  /* Master */
1404  if (m_clusters.size() > 1)
1405  {
1406  Cluster* pmaster = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1407  pmaster->m_collide = false;
1408  pmaster->m_nodes.reserve(m_nodes.size());
1409  for (int i = 0; i < m_nodes.size(); ++i) pmaster->m_nodes.push_back(&m_nodes[i]);
1410  m_clusters.push_back(pmaster);
1412  }
1413  /* Terminate */
1414  for (i = 0; i < m_clusters.size(); ++i)
1415  {
1416  if (m_clusters[i]->m_nodes.size() == 0)
1417  {
1418  releaseCluster(i--);
1419  }
1420  }
1421  }
1422  else
1423  {
1424  //create a cluster for each tetrahedron (if tetrahedra exist) or each face
1425  if (m_tetras.size())
1426  {
1428  for (i = 0; i < m_clusters.size(); ++i)
1429  {
1430  m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1431  m_clusters[i]->m_collide = true;
1432  }
1433  for (i = 0; i < m_tetras.size(); i++)
1434  {
1435  for (int j = 0; j < 4; j++)
1436  {
1437  m_clusters[i]->m_nodes.push_back(m_tetras[i].m_n[j]);
1438  }
1439  }
1440  }
1441  else
1442  {
1444  for (i = 0; i < m_clusters.size(); ++i)
1445  {
1446  m_clusters[i] = new (btAlignedAlloc(sizeof(Cluster), 16)) Cluster();
1447  m_clusters[i]->m_collide = true;
1448  }
1449 
1450  for (i = 0; i < m_faces.size(); ++i)
1451  {
1452  for (int j = 0; j < 3; ++j)
1453  {
1454  m_clusters[i]->m_nodes.push_back(m_faces[i].m_n[j]);
1455  }
1456  }
1457  }
1458  }
1459 
1460  if (m_clusters.size())
1461  {
1463  updateClusters();
1464 
1465  //for self-collision
1467  {
1468  for (int c0 = 0; c0 < m_clusters.size(); c0++)
1469  {
1470  m_clusters[c0]->m_clusterIndex = c0;
1471  for (int c1 = 0; c1 < m_clusters.size(); c1++)
1472  {
1473  bool connected = false;
1474  Cluster* cla = m_clusters[c0];
1475  Cluster* clb = m_clusters[c1];
1476  for (int i = 0; !connected && i < cla->m_nodes.size(); i++)
1477  {
1478  for (int j = 0; j < clb->m_nodes.size(); j++)
1479  {
1480  if (cla->m_nodes[i] == clb->m_nodes[j])
1481  {
1482  connected = true;
1483  break;
1484  }
1485  }
1486  }
1487  m_clusterConnectivity[c0 + c1 * m_clusters.size()] = connected;
1488  }
1489  }
1490  }
1491  }
1492 
1493  return (m_clusters.size());
1494 }
1495 
1496 //
1497 void btSoftBody::refine(ImplicitFn* ifn, btScalar accurary, bool cut)
1498 {
1499  const Node* nbase = &m_nodes[0];
1500  int ncount = m_nodes.size();
1501  btSymMatrix<int> edges(ncount, -2);
1502  int newnodes = 0;
1503  int i, j, k, ni;
1504 
1505  /* Filter out */
1506  for (i = 0; i < m_links.size(); ++i)
1507  {
1508  Link& l = m_links[i];
1509  if (l.m_bbending)
1510  {
1511  if (!SameSign(ifn->Eval(l.m_n[0]->m_x), ifn->Eval(l.m_n[1]->m_x)))
1512  {
1513  btSwap(m_links[i], m_links[m_links.size() - 1]);
1514  m_links.pop_back();
1515  --i;
1516  }
1517  }
1518  }
1519  /* Fill edges */
1520  for (i = 0; i < m_links.size(); ++i)
1521  {
1522  Link& l = m_links[i];
1523  edges(int(l.m_n[0] - nbase), int(l.m_n[1] - nbase)) = -1;
1524  }
1525  for (i = 0; i < m_faces.size(); ++i)
1526  {
1527  Face& f = m_faces[i];
1528  edges(int(f.m_n[0] - nbase), int(f.m_n[1] - nbase)) = -1;
1529  edges(int(f.m_n[1] - nbase), int(f.m_n[2] - nbase)) = -1;
1530  edges(int(f.m_n[2] - nbase), int(f.m_n[0] - nbase)) = -1;
1531  }
1532  /* Intersect */
1533  for (i = 0; i < ncount; ++i)
1534  {
1535  for (j = i + 1; j < ncount; ++j)
1536  {
1537  if (edges(i, j) == -1)
1538  {
1539  Node& a = m_nodes[i];
1540  Node& b = m_nodes[j];
1541  const btScalar t = ImplicitSolve(ifn, a.m_x, b.m_x, accurary);
1542  if (t > 0)
1543  {
1544  const btVector3 x = Lerp(a.m_x, b.m_x, t);
1545  const btVector3 v = Lerp(a.m_v, b.m_v, t);
1546  btScalar m = 0;
1547  if (a.m_im > 0)
1548  {
1549  if (b.m_im > 0)
1550  {
1551  const btScalar ma = 1 / a.m_im;
1552  const btScalar mb = 1 / b.m_im;
1553  const btScalar mc = Lerp(ma, mb, t);
1554  const btScalar f = (ma + mb) / (ma + mb + mc);
1555  a.m_im = 1 / (ma * f);
1556  b.m_im = 1 / (mb * f);
1557  m = mc * f;
1558  }
1559  else
1560  {
1561  a.m_im /= 0.5f;
1562  m = 1 / a.m_im;
1563  }
1564  }
1565  else
1566  {
1567  if (b.m_im > 0)
1568  {
1569  b.m_im /= 0.5f;
1570  m = 1 / b.m_im;
1571  }
1572  else
1573  m = 0;
1574  }
1575  appendNode(x, m);
1576  edges(i, j) = m_nodes.size() - 1;
1577  m_nodes[edges(i, j)].m_v = v;
1578  ++newnodes;
1579  }
1580  }
1581  }
1582  }
1583  nbase = &m_nodes[0];
1584  /* Refine links */
1585  for (i = 0, ni = m_links.size(); i < ni; ++i)
1586  {
1587  Link& feat = m_links[i];
1588  const int idx[] = {int(feat.m_n[0] - nbase),
1589  int(feat.m_n[1] - nbase)};
1590  if ((idx[0] < ncount) && (idx[1] < ncount))
1591  {
1592  const int ni = edges(idx[0], idx[1]);
1593  if (ni > 0)
1594  {
1595  appendLink(i);
1596  Link* pft[] = {&m_links[i],
1597  &m_links[m_links.size() - 1]};
1598  pft[0]->m_n[0] = &m_nodes[idx[0]];
1599  pft[0]->m_n[1] = &m_nodes[ni];
1600  pft[1]->m_n[0] = &m_nodes[ni];
1601  pft[1]->m_n[1] = &m_nodes[idx[1]];
1602  }
1603  }
1604  }
1605  /* Refine faces */
1606  for (i = 0; i < m_faces.size(); ++i)
1607  {
1608  const Face& feat = m_faces[i];
1609  const int idx[] = {int(feat.m_n[0] - nbase),
1610  int(feat.m_n[1] - nbase),
1611  int(feat.m_n[2] - nbase)};
1612  for (j = 2, k = 0; k < 3; j = k++)
1613  {
1614  if ((idx[j] < ncount) && (idx[k] < ncount))
1615  {
1616  const int ni = edges(idx[j], idx[k]);
1617  if (ni > 0)
1618  {
1619  appendFace(i);
1620  const int l = (k + 1) % 3;
1621  Face* pft[] = {&m_faces[i],
1622  &m_faces[m_faces.size() - 1]};
1623  pft[0]->m_n[0] = &m_nodes[idx[l]];
1624  pft[0]->m_n[1] = &m_nodes[idx[j]];
1625  pft[0]->m_n[2] = &m_nodes[ni];
1626  pft[1]->m_n[0] = &m_nodes[ni];
1627  pft[1]->m_n[1] = &m_nodes[idx[k]];
1628  pft[1]->m_n[2] = &m_nodes[idx[l]];
1629  appendLink(ni, idx[l], pft[0]->m_material);
1630  --i;
1631  break;
1632  }
1633  }
1634  }
1635  }
1636  /* Cut */
1637  if (cut)
1638  {
1640  const int pcount = ncount;
1641  int i;
1642  ncount = m_nodes.size();
1643  cnodes.resize(ncount, 0);
1644  /* Nodes */
1645  for (i = 0; i < ncount; ++i)
1646  {
1647  const btVector3 x = m_nodes[i].m_x;
1648  if ((i >= pcount) || (btFabs(ifn->Eval(x)) < accurary))
1649  {
1650  const btVector3 v = m_nodes[i].m_v;
1651  btScalar m = getMass(i);
1652  if (m > 0)
1653  {
1654  m *= 0.5f;
1655  m_nodes[i].m_im /= 0.5f;
1656  }
1657  appendNode(x, m);
1658  cnodes[i] = m_nodes.size() - 1;
1659  m_nodes[cnodes[i]].m_v = v;
1660  }
1661  }
1662  nbase = &m_nodes[0];
1663  /* Links */
1664  for (i = 0, ni = m_links.size(); i < ni; ++i)
1665  {
1666  const int id[] = {int(m_links[i].m_n[0] - nbase),
1667  int(m_links[i].m_n[1] - nbase)};
1668  int todetach = 0;
1669  if (cnodes[id[0]] && cnodes[id[1]])
1670  {
1671  appendLink(i);
1672  todetach = m_links.size() - 1;
1673  }
1674  else
1675  {
1676  if (((ifn->Eval(m_nodes[id[0]].m_x) < accurary) &&
1677  (ifn->Eval(m_nodes[id[1]].m_x) < accurary)))
1678  todetach = i;
1679  }
1680  if (todetach)
1681  {
1682  Link& l = m_links[todetach];
1683  for (int j = 0; j < 2; ++j)
1684  {
1685  int cn = cnodes[int(l.m_n[j] - nbase)];
1686  if (cn) l.m_n[j] = &m_nodes[cn];
1687  }
1688  }
1689  }
1690  /* Faces */
1691  for (i = 0, ni = m_faces.size(); i < ni; ++i)
1692  {
1693  Node** n = m_faces[i].m_n;
1694  if ((ifn->Eval(n[0]->m_x) < accurary) &&
1695  (ifn->Eval(n[1]->m_x) < accurary) &&
1696  (ifn->Eval(n[2]->m_x) < accurary))
1697  {
1698  for (int j = 0; j < 3; ++j)
1699  {
1700  int cn = cnodes[int(n[j] - nbase)];
1701  if (cn) n[j] = &m_nodes[cn];
1702  }
1703  }
1704  }
1705  /* Clean orphans */
1706  int nnodes = m_nodes.size();
1708  btAlignedObjectArray<int> todelete;
1709  ranks.resize(nnodes, 0);
1710  for (i = 0, ni = m_links.size(); i < ni; ++i)
1711  {
1712  for (int j = 0; j < 2; ++j) ranks[int(m_links[i].m_n[j] - nbase)]++;
1713  }
1714  for (i = 0, ni = m_faces.size(); i < ni; ++i)
1715  {
1716  for (int j = 0; j < 3; ++j) ranks[int(m_faces[i].m_n[j] - nbase)]++;
1717  }
1718  for (i = 0; i < m_links.size(); ++i)
1719  {
1720  const int id[] = {int(m_links[i].m_n[0] - nbase),
1721  int(m_links[i].m_n[1] - nbase)};
1722  const bool sg[] = {ranks[id[0]] == 1,
1723  ranks[id[1]] == 1};
1724  if (sg[0] || sg[1])
1725  {
1726  --ranks[id[0]];
1727  --ranks[id[1]];
1728  btSwap(m_links[i], m_links[m_links.size() - 1]);
1729  m_links.pop_back();
1730  --i;
1731  }
1732  }
1733 #if 0
1734  for(i=nnodes-1;i>=0;--i)
1735  {
1736  if(!ranks[i]) todelete.push_back(i);
1737  }
1738  if(todelete.size())
1739  {
1740  btAlignedObjectArray<int>& map=ranks;
1741  for(int i=0;i<nnodes;++i) map[i]=i;
1742  PointersToIndices(this);
1743  for(int i=0,ni=todelete.size();i<ni;++i)
1744  {
1745  int j=todelete[i];
1746  int& a=map[j];
1747  int& b=map[--nnodes];
1748  m_ndbvt.remove(m_nodes[a].m_leaf);m_nodes[a].m_leaf=0;
1749  btSwap(m_nodes[a],m_nodes[b]);
1750  j=a;a=b;b=j;
1751  }
1752  IndicesToPointers(this,&map[0]);
1753  m_nodes.resize(nnodes);
1754  }
1755 #endif
1756  }
1757  m_bUpdateRtCst = true;
1758 }
1759 
1760 //
1761 bool btSoftBody::cutLink(const Node* node0, const Node* node1, btScalar position)
1762 {
1763  return (cutLink(int(node0 - &m_nodes[0]), int(node1 - &m_nodes[0]), position));
1764 }
1765 
1766 //
1767 bool btSoftBody::cutLink(int node0, int node1, btScalar position)
1768 {
1769  bool done = false;
1770  int i, ni;
1771  // const btVector3 d=m_nodes[node0].m_x-m_nodes[node1].m_x;
1772  const btVector3 x = Lerp(m_nodes[node0].m_x, m_nodes[node1].m_x, position);
1773  const btVector3 v = Lerp(m_nodes[node0].m_v, m_nodes[node1].m_v, position);
1774  const btScalar m = 1;
1775  appendNode(x, m);
1776  appendNode(x, m);
1777  Node* pa = &m_nodes[node0];
1778  Node* pb = &m_nodes[node1];
1779  Node* pn[2] = {&m_nodes[m_nodes.size() - 2],
1780  &m_nodes[m_nodes.size() - 1]};
1781  pn[0]->m_v = v;
1782  pn[1]->m_v = v;
1783  for (i = 0, ni = m_links.size(); i < ni; ++i)
1784  {
1785  const int mtch = MatchEdge(m_links[i].m_n[0], m_links[i].m_n[1], pa, pb);
1786  if (mtch != -1)
1787  {
1788  appendLink(i);
1789  Link* pft[] = {&m_links[i], &m_links[m_links.size() - 1]};
1790  pft[0]->m_n[1] = pn[mtch];
1791  pft[1]->m_n[0] = pn[1 - mtch];
1792  done = true;
1793  }
1794  }
1795  for (i = 0, ni = m_faces.size(); i < ni; ++i)
1796  {
1797  for (int k = 2, l = 0; l < 3; k = l++)
1798  {
1799  const int mtch = MatchEdge(m_faces[i].m_n[k], m_faces[i].m_n[l], pa, pb);
1800  if (mtch != -1)
1801  {
1802  appendFace(i);
1803  Face* pft[] = {&m_faces[i], &m_faces[m_faces.size() - 1]};
1804  pft[0]->m_n[l] = pn[mtch];
1805  pft[1]->m_n[k] = pn[1 - mtch];
1806  appendLink(pn[0], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
1807  appendLink(pn[1], pft[0]->m_n[(l + 1) % 3], pft[0]->m_material, true);
1808  }
1809  }
1810  }
1811  if (!done)
1812  {
1813  m_ndbvt.remove(pn[0]->m_leaf);
1814  m_ndbvt.remove(pn[1]->m_leaf);
1815  m_nodes.pop_back();
1816  m_nodes.pop_back();
1817  }
1818  return (done);
1819 }
1820 
1821 //
1822 bool btSoftBody::rayTest(const btVector3& rayFrom,
1823  const btVector3& rayTo,
1824  sRayCast& results)
1825 {
1826  if (m_faces.size() && m_fdbvt.empty())
1828 
1829  results.body = this;
1830  results.fraction = 1.f;
1831  results.feature = eFeature::None;
1832  results.index = -1;
1833 
1834  return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
1835 }
1836 
1837 //
1839 {
1843  switch (preset)
1844  {
1850  break;
1853 
1857 
1859  break;
1860  }
1861 }
1862 
1864 {
1865  int i, ni;
1866 
1867  /* Update */
1868  if (m_bUpdateRtCst)
1869  {
1870  m_bUpdateRtCst = false;
1871  updateConstants();
1872  m_fdbvt.clear();
1874  {
1876  }
1877  }
1878 
1879  /* Prepare */
1880  m_sst.sdt = dt * m_cfg.timescale;
1881  m_sst.isdt = 1 / m_sst.sdt;
1882  m_sst.velmrg = m_sst.sdt * 3;
1884  m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
1885  /* Forces */
1887  applyForces();
1888  /* Integrate */
1889  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1890  {
1891  Node& n = m_nodes[i];
1892  n.m_q = n.m_x;
1893  btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
1894  {
1895  btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
1896  btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
1897  for (int c = 0; c < 3; c++)
1898  {
1899  if (deltaV[c] > clampDeltaV)
1900  {
1901  deltaV[c] = clampDeltaV;
1902  }
1903  if (deltaV[c] < -clampDeltaV)
1904  {
1905  deltaV[c] = -clampDeltaV;
1906  }
1907  }
1908  }
1909  n.m_v += deltaV;
1910  n.m_x += n.m_v * m_sst.sdt;
1911  n.m_f = btVector3(0, 0, 0);
1912  }
1913  /* Clusters */
1914  updateClusters();
1915  /* Bounds */
1916  updateBounds();
1917  /* Nodes */
1919  vol;
1920  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
1921  {
1922  Node& n = m_nodes[i];
1924  m_ndbvt.update(n.m_leaf,
1925  vol,
1926  n.m_v * m_sst.velmrg,
1927  m_sst.updmrg);
1928  }
1929  /* Faces */
1930  if (!m_fdbvt.empty())
1931  {
1932  for (int i = 0; i < m_faces.size(); ++i)
1933  {
1934  Face& f = m_faces[i];
1935  const btVector3 v = (f.m_n[0]->m_v +
1936  f.m_n[1]->m_v +
1937  f.m_n[2]->m_v) /
1938  3;
1939  vol = VolumeOf(f, m_sst.radmrg);
1940  m_fdbvt.update(f.m_leaf,
1941  vol,
1942  v * m_sst.velmrg,
1943  m_sst.updmrg);
1944  }
1945  }
1946  /* Pose */
1947  updatePose();
1948  /* Match */
1949  if (m_pose.m_bframe && (m_cfg.kMT > 0))
1950  {
1951  const btMatrix3x3 posetrs = m_pose.m_rot;
1952  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
1953  {
1954  Node& n = m_nodes[i];
1955  if (n.m_im > 0)
1956  {
1957  const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
1958  n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
1959  }
1960  }
1961  }
1962  /* Clear contacts */
1963  m_rcontacts.resize(0);
1964  m_scontacts.resize(0);
1965  /* Optimize dbvt's */
1969 }
1970 
1971 
1972 //
1974 {
1975  /* Apply clusters */
1976  applyClusters(false);
1977  /* Prepare links */
1978 
1979  int i, ni;
1980 
1981  for (i = 0, ni = m_links.size(); i < ni; ++i)
1982  {
1983  Link& l = m_links[i];
1984  l.m_c3 = l.m_n[1]->m_q - l.m_n[0]->m_q;
1985  l.m_c2 = 1 / (l.m_c3.length2() * l.m_c0);
1986  }
1987  /* Prepare anchors */
1988  for (i = 0, ni = m_anchors.size(); i < ni; ++i)
1989  {
1990  Anchor& a = m_anchors[i];
1991  const btVector3 ra = a.m_body->getWorldTransform().getBasis() * a.m_local;
1993  a.m_node->m_im,
1994  a.m_body->getInvMass(),
1996  ra);
1997  a.m_c1 = ra;
1998  a.m_c2 = m_sst.sdt * a.m_node->m_im;
1999  a.m_body->activate();
2000  }
2001  /* Solve velocities */
2002  if (m_cfg.viterations > 0)
2003  {
2004  /* Solve */
2005  for (int isolve = 0; isolve < m_cfg.viterations; ++isolve)
2006  {
2007  for (int iseq = 0; iseq < m_cfg.m_vsequence.size(); ++iseq)
2008  {
2009  getSolver(m_cfg.m_vsequence[iseq])(this, 1);
2010  }
2011  }
2012  /* Update */
2013  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2014  {
2015  Node& n = m_nodes[i];
2016  n.m_x = n.m_q + n.m_v * m_sst.sdt;
2017  }
2018  }
2019  /* Solve positions */
2020  if (m_cfg.piterations > 0)
2021  {
2022  for (int isolve = 0; isolve < m_cfg.piterations; ++isolve)
2023  {
2024  const btScalar ti = isolve / (btScalar)m_cfg.piterations;
2025  for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2026  {
2027  getSolver(m_cfg.m_psequence[iseq])(this, 1, ti);
2028  }
2029  }
2030  const btScalar vc = m_sst.isdt * (1 - m_cfg.kDP);
2031  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2032  {
2033  Node& n = m_nodes[i];
2034  n.m_v = (n.m_x - n.m_q) * vc;
2035  n.m_f = btVector3(0, 0, 0);
2036  }
2037  }
2038  /* Solve drift */
2039  if (m_cfg.diterations > 0)
2040  {
2041  const btScalar vcf = m_cfg.kVCF * m_sst.isdt;
2042  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2043  {
2044  Node& n = m_nodes[i];
2045  n.m_q = n.m_x;
2046  }
2047  for (int idrift = 0; idrift < m_cfg.diterations; ++idrift)
2048  {
2049  for (int iseq = 0; iseq < m_cfg.m_dsequence.size(); ++iseq)
2050  {
2051  getSolver(m_cfg.m_dsequence[iseq])(this, 1, 0);
2052  }
2053  }
2054  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2055  {
2056  Node& n = m_nodes[i];
2057  n.m_v += (n.m_x - n.m_q) * vcf;
2058  }
2059  }
2060  /* Apply clusters */
2061  dampClusters();
2062  applyClusters(true);
2063 }
2064 
2065 //
2066 void btSoftBody::staticSolve(int iterations)
2067 {
2068  for (int isolve = 0; isolve < iterations; ++isolve)
2069  {
2070  for (int iseq = 0; iseq < m_cfg.m_psequence.size(); ++iseq)
2071  {
2072  getSolver(m_cfg.m_psequence[iseq])(this, 1, 0);
2073  }
2074  }
2075 }
2076 
2077 //
2078 void btSoftBody::solveCommonConstraints(btSoftBody** /*bodies*/, int /*count*/, int /*iterations*/)
2079 {
2081 }
2082 
2083 //
2085 {
2086  const int nb = bodies.size();
2087  int iterations = 0;
2088  int i;
2089 
2090  for (i = 0; i < nb; ++i)
2091  {
2092  iterations = btMax(iterations, bodies[i]->m_cfg.citerations);
2093  }
2094  for (i = 0; i < nb; ++i)
2095  {
2096  bodies[i]->prepareClusters(iterations);
2097  }
2098  for (i = 0; i < iterations; ++i)
2099  {
2100  const btScalar sor = 1;
2101  for (int j = 0; j < nb; ++j)
2102  {
2103  bodies[j]->solveClusters(sor);
2104  }
2105  }
2106  for (i = 0; i < nb; ++i)
2107  {
2108  bodies[i]->cleanupClusters();
2109  }
2110 }
2111 
2112 //
2114 {
2115  /* Update */
2116  updateNormals();
2117 }
2118 
2119 //
2121 {
2122  m_rayFrom = rayFrom;
2123  m_rayNormalizedDirection = (rayTo - rayFrom);
2124  m_rayTo = rayTo;
2125  m_mint = mxt;
2126  m_face = 0;
2127  m_tests = 0;
2128 }
2129 
2130 //
2132 {
2133  btSoftBody::Face& f = *(btSoftBody::Face*)leaf->data;
2134  const btScalar t = rayFromToTriangle(m_rayFrom, m_rayTo, m_rayNormalizedDirection,
2135  f.m_n[0]->m_x,
2136  f.m_n[1]->m_x,
2137  f.m_n[2]->m_x,
2138  m_mint);
2139  if ((t > 0) && (t < m_mint))
2140  {
2141  m_mint = t;
2142  m_face = &f;
2143  }
2144  ++m_tests;
2145 }
2146 
2147 //
2149  const btVector3& rayTo,
2150  const btVector3& rayNormalizedDirection,
2151  const btVector3& a,
2152  const btVector3& b,
2153  const btVector3& c,
2154  btScalar maxt)
2155 {
2156  static const btScalar ceps = -SIMD_EPSILON * 10;
2157  static const btScalar teps = SIMD_EPSILON * 10;
2158 
2159  const btVector3 n = btCross(b - a, c - a);
2160  const btScalar d = btDot(a, n);
2161  const btScalar den = btDot(rayNormalizedDirection, n);
2162  if (!btFuzzyZero(den))
2163  {
2164  const btScalar num = btDot(rayFrom, n) - d;
2165  const btScalar t = -num / den;
2166  if ((t > teps) && (t < maxt))
2167  {
2168  const btVector3 hit = rayFrom + rayNormalizedDirection * t;
2169  if ((btDot(n, btCross(a - hit, b - hit)) > ceps) &&
2170  (btDot(n, btCross(b - hit, c - hit)) > ceps) &&
2171  (btDot(n, btCross(c - hit, a - hit)) > ceps))
2172  {
2173  return (t);
2174  }
2175  }
2176  }
2177  return (-1);
2178 }
2179 
2180 //
2182 {
2183 #define PTR2IDX(_p_, _b_) reinterpret_cast<btSoftBody::Node*>((_p_) - (_b_))
2184  btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2185  int i, ni;
2186 
2187  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2188  {
2189  if (m_nodes[i].m_leaf)
2190  {
2191  m_nodes[i].m_leaf->data = *(void**)&i;
2192  }
2193  }
2194  for (i = 0, ni = m_links.size(); i < ni; ++i)
2195  {
2196  m_links[i].m_n[0] = PTR2IDX(m_links[i].m_n[0], base);
2197  m_links[i].m_n[1] = PTR2IDX(m_links[i].m_n[1], base);
2198  }
2199  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2200  {
2201  m_faces[i].m_n[0] = PTR2IDX(m_faces[i].m_n[0], base);
2202  m_faces[i].m_n[1] = PTR2IDX(m_faces[i].m_n[1], base);
2203  m_faces[i].m_n[2] = PTR2IDX(m_faces[i].m_n[2], base);
2204  if (m_faces[i].m_leaf)
2205  {
2206  m_faces[i].m_leaf->data = *(void**)&i;
2207  }
2208  }
2209  for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2210  {
2211  m_anchors[i].m_node = PTR2IDX(m_anchors[i].m_node, base);
2212  }
2213  for (i = 0, ni = m_notes.size(); i < ni; ++i)
2214  {
2215  for (int j = 0; j < m_notes[i].m_rank; ++j)
2216  {
2217  m_notes[i].m_nodes[j] = PTR2IDX(m_notes[i].m_nodes[j], base);
2218  }
2219  }
2220 #undef PTR2IDX
2221 }
2222 
2223 //
2224 void btSoftBody::indicesToPointers(const int* map)
2225 {
2226 #define IDX2PTR(_p_, _b_) map ? (&(_b_)[map[(((char*)_p_) - (char*)0)]]) : (&(_b_)[(((char*)_p_) - (char*)0)])
2227  btSoftBody::Node* base = m_nodes.size() ? &m_nodes[0] : 0;
2228  int i, ni;
2229 
2230  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2231  {
2232  if (m_nodes[i].m_leaf)
2233  {
2234  m_nodes[i].m_leaf->data = &m_nodes[i];
2235  }
2236  }
2237  for (i = 0, ni = m_links.size(); i < ni; ++i)
2238  {
2239  m_links[i].m_n[0] = IDX2PTR(m_links[i].m_n[0], base);
2240  m_links[i].m_n[1] = IDX2PTR(m_links[i].m_n[1], base);
2241  }
2242  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2243  {
2244  m_faces[i].m_n[0] = IDX2PTR(m_faces[i].m_n[0], base);
2245  m_faces[i].m_n[1] = IDX2PTR(m_faces[i].m_n[1], base);
2246  m_faces[i].m_n[2] = IDX2PTR(m_faces[i].m_n[2], base);
2247  if (m_faces[i].m_leaf)
2248  {
2249  m_faces[i].m_leaf->data = &m_faces[i];
2250  }
2251  }
2252  for (i = 0, ni = m_anchors.size(); i < ni; ++i)
2253  {
2254  m_anchors[i].m_node = IDX2PTR(m_anchors[i].m_node, base);
2255  }
2256  for (i = 0, ni = m_notes.size(); i < ni; ++i)
2257  {
2258  for (int j = 0; j < m_notes[i].m_rank; ++j)
2259  {
2260  m_notes[i].m_nodes[j] = IDX2PTR(m_notes[i].m_nodes[j], base);
2261  }
2262  }
2263 #undef IDX2PTR
2264 }
2265 
2266 //
2267 int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
2268  btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const
2269 {
2270  int cnt = 0;
2271  btVector3 dir = rayTo - rayFrom;
2272 
2273  if (bcountonly || m_fdbvt.empty())
2274  { /* Full search */
2275 
2276  for (int i = 0, ni = m_faces.size(); i < ni; ++i)
2277  {
2278  const btSoftBody::Face& f = m_faces[i];
2279 
2280  const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2281  f.m_n[0]->m_x,
2282  f.m_n[1]->m_x,
2283  f.m_n[2]->m_x,
2284  mint);
2285  if (t > 0)
2286  {
2287  ++cnt;
2288  if (!bcountonly)
2289  {
2290  feature = btSoftBody::eFeature::Face;
2291  index = i;
2292  mint = t;
2293  }
2294  }
2295  }
2296  }
2297  else
2298  { /* Use dbvt */
2299  RayFromToCaster collider(rayFrom, rayTo, mint);
2300 
2301  btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
2302  if (collider.m_face)
2303  {
2304  mint = collider.m_mint;
2305  feature = btSoftBody::eFeature::Face;
2306  index = (int)(collider.m_face - &m_faces[0]);
2307  cnt = 1;
2308  }
2309  }
2310 
2311  for (int i = 0; i < m_tetras.size(); i++)
2312  {
2313  const btSoftBody::Tetra& tet = m_tetras[i];
2314  int tetfaces[4][3] = {{0, 1, 2}, {0, 1, 3}, {1, 2, 3}, {0, 2, 3}};
2315  for (int f = 0; f < 4; f++)
2316  {
2317  int index0 = tetfaces[f][0];
2318  int index1 = tetfaces[f][1];
2319  int index2 = tetfaces[f][2];
2320  btVector3 v0 = tet.m_n[index0]->m_x;
2321  btVector3 v1 = tet.m_n[index1]->m_x;
2322  btVector3 v2 = tet.m_n[index2]->m_x;
2323 
2324  const btScalar t = RayFromToCaster::rayFromToTriangle(rayFrom, rayTo, dir,
2325  v0, v1, v2,
2326  mint);
2327  if (t > 0)
2328  {
2329  ++cnt;
2330  if (!bcountonly)
2331  {
2332  feature = btSoftBody::eFeature::Tetra;
2333  index = i;
2334  mint = t;
2335  }
2336  }
2337  }
2338  }
2339  return (cnt);
2340 }
2341 
2342 //
2344 {
2345  m_fdbvt.clear();
2346  for (int i = 0; i < m_faces.size(); ++i)
2347  {
2348  Face& f = m_faces[i];
2349  f.m_leaf = m_fdbvt.insert(VolumeOf(f, 0), &f);
2350  }
2351 }
2352 
2353 //
2355 {
2356  btVector3 com(0, 0, 0);
2357  if (m_pose.m_bframe)
2358  {
2359  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2360  {
2361  com += m_nodes[i].m_x * m_pose.m_wgh[i];
2362  }
2363  }
2364  return (com);
2365 }
2366 
2368  const btVector3& x,
2369  btScalar margin,
2370  btSoftBody::sCti& cti) const
2371 {
2372  btVector3 nrm;
2373  const btCollisionShape* shp = colObjWrap->getCollisionShape();
2374  // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
2375  //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
2376  const btTransform& wtr = colObjWrap->getWorldTransform();
2377  //todo: check which transform is needed here
2378 
2379  btScalar dst =
2381  wtr.invXform(x),
2382  shp,
2383  nrm,
2384  margin);
2385  if (dst < 0)
2386  {
2387  cti.m_colObj = colObjWrap->getCollisionObject();
2388  cti.m_normal = wtr.getBasis() * nrm;
2389  cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
2390  return (true);
2391  }
2392  return (false);
2393 }
2394 
2395 //
2397  const btVector3& x,
2398  btScalar margin,
2399  btSoftBody::sCti& cti, bool predict) const
2400 {
2401  btVector3 nrm;
2402  const btCollisionShape* shp = colObjWrap->getCollisionShape();
2403  const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2404  // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2405  // but resolve contact at x_n
2406 // btTransform wtr = (predict) ?
2407 // (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2408 // : colObjWrap->getWorldTransform();
2409  const btTransform& wtr = colObjWrap->getWorldTransform();
2410  btScalar dst =
2412  wtr.invXform(x),
2413  shp,
2414  nrm,
2415  margin);
2416  if (!predict)
2417  {
2418  cti.m_colObj = colObjWrap->getCollisionObject();
2419  cti.m_normal = wtr.getBasis() * nrm;
2420  cti.m_offset = dst;
2421  }
2422  if (dst < 0)
2423  return true;
2424  return (false);
2425 }
2426 
2427 //
2428 // Compute barycentric coordinates (u, v, w) for
2429 // point p with respect to triangle (a, b, c)
2430 static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
2431 {
2432  btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
2433  btScalar d00 = v0.dot(v0);
2434  btScalar d01 = v0.dot(v1);
2435  btScalar d11 = v1.dot(v1);
2436  btScalar d20 = v2.dot(v0);
2437  btScalar d21 = v2.dot(v1);
2438  btScalar denom = d00 * d11 - d01 * d01;
2439  bary.setY((d11 * d20 - d01 * d21) / denom);
2440  bary.setZ((d00 * d21 - d01 * d20) / denom);
2441  bary.setX(btScalar(1) - bary.getY() - bary.getZ());
2442 }
2443 
2444 //
2446  Face& f,
2447  btVector3& contact_point,
2448  btVector3& bary,
2449  btScalar margin,
2450  btSoftBody::sCti& cti, bool predict) const
2451 {
2452  btVector3 nrm;
2453  const btCollisionShape* shp = colObjWrap->getCollisionShape();
2454  const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
2455  // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
2456  // but resolve contact at x_n
2457  btTransform wtr = (predict) ?
2458  (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
2459  : colObjWrap->getWorldTransform();
2460 // const btTransform& wtr = colObjWrap->getWorldTransform();
2461  btScalar dst;
2462 
2463 //#define USE_QUADRATURE 1
2464 //#define CACHE_PREV_COLLISION
2465 
2466  // use the contact position of the previous collision
2467 #ifdef CACHE_PREV_COLLISION
2468  if (f.m_pcontact[3] != 0)
2469  {
2470  for (int i = 0; i < 3; ++i)
2471  bary[i] = f.m_pcontact[i];
2472  contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2474  wtr.invXform(contact_point),
2475  shp,
2476  nrm,
2477  margin);
2478  nrm = wtr.getBasis() * nrm;
2479  // use cached contact point
2480  }
2481  else
2482  {
2483  btGjkEpaSolver2::sResults results;
2484  btTransform triangle_transform;
2485  triangle_transform.setIdentity();
2486  triangle_transform.setOrigin(f.m_n[0]->m_x);
2487  btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
2488  btVector3 guess(0,0,0);
2489  const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2490  btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2491  dst = results.distance - margin;
2492  contact_point = results.witnesses[0];
2493  getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2494  nrm = results.normal;
2495  for (int i = 0; i < 3; ++i)
2496  f.m_pcontact[i] = bary[i];
2497  }
2498 
2499 #endif
2500 
2501  // use collision quadrature point
2502 #ifdef USE_QUADRATURE
2503  {
2504  dst = SIMD_INFINITY;
2505  btVector3 local_nrm;
2506  for (int q = 0; q < m_quads.size(); ++q)
2507  {
2508  btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
2510  wtr.invXform(p),
2511  shp,
2512  local_nrm,
2513  margin);
2514  if (local_dst < dst)
2515  {
2516  dst = local_dst;
2517  contact_point = p;
2518  bary = m_quads[q];
2519  nrm = wtr.getBasis() * local_nrm;
2520  }
2521  }
2522  }
2523 #endif
2524 
2525  // regular face contact
2526  {
2527  btGjkEpaSolver2::sResults results;
2528  btTransform triangle_transform;
2529  triangle_transform.setIdentity();
2530  triangle_transform.setOrigin(f.m_n[0]->m_x);
2531  btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
2532  btVector3 guess(0,0,0);
2533  const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
2534  btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
2535  dst = results.distance - margin;
2536  contact_point = results.witnesses[0];
2537  getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
2538  nrm = results.normal;
2539  for (int i = 0; i < 3; ++i)
2540  f.m_pcontact[i] = bary[i];
2541  }
2542 
2543  if (!predict)
2544  {
2545  cti.m_colObj = colObjWrap->getCollisionObject();
2546  cti.m_normal = nrm;
2547  cti.m_offset = dst;
2548  }
2549 
2550  if (dst < 0)
2551  return true;
2552  return (false);
2553 }
2554 
2555 //
2557 {
2558  const btVector3 zv(0, 0, 0);
2559  int i, ni;
2560 
2561  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2562  {
2563  m_nodes[i].m_n = zv;
2564  }
2565  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2566  {
2567  btSoftBody::Face& f = m_faces[i];
2568  const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
2569  f.m_n[2]->m_x - f.m_n[0]->m_x);
2570  f.m_normal = n;
2571  f.m_normal.safeNormalize();
2572  f.m_n[0]->m_n += n;
2573  f.m_n[1]->m_n += n;
2574  f.m_n[2]->m_n += n;
2575  }
2576  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2577  {
2578  btScalar len = m_nodes[i].m_n.length();
2579  if (len > SIMD_EPSILON)
2580  m_nodes[i].m_n /= len;
2581  }
2582 }
2583 
2584 //
2586 {
2587  /*if( m_acceleratedSoftBody )
2588  {
2589  // If we have an accelerated softbody we need to obtain the bounds correctly
2590  // For now (slightly hackily) just have a very large AABB
2591  // TODO: Write get bounds kernel
2592  // If that is updating in place, atomic collisions might be low (when the cloth isn't perfectly aligned to an axis) and we could
2593  // probably do a test and exchange reasonably efficiently.
2594 
2595  m_bounds[0] = btVector3(-1000, -1000, -1000);
2596  m_bounds[1] = btVector3(1000, 1000, 1000);
2597 
2598  } else {*/
2599 // if (m_ndbvt.m_root)
2600 // {
2601 // const btVector3& mins = m_ndbvt.m_root->volume.Mins();
2602 // const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
2603 // const btScalar csm = getCollisionShape()->getMargin();
2604 // const btVector3 mrg = btVector3(csm,
2605 // csm,
2606 // csm) *
2607 // 1; // ??? to investigate...
2608 // m_bounds[0] = mins - mrg;
2609 // m_bounds[1] = maxs + mrg;
2610 // if (0 != getBroadphaseHandle())
2611 // {
2612 // m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
2613 // m_bounds[0],
2614 // m_bounds[1],
2615 // m_worldInfo->m_dispatcher);
2616 // }
2617 // }
2618 // else
2619 // {
2620 // m_bounds[0] =
2621 // m_bounds[1] = btVector3(0, 0, 0);
2622 // }
2623  if (m_nodes.size())
2624  {
2625  btVector3 mins = m_nodes[0].m_x;
2626  btVector3 maxs = m_nodes[0].m_x;
2627  for (int i = 1; i < m_nodes.size(); ++i)
2628  {
2629  for (int d = 0; d < 3; ++d)
2630  {
2631  if (m_nodes[i].m_x[d] > maxs[d])
2632  maxs[d] = m_nodes[i].m_x[d];
2633  if (m_nodes[i].m_x[d] < mins[d])
2634  mins[d] = m_nodes[i].m_x[d];
2635  }
2636  }
2637  const btScalar csm = getCollisionShape()->getMargin();
2638  const btVector3 mrg = btVector3(csm,
2639  csm,
2640  csm);
2641  m_bounds[0] = mins - mrg;
2642  m_bounds[1] = maxs + mrg;
2643  if (0 != getBroadphaseHandle())
2644  {
2646  m_bounds[0],
2647  m_bounds[1],
2649  }
2650  }
2651  else
2652  {
2653  m_bounds[0] =
2654  m_bounds[1] = btVector3(0, 0, 0);
2655  }
2656 }
2657 
2658 //
2660 {
2661  if (m_pose.m_bframe)
2662  {
2663  btSoftBody::Pose& pose = m_pose;
2664  const btVector3 com = evaluateCom();
2665  /* Com */
2666  pose.m_com = com;
2667  /* Rotation */
2668  btMatrix3x3 Apq;
2669  const btScalar eps = SIMD_EPSILON;
2670  Apq[0] = Apq[1] = Apq[2] = btVector3(0, 0, 0);
2671  Apq[0].setX(eps);
2672  Apq[1].setY(eps * 2);
2673  Apq[2].setZ(eps * 3);
2674  for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
2675  {
2676  const btVector3 a = pose.m_wgh[i] * (m_nodes[i].m_x - com);
2677  const btVector3& b = pose.m_pos[i];
2678  Apq[0] += a.x() * b;
2679  Apq[1] += a.y() * b;
2680  Apq[2] += a.z() * b;
2681  }
2682  btMatrix3x3 r, s;
2683  PolarDecompose(Apq, r, s);
2684  pose.m_rot = r;
2685  pose.m_scl = pose.m_aqq * r.transpose() * Apq;
2686  if (m_cfg.maxvolume > 1)
2687  {
2688  const btScalar idet = Clamp<btScalar>(1 / pose.m_scl.determinant(),
2689  1, m_cfg.maxvolume);
2690  pose.m_scl = Mul(pose.m_scl, idet);
2691  }
2692  }
2693 }
2694 
2695 //
2696 void btSoftBody::updateArea(bool averageArea)
2697 {
2698  int i, ni;
2699 
2700  /* Face area */
2701  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2702  {
2703  Face& f = m_faces[i];
2704  f.m_ra = AreaOf(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x);
2705  }
2706 
2707  /* Node area */
2708 
2709  if (averageArea)
2710  {
2712  counts.resize(m_nodes.size(), 0);
2713  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2714  {
2715  m_nodes[i].m_area = 0;
2716  }
2717  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2718  {
2719  btSoftBody::Face& f = m_faces[i];
2720  for (int j = 0; j < 3; ++j)
2721  {
2722  const int index = (int)(f.m_n[j] - &m_nodes[0]);
2723  counts[index]++;
2724  f.m_n[j]->m_area += btFabs(f.m_ra);
2725  }
2726  }
2727  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2728  {
2729  if (counts[i] > 0)
2730  m_nodes[i].m_area /= (btScalar)counts[i];
2731  else
2732  m_nodes[i].m_area = 0;
2733  }
2734  }
2735  else
2736  {
2737  // initialize node area as zero
2738  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2739  {
2740  m_nodes[i].m_area = 0;
2741  }
2742 
2743  for (i = 0, ni = m_faces.size(); i < ni; ++i)
2744  {
2745  btSoftBody::Face& f = m_faces[i];
2746 
2747  for (int j = 0; j < 3; ++j)
2748  {
2749  f.m_n[j]->m_area += f.m_ra;
2750  }
2751  }
2752 
2753  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
2754  {
2755  m_nodes[i].m_area *= 0.3333333f;
2756  }
2757  }
2758 }
2759 
2761 {
2762  int i, ni;
2763 
2764  /* Links */
2765  for (i = 0, ni = m_links.size(); i < ni; ++i)
2766  {
2767  Link& l = m_links[i];
2768  Material& m = *l.m_material;
2769  l.m_c0 = (l.m_n[0]->m_im + l.m_n[1]->m_im) / m.m_kLST;
2770  }
2771 }
2772 
2774 {
2777  updateArea();
2778 }
2779 
2780 //
2782 {
2783  int i;
2784 
2785  for (i = 0; i < m_clusters.size(); ++i)
2786  {
2787  Cluster& c = *m_clusters[i];
2788  c.m_imass = 0;
2789  c.m_masses.resize(c.m_nodes.size());
2790  for (int j = 0; j < c.m_nodes.size(); ++j)
2791  {
2792  if (c.m_nodes[j]->m_im == 0)
2793  {
2794  c.m_containsAnchor = true;
2795  c.m_masses[j] = BT_LARGE_FLOAT;
2796  }
2797  else
2798  {
2799  c.m_masses[j] = btScalar(1.) / c.m_nodes[j]->m_im;
2800  }
2801  c.m_imass += c.m_masses[j];
2802  }
2803  c.m_imass = btScalar(1.) / c.m_imass;
2804  c.m_com = btSoftBody::clusterCom(&c);
2805  c.m_lv = btVector3(0, 0, 0);
2806  c.m_av = btVector3(0, 0, 0);
2807  c.m_leaf = 0;
2808  /* Inertia */
2809  btMatrix3x3& ii = c.m_locii;
2810  ii[0] = ii[1] = ii[2] = btVector3(0, 0, 0);
2811  {
2812  int i, ni;
2813 
2814  for (i = 0, ni = c.m_nodes.size(); i < ni; ++i)
2815  {
2816  const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
2817  const btVector3 q = k * k;
2818  const btScalar m = c.m_masses[i];
2819  ii[0][0] += m * (q[1] + q[2]);
2820  ii[1][1] += m * (q[0] + q[2]);
2821  ii[2][2] += m * (q[0] + q[1]);
2822  ii[0][1] -= m * k[0] * k[1];
2823  ii[0][2] -= m * k[0] * k[2];
2824  ii[1][2] -= m * k[1] * k[2];
2825  }
2826  }
2827  ii[1][0] = ii[0][1];
2828  ii[2][0] = ii[0][2];
2829  ii[2][1] = ii[1][2];
2830 
2831  ii = ii.inverse();
2832 
2833  /* Frame */
2836  c.m_framerefs.resize(c.m_nodes.size());
2837  {
2838  int i;
2839  for (i = 0; i < c.m_framerefs.size(); ++i)
2840  {
2841  c.m_framerefs[i] = c.m_nodes[i]->m_x - c.m_com;
2842  }
2843  }
2844  }
2845 }
2846 
2847 //
2849 {
2850  BT_PROFILE("UpdateClusters");
2851  int i;
2852 
2853  for (i = 0; i < m_clusters.size(); ++i)
2854  {
2856  const int n = c.m_nodes.size();
2857  //const btScalar invn=1/(btScalar)n;
2858  if (n)
2859  {
2860  /* Frame */
2861  const btScalar eps = btScalar(0.0001);
2862  btMatrix3x3 m, r, s;
2863  m[0] = m[1] = m[2] = btVector3(0, 0, 0);
2864  m[0][0] = eps * 1;
2865  m[1][1] = eps * 2;
2866  m[2][2] = eps * 3;
2867  c.m_com = clusterCom(&c);
2868  for (int i = 0; i < c.m_nodes.size(); ++i)
2869  {
2870  const btVector3 a = c.m_nodes[i]->m_x - c.m_com;
2871  const btVector3& b = c.m_framerefs[i];
2872  m[0] += a[0] * b;
2873  m[1] += a[1] * b;
2874  m[2] += a[2] * b;
2875  }
2876  PolarDecompose(m, r, s);
2878  c.m_framexform.setBasis(r);
2879  /* Inertia */
2880 #if 1 /* Constant */
2882 #else
2883 #if 0 /* Sphere */
2884  const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass);
2885  const btVector3 inertia(rk,rk,rk);
2886  const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0,
2887  btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0,
2888  btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0);
2889 
2890  c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose();
2891 #else /* Actual */
2892  c.m_invwi[0] = c.m_invwi[1] = c.m_invwi[2] = btVector3(0, 0, 0);
2893  for (int i = 0; i < n; ++i)
2894  {
2895  const btVector3 k = c.m_nodes[i]->m_x - c.m_com;
2896  const btVector3 q = k * k;
2897  const btScalar m = 1 / c.m_nodes[i]->m_im;
2898  c.m_invwi[0][0] += m * (q[1] + q[2]);
2899  c.m_invwi[1][1] += m * (q[0] + q[2]);
2900  c.m_invwi[2][2] += m * (q[0] + q[1]);
2901  c.m_invwi[0][1] -= m * k[0] * k[1];
2902  c.m_invwi[0][2] -= m * k[0] * k[2];
2903  c.m_invwi[1][2] -= m * k[1] * k[2];
2904  }
2905  c.m_invwi[1][0] = c.m_invwi[0][1];
2906  c.m_invwi[2][0] = c.m_invwi[0][2];
2907  c.m_invwi[2][1] = c.m_invwi[1][2];
2908  c.m_invwi = c.m_invwi.inverse();
2909 #endif
2910 #endif
2911  /* Velocities */
2912  c.m_lv = btVector3(0, 0, 0);
2913  c.m_av = btVector3(0, 0, 0);
2914  {
2915  int i;
2916 
2917  for (i = 0; i < n; ++i)
2918  {
2919  const btVector3 v = c.m_nodes[i]->m_v * c.m_masses[i];
2920  c.m_lv += v;
2921  c.m_av += btCross(c.m_nodes[i]->m_x - c.m_com, v);
2922  }
2923  }
2924  c.m_lv = c.m_imass * c.m_lv * (1 - c.m_ldamping);
2925  c.m_av = c.m_invwi * c.m_av * (1 - c.m_adamping);
2926  c.m_vimpulses[0] =
2927  c.m_vimpulses[1] = btVector3(0, 0, 0);
2928  c.m_dimpulses[0] =
2929  c.m_dimpulses[1] = btVector3(0, 0, 0);
2930  c.m_nvimpulses = 0;
2931  c.m_ndimpulses = 0;
2932  /* Matching */
2933  if (c.m_matching > 0)
2934  {
2935  for (int j = 0; j < c.m_nodes.size(); ++j)
2936  {
2937  Node& n = *c.m_nodes[j];
2938  const btVector3 x = c.m_framexform * c.m_framerefs[j];
2939  n.m_x = Lerp(n.m_x, x, c.m_matching);
2940  }
2941  }
2942  /* Dbvt */
2943  if (c.m_collide)
2944  {
2945  btVector3 mi = c.m_nodes[0]->m_x;
2946  btVector3 mx = mi;
2947  for (int j = 1; j < n; ++j)
2948  {
2949  mi.setMin(c.m_nodes[j]->m_x);
2950  mx.setMax(c.m_nodes[j]->m_x);
2951  }
2953  bounds = btDbvtVolume::FromMM(mi, mx);
2954  if (c.m_leaf)
2956  else
2957  c.m_leaf = m_cdbvt.insert(bounds, &c);
2958  }
2959  }
2960  }
2961 }
2962 
2963 //
2965 {
2966  for (int i = 0; i < m_joints.size(); ++i)
2967  {
2968  m_joints[i]->Terminate(m_sst.sdt);
2969  if (m_joints[i]->m_delete)
2970  {
2971  btAlignedFree(m_joints[i]);
2972  m_joints.remove(m_joints[i--]);
2973  }
2974  }
2975 }
2976 
2977 //
2978 void btSoftBody::prepareClusters(int iterations)
2979 {
2980  for (int i = 0; i < m_joints.size(); ++i)
2981  {
2982  m_joints[i]->Prepare(m_sst.sdt, iterations);
2983  }
2984 }
2985 
2986 //
2988 {
2989  for (int i = 0, ni = m_joints.size(); i < ni; ++i)
2990  {
2991  m_joints[i]->Solve(m_sst.sdt, sor);
2992  }
2993 }
2994 
2995 //
2997 {
2998  BT_PROFILE("ApplyClusters");
2999  // const btScalar f0=m_sst.sdt;
3000  //const btScalar f1=f0/2;
3003  deltas.resize(m_nodes.size(), btVector3(0, 0, 0));
3004  weights.resize(m_nodes.size(), 0);
3005  int i;
3006 
3007  if (drift)
3008  {
3009  for (i = 0; i < m_clusters.size(); ++i)
3010  {
3011  Cluster& c = *m_clusters[i];
3012  if (c.m_ndimpulses)
3013  {
3014  c.m_dimpulses[0] /= (btScalar)c.m_ndimpulses;
3015  c.m_dimpulses[1] /= (btScalar)c.m_ndimpulses;
3016  }
3017  }
3018  }
3019 
3020  for (i = 0; i < m_clusters.size(); ++i)
3021  {
3022  Cluster& c = *m_clusters[i];
3023  if (0 < (drift ? c.m_ndimpulses : c.m_nvimpulses))
3024  {
3025  const btVector3 v = (drift ? c.m_dimpulses[0] : c.m_vimpulses[0]) * m_sst.sdt;
3026  const btVector3 w = (drift ? c.m_dimpulses[1] : c.m_vimpulses[1]) * m_sst.sdt;
3027  for (int j = 0; j < c.m_nodes.size(); ++j)
3028  {
3029  const int idx = int(c.m_nodes[j] - &m_nodes[0]);
3030  const btVector3& x = c.m_nodes[j]->m_x;
3031  const btScalar q = c.m_masses[j];
3032  deltas[idx] += (v + btCross(w, x - c.m_com)) * q;
3033  weights[idx] += q;
3034  }
3035  }
3036  }
3037  for (i = 0; i < deltas.size(); ++i)
3038  {
3039  if (weights[i] > 0)
3040  {
3041  m_nodes[i].m_x += deltas[i] / weights[i];
3042  }
3043  }
3044 }
3045 
3046 //
3048 {
3049  int i;
3050 
3051  for (i = 0; i < m_clusters.size(); ++i)
3052  {
3053  Cluster& c = *m_clusters[i];
3054  if (c.m_ndamping > 0)
3055  {
3056  for (int j = 0; j < c.m_nodes.size(); ++j)
3057  {
3058  Node& n = *c.m_nodes[j];
3059  if (n.m_im > 0)
3060  {
3061  const btVector3 vx = c.m_lv + btCross(c.m_av, c.m_nodes[j]->m_q - c.m_com);
3062  if (vx.length2() <= n.m_v.length2())
3063  {
3064  n.m_v += c.m_ndamping * (vx - n.m_v);
3065  }
3066  }
3067  }
3068  }
3069  }
3070 }
3071 
3073 {
3074  for (int i = 0; i < m_links.size(); ++i)
3075  {
3076  m_links[i].Feature::m_material->m_kLST = k;
3077  }
3078 }
3079 
3081 {
3082  btScalar unit_simplex_measure = 1./6.;
3083 
3084  for (int i = 0; i < m_tetras.size(); ++i)
3085  {
3086  Tetra &t = m_tetras[i];
3087  btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
3088  btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
3089  btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
3090  btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(),
3091  c1.getY(), c2.getY(), c3.getY(),
3092  c1.getZ(), c2.getZ(), c3.getZ());
3093  t.m_element_measure = Dm.determinant() * unit_simplex_measure;
3094  t.m_Dm_inverse = Dm.inverse();
3095  }
3096 }
3097 
3099 {
3100  for (int i = 0; i < m_tetras.size(); ++i)
3101  {
3102  btSoftBody::Tetra& t = m_tetras[i];
3103  btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
3104  btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
3105  btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
3106  btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
3107  c1.getY(), c2.getY(), c3.getY(),
3108  c1.getZ(), c2.getZ(), c3.getZ());
3109  t.m_F = Ds * t.m_Dm_inverse;
3110 
3112  s.m_F = t.m_F;
3113  s.m_J = t.m_F.determinant();
3114  btMatrix3x3 C = t.m_F.transpose()*t.m_F;
3115  s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ();
3116  s.m_cofF = t.m_F.adjoint().transpose();
3117  }
3118 }
3119 
3121 {
3123  for (int i = 0; i < m_tetras.size(); ++i)
3124  {
3126  }
3127 }
3128 //
3130 {
3131  m_bodies[0].activate();
3132  m_bodies[1].activate();
3133 }
3134 
3135 //
3136 void btSoftBody::LJoint::Prepare(btScalar dt, int iterations)
3137 {
3138  static const btScalar maxdrift = 4;
3139  Joint::Prepare(dt, iterations);
3140  m_rpos[0] = m_bodies[0].xform() * m_refs[0];
3141  m_rpos[1] = m_bodies[1].xform() * m_refs[1];
3142  m_drift = Clamp(m_rpos[0] - m_rpos[1], maxdrift) * m_erp / dt;
3143  m_rpos[0] -= m_bodies[0].xform().getOrigin();
3144  m_rpos[1] -= m_bodies[1].xform().getOrigin();
3145  m_massmatrix = ImpulseMatrix(m_bodies[0].invMass(), m_bodies[0].invWorldInertia(), m_rpos[0],
3146  m_bodies[1].invMass(), m_bodies[1].invWorldInertia(), m_rpos[1]);
3147  if (m_split > 0)
3148  {
3149  m_sdrift = m_massmatrix * (m_drift * m_split);
3150  m_drift *= 1 - m_split;
3151  }
3152  m_drift /= (btScalar)iterations;
3153 }
3154 
3155 //
3157 {
3158  const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3159  const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3160  const btVector3 vr = va - vb;
3161  btSoftBody::Impulse impulse;
3162  impulse.m_asVelocity = 1;
3163  impulse.m_velocity = m_massmatrix * (m_drift + vr * m_cfm) * sor;
3164  m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3165  m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3166 }
3167 
3168 //
3170 {
3171  if (m_split > 0)
3172  {
3173  m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3174  m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3175  }
3176 }
3177 
3178 //
3179 void btSoftBody::AJoint::Prepare(btScalar dt, int iterations)
3180 {
3181  static const btScalar maxdrift = SIMD_PI / 16;
3182  m_icontrol->Prepare(this);
3183  Joint::Prepare(dt, iterations);
3184  m_axis[0] = m_bodies[0].xform().getBasis() * m_refs[0];
3185  m_axis[1] = m_bodies[1].xform().getBasis() * m_refs[1];
3186  m_drift = NormalizeAny(btCross(m_axis[1], m_axis[0]));
3187  m_drift *= btMin(maxdrift, btAcos(Clamp<btScalar>(btDot(m_axis[0], m_axis[1]), -1, +1)));
3188  m_drift *= m_erp / dt;
3189  m_massmatrix = AngularImpulseMatrix(m_bodies[0].invWorldInertia(), m_bodies[1].invWorldInertia());
3190  if (m_split > 0)
3191  {
3192  m_sdrift = m_massmatrix * (m_drift * m_split);
3193  m_drift *= 1 - m_split;
3194  }
3195  m_drift /= (btScalar)iterations;
3196 }
3197 
3198 //
3200 {
3201  const btVector3 va = m_bodies[0].angularVelocity();
3202  const btVector3 vb = m_bodies[1].angularVelocity();
3203  const btVector3 vr = va - vb;
3204  const btScalar sp = btDot(vr, m_axis[0]);
3205  const btVector3 vc = vr - m_axis[0] * m_icontrol->Speed(this, sp);
3206  btSoftBody::Impulse impulse;
3207  impulse.m_asVelocity = 1;
3208  impulse.m_velocity = m_massmatrix * (m_drift + vc * m_cfm) * sor;
3209  m_bodies[0].applyAImpulse(-impulse);
3210  m_bodies[1].applyAImpulse(impulse);
3211 }
3212 
3213 //
3215 {
3216  if (m_split > 0)
3217  {
3218  m_bodies[0].applyDAImpulse(-m_sdrift);
3219  m_bodies[1].applyDAImpulse(m_sdrift);
3220  }
3221 }
3222 
3223 //
3224 void btSoftBody::CJoint::Prepare(btScalar dt, int iterations)
3225 {
3226  Joint::Prepare(dt, iterations);
3227  const bool dodrift = (m_life == 0);
3228  m_delete = (++m_life) > m_maxlife;
3229  if (dodrift)
3230  {
3231  m_drift = m_drift * m_erp / dt;
3232  if (m_split > 0)
3233  {
3234  m_sdrift = m_massmatrix * (m_drift * m_split);
3235  m_drift *= 1 - m_split;
3236  }
3237  m_drift /= (btScalar)iterations;
3238  }
3239  else
3240  {
3241  m_drift = m_sdrift = btVector3(0, 0, 0);
3242  }
3243 }
3244 
3245 //
3247 {
3248  const btVector3 va = m_bodies[0].velocity(m_rpos[0]);
3249  const btVector3 vb = m_bodies[1].velocity(m_rpos[1]);
3250  const btVector3 vrel = va - vb;
3251  const btScalar rvac = btDot(vrel, m_normal);
3252  btSoftBody::Impulse impulse;
3253  impulse.m_asVelocity = 1;
3254  impulse.m_velocity = m_drift;
3255  if (rvac < 0)
3256  {
3257  const btVector3 iv = m_normal * rvac;
3258  const btVector3 fv = vrel - iv;
3259  impulse.m_velocity += iv + fv * m_friction;
3260  }
3261  impulse.m_velocity = m_massmatrix * impulse.m_velocity * sor;
3262 
3263  if (m_bodies[0].m_soft == m_bodies[1].m_soft)
3264  {
3265  if ((impulse.m_velocity.getX() == impulse.m_velocity.getX()) && (impulse.m_velocity.getY() == impulse.m_velocity.getY()) &&
3266  (impulse.m_velocity.getZ() == impulse.m_velocity.getZ()))
3267  {
3268  if (impulse.m_asVelocity)
3269  {
3270  if (impulse.m_velocity.length() < m_bodies[0].m_soft->m_maxSelfCollisionImpulse)
3271  {
3272  }
3273  else
3274  {
3275  m_bodies[0].applyImpulse(-impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[0]);
3276  m_bodies[1].applyImpulse(impulse * m_bodies[0].m_soft->m_selfCollisionImpulseFactor, m_rpos[1]);
3277  }
3278  }
3279  }
3280  }
3281  else
3282  {
3283  m_bodies[0].applyImpulse(-impulse, m_rpos[0]);
3284  m_bodies[1].applyImpulse(impulse, m_rpos[1]);
3285  }
3286 }
3287 
3288 //
3290 {
3291  if (m_split > 0)
3292  {
3293  m_bodies[0].applyDImpulse(-m_sdrift, m_rpos[0]);
3294  m_bodies[1].applyDImpulse(m_sdrift, m_rpos[1]);
3295  }
3296 }
3297 
3298 //
3300 {
3301  BT_PROFILE("SoftBody applyForces");
3302  // const btScalar dt = m_sst.sdt;
3303  const btScalar kLF = m_cfg.kLF;
3304  const btScalar kDG = m_cfg.kDG;
3305  const btScalar kPR = m_cfg.kPR;
3306  const btScalar kVC = m_cfg.kVC;
3307  const bool as_lift = kLF > 0;
3308  const bool as_drag = kDG > 0;
3309  const bool as_pressure = kPR != 0;
3310  const bool as_volume = kVC > 0;
3311  const bool as_aero = as_lift ||
3312  as_drag;
3313  //const bool as_vaero = as_aero &&
3314  // (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided);
3315  //const bool as_faero = as_aero &&
3316  // (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided);
3317  const bool use_medium = as_aero;
3318  const bool use_volume = as_pressure ||
3319  as_volume;
3320  btScalar volume = 0;
3321  btScalar ivolumetp = 0;
3322  btScalar dvolumetv = 0;
3323  btSoftBody::sMedium medium;
3324  if (use_volume)
3325  {
3326  volume = getVolume();
3327  ivolumetp = 1 / btFabs(volume) * kPR;
3328  dvolumetv = (m_pose.m_volume - volume) * kVC;
3329  }
3330  /* Per vertex forces */
3331  int i, ni;
3332 
3333  for (i = 0, ni = m_nodes.size(); i < ni; ++i)
3334  {
3335  btSoftBody::Node& n = m_nodes[i];
3336  if (n.m_im > 0)
3337  {
3338  if (use_medium)
3339  {
3340  /* Aerodynamics */
3342  }
3343  /* Pressure */
3344  if (as_pressure)
3345  {
3346  n.m_f += n.m_n * (n.m_area * ivolumetp);
3347  }
3348  /* Volume */
3349  if (as_volume)
3350  {
3351  n.m_f += n.m_n * (n.m_area * dvolumetv);
3352  }
3353  }
3354  }
3355 
3356  /* Per face forces */
3357  for (i = 0, ni = m_faces.size(); i < ni; ++i)
3358  {
3359  // btSoftBody::Face& f=m_faces[i];
3360 
3361  /* Aerodynamics */
3363  }
3364 }
3365 
3366 //
3368 {
3369  m_cfg.m_maxStress = maxStress;
3370 }
3371 
3372 //
3374 {
3375  for (int i = 0; i < m_renderNodes.size(); ++i)
3376  {
3377  Node& n = m_renderNodes[i];
3378  n.m_x.setZero();
3379  for (int j = 0; j < 4; ++j)
3380  {
3381  if (m_renderNodesParents[i].size())
3382  {
3384  }
3385  }
3386  }
3387 }
3388 
3390 {
3391  for (int i = 0; i <= N; ++i)
3392  {
3393  for (int j = 0; i+j <= N; ++j)
3394  {
3396  }
3397  }
3398 }
3399 
3400 //
3402 {
3403  BT_PROFILE("PSolve_Anchors");
3404  const btScalar kAHR = psb->m_cfg.kAHR * kst;
3405  const btScalar dt = psb->m_sst.sdt;
3406  for (int i = 0, ni = psb->m_anchors.size(); i < ni; ++i)
3407  {
3408  const Anchor& a = psb->m_anchors[i];
3409  const btTransform& t = a.m_body->getWorldTransform();
3410  Node& n = *a.m_node;
3411  const btVector3 wa = t * a.m_local;
3412  const btVector3 va = a.m_body->getVelocityInLocalPoint(a.m_c1) * dt;
3413  const btVector3 vb = n.m_x - n.m_q;
3414  const btVector3 vr = (va - vb) + (wa - n.m_x) * kAHR;
3415  const btVector3 impulse = a.m_c0 * vr * a.m_influence;
3416  n.m_x += impulse * a.m_c2;
3417  a.m_body->applyImpulse(-impulse, a.m_c1);
3418  }
3419 }
3420 
3421 //
3423 {
3424  BT_PROFILE("PSolve_RContacts");
3425  const btScalar dt = psb->m_sst.sdt;
3426  const btScalar mrg = psb->getCollisionShape()->getMargin();
3427  btMultiBodyJacobianData jacobianData;
3428  for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
3429  {
3430  const RContact& c = psb->m_rcontacts[i];
3431  const sCti& cti = c.m_cti;
3432  if (cti.m_colObj->hasContactResponse())
3433  {
3434  btVector3 va(0, 0, 0);
3435  btRigidBody* rigidCol = 0;
3436  btMultiBodyLinkCollider* multibodyLinkCol = 0;
3437  btScalar* deltaV;
3438 
3440  {
3441  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
3442  va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0);
3443  }
3445  {
3447  if (multibodyLinkCol)
3448  {
3449  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
3450  jacobianData.m_jacobians.resize(ndof);
3451  jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
3452  btScalar* jac = &jacobianData.m_jacobians[0];
3453 
3454  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);
3455  deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
3456  multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
3457 
3458  btScalar vel = 0.0;
3459  for (int j = 0; j < ndof; ++j)
3460  {
3461  vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
3462  }
3463  va = cti.m_normal * vel * dt;
3464  }
3465  }
3466 
3467  const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
3468  const btVector3 vr = vb - va;
3469  const btScalar dn = btDot(vr, cti.m_normal);
3470  if (dn <= SIMD_EPSILON)
3471  {
3472  const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
3473  const btVector3 fv = vr - (cti.m_normal * dn);
3474  // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
3475  const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst);
3476  c.m_node->m_x -= impulse * c.m_c2;
3477 
3479  {
3480  if (rigidCol)
3481  rigidCol->applyImpulse(impulse, c.m_c1);
3482  }
3484  {
3485  if (multibodyLinkCol)
3486  {
3487  double multiplier = 0.5;
3488  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
3489  }
3490  }
3491  }
3492  }
3493  }
3494 }
3495 
3496 //
3498 {
3499  BT_PROFILE("PSolve_SContacts");
3500 
3501  for (int i = 0, ni = psb->m_scontacts.size(); i < ni; ++i)
3502  {
3503  const SContact& c = psb->m_scontacts[i];
3504  const btVector3& nr = c.m_normal;
3505  Node& n = *c.m_node;
3506  Face& f = *c.m_face;
3507  const btVector3 p = BaryEval(f.m_n[0]->m_x,
3508  f.m_n[1]->m_x,
3509  f.m_n[2]->m_x,
3510  c.m_weights);
3511  const btVector3 q = BaryEval(f.m_n[0]->m_q,
3512  f.m_n[1]->m_q,
3513  f.m_n[2]->m_q,
3514  c.m_weights);
3515  const btVector3 vr = (n.m_x - n.m_q) - (p - q);
3516  btVector3 corr(0, 0, 0);
3517  btScalar dot = btDot(vr, nr);
3518  if (dot < 0)
3519  {
3520  const btScalar j = c.m_margin - (btDot(nr, n.m_x) - btDot(nr, p));
3521  corr += c.m_normal * j;
3522  }
3523  corr -= ProjectOnPlane(vr, nr) * c.m_friction;
3524  n.m_x += corr * c.m_cfm[0];
3525  f.m_n[0]->m_x -= corr * (c.m_cfm[1] * c.m_weights.x());
3526  f.m_n[1]->m_x -= corr * (c.m_cfm[1] * c.m_weights.y());
3527  f.m_n[2]->m_x -= corr * (c.m_cfm[1] * c.m_weights.z());
3528  }
3529 }
3530 
3531 //
3533 {
3534  BT_PROFILE("PSolve_Links");
3535  for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
3536  {
3537  Link& l = psb->m_links[i];
3538  if (l.m_c0 > 0)
3539  {
3540  Node& a = *l.m_n[0];
3541  Node& b = *l.m_n[1];
3542  const btVector3 del = b.m_x - a.m_x;
3543  const btScalar len = del.length2();
3544  if (l.m_c1 + len > SIMD_EPSILON)
3545  {
3546  const btScalar k = ((l.m_c1 - len) / (l.m_c0 * (l.m_c1 + len))) * kst;
3547  a.m_x -= del * (k * a.m_im);
3548  b.m_x += del * (k * b.m_im);
3549  }
3550  }
3551  }
3552 }
3553 
3554 //
3556 {
3557  BT_PROFILE("VSolve_Links");
3558  for (int i = 0, ni = psb->m_links.size(); i < ni; ++i)
3559  {
3560  Link& l = psb->m_links[i];
3561  Node** n = l.m_n;
3562  const btScalar j = -btDot(l.m_c3, n[0]->m_v - n[1]->m_v) * l.m_c2 * kst;
3563  n[0]->m_v += l.m_c3 * (j * n[0]->m_im);
3564  n[1]->m_v -= l.m_c3 * (j * n[1]->m_im);
3565  }
3566 }
3567 
3568 //
3570 {
3571  switch (solver)
3572  {
3573  case ePSolver::Anchors:
3574  return (&btSoftBody::PSolve_Anchors);
3575  case ePSolver::Linear:
3576  return (&btSoftBody::PSolve_Links);
3577  case ePSolver::RContacts:
3578  return (&btSoftBody::PSolve_RContacts);
3579  case ePSolver::SContacts:
3580  return (&btSoftBody::PSolve_SContacts);
3581  default:
3582  {
3583  }
3584  }
3585  return (0);
3586 }
3587 
3588 //
3590 {
3591  switch (solver)
3592  {
3593  case eVSolver::Linear:
3594  return (&btSoftBody::VSolve_Links);
3595  default:
3596  {
3597  }
3598  }
3599  return (0);
3600 }
3601 
3603 {
3605 }
3606 
3608 {
3609  return m_useSelfCollision;
3610 }
3611 
3612 //
3614 {
3616  {
3617  case fCollision::SDF_RS:
3618  {
3621  btTransform wtr = pcoWrap->getWorldTransform();
3622 
3623  const btTransform ctr = pcoWrap->getWorldTransform();
3624  const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
3625  const btScalar basemargin = getCollisionShape()->getMargin();
3626  btVector3 mins;
3627  btVector3 maxs;
3629  volume;
3630  pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
3631  mins,
3632  maxs);
3633  volume = btDbvtVolume::FromMM(mins, maxs);
3634  volume.Expand(btVector3(basemargin, basemargin, basemargin));
3635  docollide.psb = this;
3636  docollide.m_colObj1Wrap = pcoWrap;
3637  docollide.m_rigidBody = prb1;
3638 
3639  docollide.dynmargin = basemargin + timemargin;
3640  docollide.stamargin = basemargin;
3641  m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
3642  }
3643  break;
3644  case fCollision::CL_RS:
3645  {
3647  collider.ProcessColObj(this, pcoWrap);
3648  }
3649  break;
3650  case fCollision::SDF_RD:
3651  {
3652 
3654  if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
3655  {
3656  const btTransform wtr = pcoWrap->getWorldTransform();
3657 // const btTransform ctr = pcoWrap->getWorldTransform();
3658 // const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
3659  const btScalar timemargin = 0;
3660  const btScalar basemargin = getCollisionShape()->getMargin();
3661  btVector3 mins;
3662  btVector3 maxs;
3664  volume;
3665  pcoWrap->getCollisionShape()->getAabb(wtr,
3666  mins,
3667  maxs);
3668  volume = btDbvtVolume::FromMM(mins, maxs);
3669  volume.Expand(btVector3(basemargin, basemargin, basemargin));
3670  btSoftColliders::CollideSDF_RD docollideNode;
3671  docollideNode.psb = this;
3672  docollideNode.m_colObj1Wrap = pcoWrap;
3673  docollideNode.m_rigidBody = prb1;
3674  docollideNode.dynmargin = basemargin + timemargin;
3675  docollideNode.stamargin = basemargin;
3676  m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
3677 
3678  if (this->m_useFaceContact)
3679  {
3680  btSoftColliders::CollideSDF_RDF docollideFace;
3681  docollideFace.psb = this;
3682  docollideFace.m_colObj1Wrap = pcoWrap;
3683  docollideFace.m_rigidBody = prb1;
3684  docollideFace.dynmargin = basemargin + timemargin;
3685  docollideFace.stamargin = basemargin;
3686  m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
3687  }
3688  }
3689  }
3690  break;
3691  }
3692 }
3693 
3694 static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
3695 {
3696  if (n == 0)
3697  return 0;
3698  btDbvntNode* root = new btDbvntNode(n);
3699  if (n->isinternal())
3700  {
3701  btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
3702  root->childs[0] = c0;
3703  btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
3704  root->childs[1] = c1;
3705  }
3706  return root;
3707 }
3708 
3709 static inline void calculateNormalCone(btDbvntNode* root)
3710 {
3711  if (!root)
3712  return;
3713  if (root->isleaf())
3714  {
3715  const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
3716  root->normal = face->m_normal;
3717  root->angle = 0;
3718  }
3719  else
3720  {
3721  btVector3 n0(0,0,0), n1(0,0,0);
3722  btScalar a0 = 0, a1 = 0;
3723  if (root->childs[0])
3724  {
3725  calculateNormalCone(root->childs[0]);
3726  n0 = root->childs[0]->normal;
3727  a0 = root->childs[0]->angle;
3728  }
3729  if (root->childs[1])
3730  {
3731  calculateNormalCone(root->childs[1]);
3732  n1 = root->childs[1]->normal;
3733  a1 = root->childs[1]->angle;
3734  }
3735  root->normal = (n0+n1).safeNormalize();
3736  root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
3737  }
3738 }
3739 //
3741 {
3742  BT_PROFILE("Deformable Collision");
3743  const int cf = m_cfg.collisions & psb->m_cfg.collisions;
3744  switch (cf & fCollision::SVSmask)
3745  {
3746  case fCollision::CL_SS:
3747  {
3748  //support self-collision if CL_SELF flag set
3749  if (this != psb || psb->m_cfg.collisions & fCollision::CL_SELF)
3750  {
3752  docollide.ProcessSoftSoft(this, psb);
3753  }
3754  }
3755  break;
3756  case fCollision::VF_SS:
3757  {
3758  //only self-collision for Cluster, not Vertex-Face yet
3759  if (this != psb)
3760  {
3762  /* common */
3763  docollide.mrg = getCollisionShape()->getMargin() +
3764  psb->getCollisionShape()->getMargin();
3765  /* psb0 nodes vs psb1 faces */
3766  docollide.psb[0] = this;
3767  docollide.psb[1] = psb;
3768  docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3769  docollide.psb[1]->m_fdbvt.m_root,
3770  docollide);
3771  /* psb1 nodes vs psb0 faces */
3772  docollide.psb[0] = psb;
3773  docollide.psb[1] = this;
3774  docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3775  docollide.psb[1]->m_fdbvt.m_root,
3776  docollide);
3777  }
3778  }
3779  break;
3780  case fCollision::VF_DD:
3781  {
3782  if (psb->isActive() || this->isActive())
3783  {
3784  if (this != psb)
3785  {
3787  /* common */
3788  docollide.mrg = getCollisionShape()->getMargin() +
3789  psb->getCollisionShape()->getMargin();
3790  /* psb0 nodes vs psb1 faces */
3791  if (psb->m_tetras.size() > 0)
3792  docollide.useFaceNormal = true;
3793  else
3794  docollide.useFaceNormal = false;
3795  docollide.psb[0] = this;
3796  docollide.psb[1] = psb;
3797  docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3798  docollide.psb[1]->m_fdbvt.m_root,
3799  docollide);
3800  /* psb1 nodes vs psb0 faces */
3801  if (this->m_tetras.size() > 0)
3802  docollide.useFaceNormal = true;
3803  else
3804  docollide.useFaceNormal = false;
3805  docollide.psb[0] = psb;
3806  docollide.psb[1] = this;
3807  docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
3808  docollide.psb[1]->m_fdbvt.m_root,
3809  docollide);
3810  }
3811  else
3812  {
3813  if (psb->useSelfCollision())
3814  {
3816  docollide.mrg = getCollisionShape()->getMargin() +
3817  psb->getCollisionShape()->getMargin();
3818  docollide.psb[0] = this;
3819  docollide.psb[1] = psb;
3820  if (this->m_tetras.size() > 0)
3821  docollide.useFaceNormal = true;
3822  else
3823  docollide.useFaceNormal = false;
3824  /* psb0 faces vs psb0 faces */
3825  btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
3826  calculateNormalCone(root);
3827  this->m_fdbvt.selfCollideT(root,docollide);
3828  delete root;
3829  }
3830  }
3831  }
3832  }
3833  break;
3834  default:
3835  {
3836  }
3837  }
3838 }
3839 
3841 {
3842  m_windVelocity = velocity;
3843 }
3844 
3846 {
3847  return m_windVelocity;
3848 }
3849 
3851 {
3852  int sz = sizeof(btSoftBodyData);
3853  return sz;
3854 }
3855 
3857 const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializer) const
3858 {
3859  btSoftBodyData* sbd = (btSoftBodyData*)dataBuffer;
3860 
3861  btCollisionObject::serialize(&sbd->m_collisionObjectData, serializer);
3862 
3863  btHashMap<btHashPtr, int> m_nodeIndexMap;
3864 
3865  sbd->m_numMaterials = m_materials.size();
3866  sbd->m_materials = sbd->m_numMaterials ? (SoftBodyMaterialData**)serializer->getUniquePointer((void*)&m_materials) : 0;
3867 
3868  if (sbd->m_materials)
3869  {
3870  int sz = sizeof(SoftBodyMaterialData*);
3871  int numElem = sbd->m_numMaterials;
3872  btChunk* chunk = serializer->allocate(sz, numElem);
3873  //SoftBodyMaterialData** memPtr = chunk->m_oldPtr;
3874  SoftBodyMaterialData** memPtr = (SoftBodyMaterialData**)chunk->m_oldPtr;
3875  for (int i = 0; i < numElem; i++, memPtr++)
3876  {
3878  *memPtr = mat ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)mat) : 0;
3879  if (!serializer->findPointer(mat))
3880  {
3881  //serialize it here
3882  btChunk* chunk = serializer->allocate(sizeof(SoftBodyMaterialData), 1);
3884  memPtr->m_flags = mat->m_flags;
3885  memPtr->m_angularStiffness = mat->m_kAST;
3886  memPtr->m_linearStiffness = mat->m_kLST;
3887  memPtr->m_volumeStiffness = mat->m_kVST;
3888  serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_SBMATERIAL_CODE, mat);
3889  }
3890  }
3891  serializer->finalizeChunk(chunk, "SoftBodyMaterialData", BT_ARRAY_CODE, (void*)&m_materials);
3892  }
3893 
3894  sbd->m_numNodes = m_nodes.size();
3895  sbd->m_nodes = sbd->m_numNodes ? (SoftBodyNodeData*)serializer->getUniquePointer((void*)&m_nodes) : 0;
3896  if (sbd->m_nodes)
3897  {
3898  int sz = sizeof(SoftBodyNodeData);
3899  int numElem = sbd->m_numNodes;
3900  btChunk* chunk = serializer->allocate(sz, numElem);
3901  SoftBodyNodeData* memPtr = (SoftBodyNodeData*)chunk->m_oldPtr;
3902  for (int i = 0; i < numElem; i++, memPtr++)
3903  {
3904  m_nodes[i].m_f.serializeFloat(memPtr->m_accumulatedForce);
3905  memPtr->m_area = m_nodes[i].m_area;
3906  memPtr->m_attach = m_nodes[i].m_battach;
3907  memPtr->m_inverseMass = m_nodes[i].m_im;
3908  memPtr->m_material = m_nodes[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_nodes[i].m_material) : 0;
3909  m_nodes[i].m_n.serializeFloat(memPtr->m_normal);
3910  m_nodes[i].m_x.serializeFloat(memPtr->m_position);
3911  m_nodes[i].m_q.serializeFloat(memPtr->m_previousPosition);
3912  m_nodes[i].m_v.serializeFloat(memPtr->m_velocity);
3913  m_nodeIndexMap.insert(&m_nodes[i], i);
3914  }
3915  serializer->finalizeChunk(chunk, "SoftBodyNodeData", BT_SBNODE_CODE, (void*)&m_nodes);
3916  }
3917 
3918  sbd->m_numLinks = m_links.size();
3919  sbd->m_links = sbd->m_numLinks ? (SoftBodyLinkData*)serializer->getUniquePointer((void*)&m_links[0]) : 0;
3920  if (sbd->m_links)
3921  {
3922  int sz = sizeof(SoftBodyLinkData);
3923  int numElem = sbd->m_numLinks;
3924  btChunk* chunk = serializer->allocate(sz, numElem);
3925  SoftBodyLinkData* memPtr = (SoftBodyLinkData*)chunk->m_oldPtr;
3926  for (int i = 0; i < numElem; i++, memPtr++)
3927  {
3928  memPtr->m_bbending = m_links[i].m_bbending;
3929  memPtr->m_material = m_links[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_links[i].m_material) : 0;
3930  memPtr->m_nodeIndices[0] = m_links[i].m_n[0] ? m_links[i].m_n[0] - &m_nodes[0] : -1;
3931  memPtr->m_nodeIndices[1] = m_links[i].m_n[1] ? m_links[i].m_n[1] - &m_nodes[0] : -1;
3932  btAssert(memPtr->m_nodeIndices[0] < m_nodes.size());
3933  btAssert(memPtr->m_nodeIndices[1] < m_nodes.size());
3934  memPtr->m_restLength = m_links[i].m_rl;
3935  }
3936  serializer->finalizeChunk(chunk, "SoftBodyLinkData", BT_ARRAY_CODE, (void*)&m_links[0]);
3937  }
3938 
3939  sbd->m_numFaces = m_faces.size();
3940  sbd->m_faces = sbd->m_numFaces ? (SoftBodyFaceData*)serializer->getUniquePointer((void*)&m_faces[0]) : 0;
3941  if (sbd->m_faces)
3942  {
3943  int sz = sizeof(SoftBodyFaceData);
3944  int numElem = sbd->m_numFaces;
3945  btChunk* chunk = serializer->allocate(sz, numElem);
3946  SoftBodyFaceData* memPtr = (SoftBodyFaceData*)chunk->m_oldPtr;
3947  for (int i = 0; i < numElem; i++, memPtr++)
3948  {
3949  memPtr->m_material = m_faces[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_faces[i].m_material) : 0;
3950  m_faces[i].m_normal.serializeFloat(memPtr->m_normal);
3951  for (int j = 0; j < 3; j++)
3952  {
3953  memPtr->m_nodeIndices[j] = m_faces[i].m_n[j] ? m_faces[i].m_n[j] - &m_nodes[0] : -1;
3954  }
3955  memPtr->m_restArea = m_faces[i].m_ra;
3956  }
3957  serializer->finalizeChunk(chunk, "SoftBodyFaceData", BT_ARRAY_CODE, (void*)&m_faces[0]);
3958  }
3959 
3960  sbd->m_numTetrahedra = m_tetras.size();
3961  sbd->m_tetrahedra = sbd->m_numTetrahedra ? (SoftBodyTetraData*)serializer->getUniquePointer((void*)&m_tetras[0]) : 0;
3962  if (sbd->m_tetrahedra)
3963  {
3964  int sz = sizeof(SoftBodyTetraData);
3965  int numElem = sbd->m_numTetrahedra;
3966  btChunk* chunk = serializer->allocate(sz, numElem);
3967  SoftBodyTetraData* memPtr = (SoftBodyTetraData*)chunk->m_oldPtr;
3968  for (int i = 0; i < numElem; i++, memPtr++)
3969  {
3970  for (int j = 0; j < 4; j++)
3971  {
3972  m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
3973  memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
3974  }
3975  memPtr->m_c1 = m_tetras[i].m_c1;
3976  memPtr->m_c2 = m_tetras[i].m_c2;
3977  memPtr->m_material = m_tetras[i].m_material ? (SoftBodyMaterialData*)serializer->getUniquePointer((void*)m_tetras[i].m_material) : 0;
3978  memPtr->m_restVolume = m_tetras[i].m_rv;
3979  }
3980  serializer->finalizeChunk(chunk, "SoftBodyTetraData", BT_ARRAY_CODE, (void*)&m_tetras[0]);
3981  }
3982 
3983  sbd->m_numAnchors = m_anchors.size();
3984  sbd->m_anchors = sbd->m_numAnchors ? (SoftRigidAnchorData*)serializer->getUniquePointer((void*)&m_anchors[0]) : 0;
3985  if (sbd->m_anchors)
3986  {
3987  int sz = sizeof(SoftRigidAnchorData);
3988  int numElem = sbd->m_numAnchors;
3989  btChunk* chunk = serializer->allocate(sz, numElem);
3991  for (int i = 0; i < numElem; i++, memPtr++)
3992  {
3993  m_anchors[i].m_c0.serializeFloat(memPtr->m_c0);
3994  m_anchors[i].m_c1.serializeFloat(memPtr->m_c1);
3995  memPtr->m_c2 = m_anchors[i].m_c2;
3996  m_anchors[i].m_local.serializeFloat(memPtr->m_localFrame);
3997  memPtr->m_nodeIndex = m_anchors[i].m_node ? m_anchors[i].m_node - &m_nodes[0] : -1;
3998 
3999  memPtr->m_rigidBody = m_anchors[i].m_body ? (btRigidBodyData*)serializer->getUniquePointer((void*)m_anchors[i].m_body) : 0;
4000  btAssert(memPtr->m_nodeIndex < m_nodes.size());
4001  }
4002  serializer->finalizeChunk(chunk, "SoftRigidAnchorData", BT_ARRAY_CODE, (void*)&m_anchors[0]);
4003  }
4004 
4005  sbd->m_config.m_dynamicFriction = m_cfg.kDF;
4006  sbd->m_config.m_baumgarte = m_cfg.kVCF;
4007  sbd->m_config.m_pressure = m_cfg.kPR;
4008  sbd->m_config.m_aeroModel = this->m_cfg.aeromodel;
4009  sbd->m_config.m_lift = m_cfg.kLF;
4010  sbd->m_config.m_drag = m_cfg.kDG;
4011  sbd->m_config.m_positionIterations = m_cfg.piterations;
4012  sbd->m_config.m_driftIterations = m_cfg.diterations;
4013  sbd->m_config.m_clusterIterations = m_cfg.citerations;
4014  sbd->m_config.m_velocityIterations = m_cfg.viterations;
4015  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4016  sbd->m_config.m_damping = m_cfg.kDP;
4017  sbd->m_config.m_poseMatch = m_cfg.kMT;
4018  sbd->m_config.m_collisionFlags = m_cfg.collisions;
4019  sbd->m_config.m_volume = m_cfg.kVC;
4020  sbd->m_config.m_rigidContactHardness = m_cfg.kCHR;
4021  sbd->m_config.m_kineticContactHardness = m_cfg.kKHR;
4022  sbd->m_config.m_softContactHardness = m_cfg.kSHR;
4023  sbd->m_config.m_anchorHardness = m_cfg.kAHR;
4024  sbd->m_config.m_timeScale = m_cfg.timescale;
4025  sbd->m_config.m_maxVolume = m_cfg.maxvolume;
4026  sbd->m_config.m_softRigidClusterHardness = m_cfg.kSRHR_CL;
4027  sbd->m_config.m_softKineticClusterHardness = m_cfg.kSKHR_CL;
4028  sbd->m_config.m_softSoftClusterHardness = m_cfg.kSSHR_CL;
4029  sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL;
4030  sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL;
4031  sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL;
4032 
4033  //pose for shape matching
4034  {
4035  sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose);
4036 
4037  int sz = sizeof(SoftBodyPoseData);
4038  btChunk* chunk = serializer->allocate(sz, 1);
4039  SoftBodyPoseData* memPtr = (SoftBodyPoseData*)chunk->m_oldPtr;
4040 
4041  m_pose.m_aqq.serializeFloat(memPtr->m_aqq);
4042  memPtr->m_bframe = m_pose.m_bframe;
4043  memPtr->m_bvolume = m_pose.m_bvolume;
4044  m_pose.m_com.serializeFloat(memPtr->m_com);
4045 
4046  memPtr->m_numPositions = m_pose.m_pos.size();
4047  memPtr->m_positions = memPtr->m_numPositions ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_pose.m_pos[0]) : 0;
4048  if (memPtr->m_numPositions)
4049  {
4050  int numElem = memPtr->m_numPositions;
4051  int sz = sizeof(btVector3Data);
4052  btChunk* chunk = serializer->allocate(sz, numElem);
4053  btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4054  for (int i = 0; i < numElem; i++, memPtr++)
4055  {
4056  m_pose.m_pos[i].serializeFloat(*memPtr);
4057  }
4058  serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_pose.m_pos[0]);
4059  }
4060  memPtr->m_restVolume = m_pose.m_volume;
4061  m_pose.m_rot.serializeFloat(memPtr->m_rot);
4063 
4064  memPtr->m_numWeigts = m_pose.m_wgh.size();
4065  memPtr->m_weights = memPtr->m_numWeigts ? (float*)serializer->getUniquePointer((void*)&m_pose.m_wgh[0]) : 0;
4066  if (memPtr->m_numWeigts)
4067  {
4068  int numElem = memPtr->m_numWeigts;
4069  int sz = sizeof(float);
4070  btChunk* chunk = serializer->allocate(sz, numElem);
4071  float* memPtr = (float*)chunk->m_oldPtr;
4072  for (int i = 0; i < numElem; i++, memPtr++)
4073  {
4074  *memPtr = m_pose.m_wgh[i];
4075  }
4076  serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_pose.m_wgh[0]);
4077  }
4078 
4079  serializer->finalizeChunk(chunk, "SoftBodyPoseData", BT_ARRAY_CODE, (void*)&m_pose);
4080  }
4081 
4082  //clusters for convex-cluster collision detection
4083 
4084  sbd->m_numClusters = m_clusters.size();
4085  sbd->m_clusters = sbd->m_numClusters ? (SoftBodyClusterData*)serializer->getUniquePointer((void*)m_clusters[0]) : 0;
4086  if (sbd->m_numClusters)
4087  {
4088  int numElem = sbd->m_numClusters;
4089  int sz = sizeof(SoftBodyClusterData);
4090  btChunk* chunk = serializer->allocate(sz, numElem);
4092  for (int i = 0; i < numElem; i++, memPtr++)
4093  {
4094  memPtr->m_adamping = m_clusters[i]->m_adamping;
4095  m_clusters[i]->m_av.serializeFloat(memPtr->m_av);
4096  memPtr->m_clusterIndex = m_clusters[i]->m_clusterIndex;
4097  memPtr->m_collide = m_clusters[i]->m_collide;
4098  m_clusters[i]->m_com.serializeFloat(memPtr->m_com);
4099  memPtr->m_containsAnchor = m_clusters[i]->m_containsAnchor;
4100  m_clusters[i]->m_dimpulses[0].serializeFloat(memPtr->m_dimpulses[0]);
4101  m_clusters[i]->m_dimpulses[1].serializeFloat(memPtr->m_dimpulses[1]);
4102  m_clusters[i]->m_framexform.serializeFloat(memPtr->m_framexform);
4103  memPtr->m_idmass = m_clusters[i]->m_idmass;
4104  memPtr->m_imass = m_clusters[i]->m_imass;
4105  m_clusters[i]->m_invwi.serializeFloat(memPtr->m_invwi);
4106  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4107  m_clusters[i]->m_locii.serializeFloat(memPtr->m_locii);
4108  m_clusters[i]->m_lv.serializeFloat(memPtr->m_lv);
4109  memPtr->m_matching = m_clusters[i]->m_matching;
4110  memPtr->m_maxSelfCollisionImpulse = m_clusters[i]->m_maxSelfCollisionImpulse;
4111  memPtr->m_ndamping = m_clusters[i]->m_ndamping;
4112  memPtr->m_ldamping = m_clusters[i]->m_ldamping;
4113  memPtr->m_adamping = m_clusters[i]->m_adamping;
4114  memPtr->m_selfCollisionImpulseFactor = m_clusters[i]->m_selfCollisionImpulseFactor;
4115 
4116  memPtr->m_numFrameRefs = m_clusters[i]->m_framerefs.size();
4117  memPtr->m_numMasses = m_clusters[i]->m_masses.size();
4118  memPtr->m_numNodes = m_clusters[i]->m_nodes.size();
4119 
4120  memPtr->m_nvimpulses = m_clusters[i]->m_nvimpulses;
4121  m_clusters[i]->m_vimpulses[0].serializeFloat(memPtr->m_vimpulses[0]);
4122  m_clusters[i]->m_vimpulses[1].serializeFloat(memPtr->m_vimpulses[1]);
4123  memPtr->m_ndimpulses = m_clusters[i]->m_ndimpulses;
4124 
4125  memPtr->m_framerefs = memPtr->m_numFrameRefs ? (btVector3FloatData*)serializer->getUniquePointer((void*)&m_clusters[i]->m_framerefs[0]) : 0;
4126  if (memPtr->m_framerefs)
4127  {
4128  int numElem = memPtr->m_numFrameRefs;
4129  int sz = sizeof(btVector3FloatData);
4130  btChunk* chunk = serializer->allocate(sz, numElem);
4131  btVector3FloatData* memPtr = (btVector3FloatData*)chunk->m_oldPtr;
4132  for (int j = 0; j < numElem; j++, memPtr++)
4133  {
4134  m_clusters[i]->m_framerefs[j].serializeFloat(*memPtr);
4135  }
4136  serializer->finalizeChunk(chunk, "btVector3FloatData", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_framerefs[0]);
4137  }
4138 
4139  memPtr->m_masses = memPtr->m_numMasses ? (float*)serializer->getUniquePointer((void*)&m_clusters[i]->m_masses[0]) : 0;
4140  if (memPtr->m_masses)
4141  {
4142  int numElem = memPtr->m_numMasses;
4143  int sz = sizeof(float);
4144  btChunk* chunk = serializer->allocate(sz, numElem);
4145  float* memPtr = (float*)chunk->m_oldPtr;
4146  for (int j = 0; j < numElem; j++, memPtr++)
4147  {
4148  *memPtr = m_clusters[i]->m_masses[j];
4149  }
4150  serializer->finalizeChunk(chunk, "float", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_masses[0]);
4151  }
4152 
4153  memPtr->m_nodeIndices = memPtr->m_numNodes ? (int*)serializer->getUniquePointer((void*)&m_clusters[i]->m_nodes) : 0;
4154  if (memPtr->m_nodeIndices)
4155  {
4156  int numElem = memPtr->m_numMasses;
4157  int sz = sizeof(int);
4158  btChunk* chunk = serializer->allocate(sz, numElem);
4159  int* memPtr = (int*)chunk->m_oldPtr;
4160  for (int j = 0; j < numElem; j++, memPtr++)
4161  {
4162  int* indexPtr = m_nodeIndexMap.find(m_clusters[i]->m_nodes[j]);
4163  btAssert(indexPtr);
4164  *memPtr = *indexPtr;
4165  }
4166  serializer->finalizeChunk(chunk, "int", BT_ARRAY_CODE, (void*)&m_clusters[i]->m_nodes);
4167  }
4168  }
4169  serializer->finalizeChunk(chunk, "SoftBodyClusterData", BT_ARRAY_CODE, (void*)m_clusters[0]);
4170  }
4171 
4172  sbd->m_numJoints = m_joints.size();
4173  sbd->m_joints = m_joints.size() ? (btSoftBodyJointData*)serializer->getUniquePointer((void*)&m_joints[0]) : 0;
4174 
4175  if (sbd->m_joints)
4176  {
4177  int sz = sizeof(btSoftBodyJointData);
4178  int numElem = m_joints.size();
4179  btChunk* chunk = serializer->allocate(sz, numElem);
4181 
4182  for (int i = 0; i < numElem; i++, memPtr++)
4183  {
4184  memPtr->m_jointType = (int)m_joints[i]->Type();
4185  m_joints[i]->m_refs[0].serializeFloat(memPtr->m_refs[0]);
4186  m_joints[i]->m_refs[1].serializeFloat(memPtr->m_refs[1]);
4187  memPtr->m_cfm = m_joints[i]->m_cfm;
4188  memPtr->m_erp = float(m_joints[i]->m_erp);
4189  memPtr->m_split = float(m_joints[i]->m_split);
4190  memPtr->m_delete = m_joints[i]->m_delete;
4191 
4192  for (int j = 0; j < 4; j++)
4193  {
4194  memPtr->m_relPosition[0].m_floats[j] = 0.f;
4195  memPtr->m_relPosition[1].m_floats[j] = 0.f;
4196  }
4197  memPtr->m_bodyA = 0;
4198  memPtr->m_bodyB = 0;
4199  if (m_joints[i]->m_bodies[0].m_soft)
4200  {
4202  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_soft);
4203  }
4204  if (m_joints[i]->m_bodies[0].m_collisionObject)
4205  {
4207  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_collisionObject);
4208  }
4209  if (m_joints[i]->m_bodies[0].m_rigid)
4210  {
4211  memPtr->m_bodyAtype = BT_JOINT_RIGID_BODY;
4212  memPtr->m_bodyA = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[0].m_rigid);
4213  }
4214 
4215  if (m_joints[i]->m_bodies[1].m_soft)
4216  {
4218  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_soft);
4219  }
4220  if (m_joints[i]->m_bodies[1].m_collisionObject)
4221  {
4223  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_collisionObject);
4224  }
4225  if (m_joints[i]->m_bodies[1].m_rigid)
4226  {
4227  memPtr->m_bodyBtype = BT_JOINT_RIGID_BODY;
4228  memPtr->m_bodyB = serializer->getUniquePointer((void*)m_joints[i]->m_bodies[1].m_rigid);
4229  }
4230  }
4231  serializer->finalizeChunk(chunk, "btSoftBodyJointData", BT_ARRAY_CODE, (void*)&m_joints[0]);
4232  }
4233 
4234  return btSoftBodyDataName;
4235 }
4236 
4238 {
4240  return;
4241 
4243  {
4244  m_deactivationTime += timeStep;
4245  }
4246  else
4247  {
4249  setActivationState(0);
4250  }
4251 }
4252 
4253 
4255 {
4256  for (int i = 0; i < m_nodes.size(); ++i)
4257  {
4258  m_nodes[i].m_v.setZero();
4259  }
4260 }
4261 
4263 {
4265  return false;
4266 
4267  //disable deactivation
4269  return false;
4270 
4272  return true;
4273 
4275  {
4276  return true;
4277  }
4278  return false;
4279 }
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define DISABLE_DEACTIVATION
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
static btDbvtVolume bounds(btDbvtNode **leaves, int count)
Definition: btDbvt.cpp:299
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btScalar btAngle(const btQuaternion &q1, const btQuaternion &q2)
Return the angle between two quaternions.
Definition: btQuaternion.h:902
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
btScalar gDeactivationTime
Definition: btRigidBody.cpp:25
#define btRigidBodyData
Definition: btRigidBody.h:35
#define SIMD_PI
Definition: btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define SIMD_INFINITY
Definition: btScalar.h:544
#define SIMD_EPSILON
Definition: btScalar.h:543
btScalar btAcos(btScalar x)
Definition: btScalar.h:501
void btSwap(T &a, T &b)
Definition: btScalar.h:643
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
#define btAssert(x)
Definition: btScalar.h:153
#define BT_SBMATERIAL_CODE
Definition: btSerializer.h:119
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
#define BT_SBNODE_CODE
Definition: btSerializer.h:120
@ BT_JOINT_RIGID_BODY
@ BT_JOINT_SOFT_BODY_CLUSTER
@ BT_JOINT_COLLISION_OBJECT
static T sum(const btAlignedObjectArray< T > &items)
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
static btMatrix3x3 Mul(const btMatrix3x3 &a, btScalar b)
static btScalar ImplicitSolve(btSoftBody::ImplicitFn *fn, const btVector3 &a, const btVector3 &b, const btScalar accuracy, const int maxiterations=256)
static btDbvtVolume VolumeOf(const btSoftBody::Face &f, btScalar margin)
static T Lerp(const T &a, const T &b, btScalar t)
static btMatrix3x3 AngularImpulseMatrix(const btMatrix3x3 &iia, const btMatrix3x3 &iib)
static btScalar AreaOf(const btVector3 &x0, const btVector3 &x1, const btVector3 &x2)
static btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static btVector3 NormalizeAny(const btVector3 &v)
static int PolarDecompose(const btMatrix3x3 &m, btMatrix3x3 &q, btMatrix3x3 &s)
static int MatchEdge(const btSoftBody::Node *a, const btSoftBody::Node *b, const btSoftBody::Node *ma, const btSoftBody::Node *mb)
static void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
static T BaryEval(const T &a, const T &b, const T &c, const btVector3 &coord)
static bool SameSign(const T &x, const T &y)
static void EvaluateMedium(const btSoftBodyWorldInfo *wfi, const btVector3 &x, btSoftBody::sMedium &medium)
static void ZeroInitialize(T &value)
static btVector3 ProjectOnPlane(const btVector3 &v, const btVector3 &a)
static btScalar ClusterMetric(const btVector3 &x, const btVector3 &y)
static btMatrix3x3 ImpulseMatrix(btScalar dt, btScalar ima, btScalar imb, const btMatrix3x3 &iwi, const btVector3 &r)
static void ApplyClampedForce(btSoftBody::Node &n, const btVector3 &f, btScalar dt)
static btVector3 Clamp(const btVector3 &v, btScalar maxlength)
static btMatrix3x3 Diagonal(btScalar x)
static void calculateNormalCone(btDbvntNode *root)
#define IDX(_x_, _y_)
#define IDX2PTR(_p_, _b_)
#define PTR2IDX(_p_, _b_)
static void getBarycentric(const btVector3 &p, btVector3 &a, btVector3 &b, btVector3 &c, btVector3 &bary)
static btDbvntNode * copyToDbvnt(const btDbvtNode *n)
#define NEXTRAND
#define btSoftBodyDataName
Definition: btSoftBody.h:37
#define btSoftBodyData
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.h:36
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
#define btVector3Data
Definition: btVector3.h:27
int size() const
return the number of elements in the array
int findLinearSearch(const T &key) const
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void remove(const T &key)
void push_back(const T &_Val)
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)=0
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionObject can be used to manage collision detection objects.
const btTransform & getInterpolationWorldTransform() const
bool isStaticOrKinematicObject() const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btTransform & getWorldTransform()
btBroadphaseProxy * getBroadphaseHandle()
btTransform m_worldTransform
btCollisionShape * m_collisionShape
int getInternalType() const
reserved for Bullet internal usage
bool hasContactResponse() const
void activate(bool forceActivation=false) const
void setActivationState(int newState) const
int m_internalType
m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody,...
btScalar getFriction() const
int getActivationState() const
const btCollisionShape * getCollisionShape() const
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
virtual void setMargin(btScalar margin)=0
virtual btScalar getMargin() const =0
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
The btHashMap template class implements a generic and lightweight hashmap.
Definition: btHashMap.h:220
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
const Value * find(const Key &key) const
Definition: btHashMap.h:424
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 adjoint() const
Return the adjoint of the matrix.
Definition: btMatrix3x3.h:1069
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1077
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
btScalar determinant() const
Return the determinant of the matrix.
Definition: btMatrix3x3.h:1006
void serializeFloat(struct btMatrix3x3FloatData &dataOut) const
Definition: btMatrix3x3.h:1391
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:456
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
int getNumDofs() const
Definition: btMultiBody.h:167
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:417
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btScalar getInvMass() const
Definition: btRigidBody.h:263
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:326
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btCollisionShape * getCollisionShape() const
Definition: btRigidBody.h:242
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void * findPointer(void *oldPtr)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:348
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:347
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:349
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
static void PSolve_Links(btSoftBody *psb, btScalar kst, btScalar ti)
static void PSolve_SContacts(btSoftBody *psb, btScalar, btScalar ti)
bool checkLink(int node0, int node1) const
Definition: btSoftBody.cpp:140
bool m_bUpdateRtCst
Definition: btSoftBody.h:796
btScalar m_sleepingThreshold
Definition: btSoftBody.h:802
bool checkFace(int node0, int node1, int node2) const
Definition: btSoftBody.cpp:162
void advanceDeformation()
void updateClusters()
btDbvt m_cdbvt
Definition: btSoftBody.h:799
void setPose(bool bvolume, bool bframe)
Definition: btSoftBody.cpp:983
bool cutLink(int node0, int node1, btScalar position)
void appendFace(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:316
void setMass(int node, btScalar mass)
Definition: btSoftBody.cpp:789
void interpolateRenderMesh()
tJointArray m_joints
Definition: btSoftBody.h:792
btScalar m_dampingCoefficient
Definition: btSoftBody.h:801
btAlignedObjectArray< TetraScratch > m_tetraScratchesTn
Definition: btSoftBody.h:784
void integrateMotion()
void appendLinearJoint(const LJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:507
void scale(const btVector3 &scl)
Definition: btSoftBody.cpp:941
btAlignedObjectArray< bool > m_clusterConnectivity
Definition: btSoftBody.h:811
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
btScalar getVolume() const
bool rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, sRayCast &results)
Ray casting using rayFrom and rayTo in worldspace, (not direction!)
SolverState m_sst
Definition: btSoftBody.h:772
void addVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:760
void predictMotion(btScalar dt)
void setSelfCollision(bool useSelfCollision)
btScalar m_timeacc
Definition: btSoftBody.h:794
Pose m_pose
Definition: btSoftBody.h:773
void appendTetra(int model, Material *mat)
Definition: btSoftBody.cpp:356
void setRestLengthScale(btScalar restLength)
Definition: btSoftBody.cpp:968
void rotate(const btQuaternion &rot)
Definition: btSoftBody.cpp:932
void applyClusters(bool drift)
void setZeroVelocity()
static void PSolve_Anchors(btSoftBody *psb, btScalar kst, btScalar ti)
btSoftBodyWorldInfo * m_worldInfo
Definition: btSoftBody.h:775
void updateArea(bool averageArea=true)
void addForce(const btVector3 &force)
Definition: btSoftBody.cpp:560
bool wantsSleeping()
void prepareClusters(int iterations)
void setCollisionQuadrature(int N)
static void clusterVImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
static void VSolve_Links(btSoftBody *psb, btScalar kst)
tTetraArray m_tetras
Definition: btSoftBody.h:782
bool useSelfCollision()
btVector3 evaluateCom() const
void setTotalDensity(btScalar density)
Definition: btSoftBody.cpp:849
btAlignedObjectArray< const class btCollisionObject * > m_collisionDisabledObjects
Definition: btSoftBody.h:75
static void clusterDAImpulse(Cluster *cluster, const btVector3 &impulse)
void appendNode(const btVector3 &x, btScalar m)
Definition: btSoftBody.cpp:256
btTransform m_initialWorldTransform
Definition: btSoftBody.h:813
void staticSolve(int iterations)
void setVolumeMass(btScalar mass)
Definition: btSoftBody.cpp:855
btScalar m_restLengthScale
Definition: btSoftBody.h:817
bool checkDeformableContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
void cleanupClusters()
Config m_cfg
Definition: btSoftBody.h:771
void updateDeactivation(btScalar timeStep)
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition: btSoftBody.h:783
const btVector3 & getWindVelocity()
Return the wind velocity for interaction with the air.
void addAeroForceToFace(const btVector3 &windVelocity, int faceIndex)
Definition: btSoftBody.cpp:664
btAlignedObjectArray< btVector4 > m_renderNodesInterpolationWeights
Definition: btSoftBody.h:807
void appendAngularJoint(const AJoint::Specs &specs, Cluster *body0, Body body1)
Definition: btSoftBody.cpp:533
tFaceArray m_faces
Definition: btSoftBody.h:780
void setVolumeDensity(btScalar density)
Definition: btSoftBody.cpp:885
static void clusterDCImpulse(Cluster *cluster, const btVector3 &impulse)
void transform(const btTransform &trs)
Definition: btSoftBody.cpp:900
static void clusterVAImpulse(Cluster *cluster, const btVector3 &impulse)
btScalar getMass(int node) const
Definition: btSoftBody.cpp:796
tMaterialArray m_materials
Definition: btSoftBody.h:793
void setMaxStress(btScalar maxStress)
void dampClusters()
btSoftBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
btSoftBody implementation by Nathanael Presson
Definition: btSoftBody.cpp:28
void updateDeformation()
void addAeroForceToNode(const btVector3 &windVelocity, int nodeIndex)
Definition: btSoftBody.cpp:575
static btVector3 clusterCom(const Cluster *cluster)
tRContactArray m_rcontacts
Definition: btSoftBody.h:787
void appendAnchor(int node, btRigidBody *body, bool disableCollisionBetweenLinkedBodies=false, btScalar influence=1)
Definition: btSoftBody.cpp:388
btVector3 m_bounds[2]
Definition: btSoftBody.h:795
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:803
void releaseCluster(int index)
void setVelocity(const btVector3 &velocity)
Definition: btSoftBody.cpp:766
tClusterArray m_clusters
Definition: btSoftBody.h:800
void solveConstraints()
int generateClusters(int k, int maxiterations=8192)
generateClusters with k=0 will create a convex cluster for each tetrahedron or triangle otherwise an ...
void releaseClusters()
void refine(ImplicitFn *ifn, btScalar accurary, bool cut)
void setSolver(eSolverPresets::_ preset)
Material * appendMaterial()
Definition: btSoftBody.cpp:186
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition: btSoftBody.h:786
static void solveClusters(const btAlignedObjectArray< btSoftBody * > &bodies)
void appendNote(const char *text, const btVector3 &o, const btVector4 &c=btVector4(1, 0, 0, 0), Node *n0=0, Node *n1=0, Node *n2=0, Node *n3=0)
Definition: btSoftBody.cpp:198
bool checkDeformableFaceContact(const btCollisionObjectWrapper *colObjWrap, Face &f, btVector3 &contact_point, btVector3 &bary, btScalar margin, btSoftBody::sCti &cti, bool predict=false) const
virtual int calculateSerializeBufferSize() const
static void PSolve_RContacts(btSoftBody *psb, btScalar kst, btScalar ti)
static void clusterImpulse(Cluster *cluster, const btVector3 &rpos, const Impulse &impulse)
btAlignedObjectArray< btAlignedObjectArray< const btSoftBody::Node * > > m_renderNodesParents
Definition: btSoftBody.h:808
void pointersToIndices()
tNoteArray m_notes
Definition: btSoftBody.h:776
void updateNormals()
static void clusterDImpulse(Cluster *cluster, const btVector3 &rpos, const btVector3 &impulse)
btDbvt m_fdbvt
Definition: btSoftBody.h:798
tLinkArray m_links
Definition: btSoftBody.h:779
void applyForces()
static btVector3 clusterVelocity(const Cluster *cluster, const btVector3 &rpos)
tSContactArray m_scontacts
Definition: btSoftBody.h:791
bool m_useSelfCollision
Definition: btSoftBody.h:809
void * m_tag
Definition: btSoftBody.h:774
void updatePose()
void(* psolver_t)(btSoftBody *, btScalar, btScalar)
Definition: btSoftBody.h:751
void initializeClusters()
tAnchorArray m_anchors
Definition: btSoftBody.h:785
btScalar getRestLengthScale()
Definition: btSoftBody.cpp:962
void randomizeConstraints()
btVector3 m_windVelocity
Definition: btSoftBody.h:815
btScalar getTotalMass() const
Definition: btSoftBody.cpp:802
bool m_useFaceContact
Definition: btSoftBody.h:804
tNodeArray m_nodes
Definition: btSoftBody.h:777
void appendLink(int model=-1, Material *mat=0)
Definition: btSoftBody.cpp:276
void setSpringStiffness(btScalar k)
void initializeDmInverse()
void updateConstants()
void setTotalMass(btScalar mass, bool fromfaces=false)
Definition: btSoftBody.cpp:813
virtual ~btSoftBody()
Definition: btSoftBody.cpp:126
void appendDeformableAnchor(int node, btRigidBody *body)
Definition: btSoftBody.cpp:415
void updateLinkConstants()
void(* vsolver_t)(btSoftBody *, btScalar)
Definition: btSoftBody.h:752
tNodeArray m_renderNodes
Definition: btSoftBody.h:778
void initDefaults()
Definition: btSoftBody.cpp:64
btAlignedObjectArray< btVector3 > m_quads
Definition: btSoftBody.h:805
static psolver_t getSolver(ePSolver::_ solver)
bool checkContact(const btCollisionObjectWrapper *colObjWrap, const btVector3 &x, btScalar margin, btSoftBody::sCti &cti) const
void indicesToPointers(const int *map=0)
static void solveCommonConstraints(btSoftBody **bodies, int count, int iterations)
void updateBounds()
void setWindVelocity(const btVector3 &velocity)
Set a wind velocity for interaction with the air.
int generateBendingConstraints(int distance, Material *mat=0)
void translate(const btVector3 &trs)
Definition: btSoftBody.cpp:923
void initializeFaceTree()
void resetLinkRestLengths()
int clusterCount() const
btDbvt m_ndbvt
Definition: btSoftBody.h:797
static void clusterAImpulse(Cluster *cluster, const Impulse &impulse)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
void setBasis(const btMatrix3x3 &basis)
Set the rotational element by btMatrix3x3.
Definition: btTransform.h:154
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 & safeNormalize()
Definition: btVector3.h:286
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
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & w() const
Return the w value.
Definition: btVector3.h:581
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
void serializeFloat(struct btVector3FloatData &dataOut) const
Definition: btVector3.h:1291
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
void setX(btScalar _x)
Set the x value.
Definition: btVector3.h:567
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
void setZero()
Definition: btVector3.h:671
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
btVector3FloatData m_av
btMatrix3x3FloatData m_invwi
btVector3FloatData m_vimpulses[2]
btVector3FloatData * m_framerefs
btVector3FloatData m_dimpulses[2]
btMatrix3x3FloatData m_locii
btVector3FloatData m_lv
btTransformFloatData m_framexform
float m_selfCollisionImpulseFactor
btVector3FloatData m_com
btVector3FloatData m_normal
SoftBodyMaterialData * m_material
SoftBodyMaterialData * m_material
btVector3FloatData m_accumulatedForce
btVector3FloatData m_normal
btVector3FloatData m_previousPosition
btVector3FloatData m_velocity
btVector3FloatData m_position
SoftBodyMaterialData * m_material
btMatrix3x3FloatData m_rot
btVector3FloatData m_com
btVector3FloatData * m_positions
btMatrix3x3FloatData m_aqq
btMatrix3x3FloatData m_scale
btVector3FloatData m_c0[4]
SoftBodyMaterialData * m_material
btMatrix3x3FloatData m_c0
btVector3FloatData m_localFrame
btVector3FloatData m_c1
btRigidBodyData * m_rigidBody
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform * m_preTransform
const btTransform & getWorldTransform() const
void * data
Definition: btDbvt.h:202
btVector3 normal
Definition: btDbvt.h:197
btDbvntNode * childs[2]
Definition: btDbvt.h:201
btScalar angle
Definition: btDbvt.h:198
DBVT_INLINE bool isleaf() const
Definition: btDbvt.h:199
static btDbvtAabbMm FromMM(const btVector3 &mi, const btVector3 &mx)
Definition: btDbvt.h:479
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition: btDbvt.h:473
DBVT_INLINE bool isinternal() const
Definition: btDbvt.h:185
btDbvtNode * childs[2]
Definition: btDbvt.h:187
void * data
Definition: btDbvt.h:188
btDbvtNode * insert(const btDbvtVolume &box, void *data)
Definition: btDbvt.cpp:535
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:514
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:544
DBVT_PREFIX void collideTV(const btDbvtNode *root, const btDbvtVolume &volume, DBVT_IPOLICY) const
Definition: btDbvt.h:1148
bool empty() const
Definition: btDbvt.h:314
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:1276
void clear()
Definition: btDbvt.cpp:477
btDbvtNode * m_root
Definition: btDbvt.h:302
void remove(btDbvtNode *leaf)
Definition: btDbvt.cpp:611
DBVT_PREFIX void collideTT(const btDbvtNode *root0, const btDbvtNode *root1, DBVT_IPOLICY)
Definition: btDbvt.h:822
DBVT_PREFIX void selfCollideT(const btDbvntNode *root, DBVT_IPOLICY)
Definition: btDbvt.h:887
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:1021
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
btVector3FloatData m_refs[2]
btVector3FloatData m_relPosition[2]
btScalar air_density
Definition: btSoftBody.h:47
btDispatcher * m_dispatcher
Definition: btSoftBody.h:53
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:55
btVector3 m_gravity
Definition: btSoftBody.h:54
btScalar m_maxDisplacement
Definition: btSoftBody.h:50
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:52
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
IControl * m_icontrol
Definition: btSoftBody.h:663
void Terminate(btScalar dt)
btScalar m_influence
Definition: btSoftBody.h:405
btVector3 m_local
Definition: btSoftBody.h:403
btRigidBody * m_body
Definition: btSoftBody.h:404
btMatrix3x3 m_c0
Definition: btSoftBody.h:406
const btTransform & xform() const
Definition: btSoftBody.h:521
void Terminate(btScalar dt)
void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
btVector3 m_dimpulses[2]
Definition: btSoftBody.h:445
tVector3Array m_framerefs
Definition: btSoftBody.h:437
btMatrix3x3 m_invwi
Definition: btSoftBody.h:442
btMatrix3x3 m_locii
Definition: btSoftBody.h:441
btAlignedObjectArray< Node * > m_nodes
Definition: btSoftBody.h:436
btDbvtNode * m_leaf
Definition: btSoftBody.h:450
btVector3 m_vimpulses[2]
Definition: btSoftBody.h:444
tScalarArray m_masses
Definition: btSoftBody.h:435
btTransform m_framexform
Definition: btSoftBody.h:438
btScalar maxvolume
Definition: btSoftBody.h:704
tPSolverArray m_psequence
Definition: btSoftBody.h:712
tPSolverArray m_dsequence
Definition: btSoftBody.h:713
btScalar kSK_SPLT_CL
Definition: btSoftBody.h:702
btScalar kSS_SPLT_CL
Definition: btSoftBody.h:703
btScalar m_maxStress
Definition: btSoftBody.h:715
eAeroModel::_ aeromodel
Definition: btSoftBody.h:685
btScalar kSR_SPLT_CL
Definition: btSoftBody.h:701
tVSolverArray m_vsequence
Definition: btSoftBody.h:711
btScalar timescale
Definition: btSoftBody.h:705
btVector4 m_pcontact
Definition: btSoftBody.h:291
btVector3 m_normal
Definition: btSoftBody.h:288
Node * m_n[3]
Definition: btSoftBody.h:287
btDbvtNode * m_leaf
Definition: btSoftBody.h:290
Material * m_material
Definition: btSoftBody.h:252
virtual btScalar Eval(const btVector3 &x)=0
btVector3 m_velocity
Definition: btSoftBody.h:467
btScalar m_split
Definition: btSoftBody.h:617
btVector3 m_refs[2]
Definition: btSoftBody.h:614
virtual void Prepare(btScalar dt, int iterations)
void Solve(btScalar dt, btScalar sor)
void Prepare(btScalar dt, int iterations)
void Terminate(btScalar dt)
btScalar m_area
Definition: btSoftBody.h:265
btVector3 m_x
Definition: btSoftBody.h:257
btVector3 m_v
Definition: btSoftBody.h:259
btVector3 m_q
Definition: btSoftBody.h:258
btDbvtNode * m_leaf
Definition: btSoftBody.h:266
btVector3 m_n
Definition: btSoftBody.h:263
btVector3 m_f
Definition: btSoftBody.h:262
btScalar m_coords[4]
Definition: btSoftBody.h:417
btVector3 m_offset
Definition: btSoftBody.h:414
Node * m_nodes[4]
Definition: btSoftBody.h:416
const char * m_text
Definition: btSoftBody.h:413
btMatrix3x3 m_scl
Definition: btSoftBody.h:429
btScalar m_volume
Definition: btSoftBody.h:424
btVector3 m_com
Definition: btSoftBody.h:427
tVector3Array m_pos
Definition: btSoftBody.h:425
btMatrix3x3 m_aqq
Definition: btSoftBody.h:430
btMatrix3x3 m_rot
Definition: btSoftBody.h:428
tScalarArray m_wgh
Definition: btSoftBody.h:426
btMatrix3x3 m_c0
Definition: btSoftBody.h:322
RayFromToCaster takes a ray from, ray to (instead of direction!)
Definition: btSoftBody.h:728
RayFromToCaster(const btVector3 &rayFrom, const btVector3 &rayTo, btScalar mxt)
void Process(const btDbvtNode *leaf)
static btScalar rayFromToTriangle(const btVector3 &rayFrom, const btVector3 &rayTo, const btVector3 &rayNormalizedDirection, const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar maxt=SIMD_INFINITY)
btVector3 m_rayNormalizedDirection
Definition: btSoftBody.h:731
btScalar m_cfm[2]
Definition: btSoftBody.h:397
btScalar m_element_measure
Definition: btSoftBody.h:305
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:303
btMatrix3x3 m_F
Definition: btSoftBody.h:304
@ V_TwoSided
Vertex normals are oriented toward velocity.
Definition: btSoftBody.h:90
@ V_OneSided
Vertex normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:92
@ V_TwoSidedLiftDrag
Vertex normals are flipped to match velocity.
Definition: btSoftBody.h:91
@ F_OneSided
Face normals are flipped to match velocity and lift and drag forces are applied.
Definition: btSoftBody.h:95
@ F_TwoSided
Vertex normals are taken as it is.
Definition: btSoftBody.h:93
@ F_TwoSidedLiftDrag
Face normals are flipped to match velocity.
Definition: btSoftBody.h:94
@ RContacts
Anchor solver.
Definition: btSoftBody.h:117
@ SContacts
Rigid contacts solver.
Definition: btSoftBody.h:118
@ Anchors
Linear solver.
Definition: btSoftBody.h:116
@ VF_SS
Rigid versus soft mask.
Definition: btSoftBody.h:168
@ Default
Vertex vs face soft vs soft handling.
Definition: btSoftBody.h:173
@ VF_DD
Cluster soft body self collision.
Definition: btSoftBody.h:171
@ CL_SS
Vertex vs face soft vs soft handling.
Definition: btSoftBody.h:169
@ CL_SELF
Cluster vs cluster soft vs soft handling.
Definition: btSoftBody.h:170
@ SVSmask
DF based rigid vs deformable faces.
Definition: btSoftBody.h:167
@ SDF_RS
Rigid versus soft mask.
Definition: btSoftBody.h:162
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:164
@ CL_RS
SDF based rigid vs soft.
Definition: btSoftBody.h:163
@ Default
Enable debug draw.
Definition: btSoftBody.h:185
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btScalar m_offset
Definition: btSoftBody.h:222
btVector3 m_normal
Definition: btSoftBody.h:221
btVector3 m_velocity
Definition: btSoftBody.h:229
eFeature::_ feature
soft body
Definition: btSoftBody.h:198
btScalar fraction
feature index
Definition: btSoftBody.h:200
int index
feature type
Definition: btSoftBody.h:199
btSoftBody * body
Definition: btSoftBody.h:197
void ProcessColObj(btSoftBody *ps, const btCollisionObjectWrapper *colObWrap)
void ProcessSoftSoft(btSoftBody *psa, btSoftBody *psb)
const btCollisionObjectWrapper * m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
const btCollisionObjectWrapper * m_colObj1Wrap
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:196