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