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