Blender  V3.3
btDeformableMultiBodyDynamicsWorld.cpp
Go to the documentation of this file.
1 /*
2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
3 
4  Bullet Continuous Collision Detection and Physics Library
5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
6  This software is provided 'as-is', without any express or implied warranty.
7  In no event will the authors be held liable for any damages arising from the use of this software.
8  Permission is granted to anyone to use this software for any purpose,
9  including commercial applications, and to alter it and redistribute it freely,
10  subject to the following restrictions:
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  */
15 
16 /* ====== Overview of the Deformable Algorithm ====== */
17 
18 /*
19 A single step of the deformable body simulation contains the following main components:
20 Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
21 1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
22 2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
23 
24 3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
25 
26 3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
27  M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
28  by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
29  Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
30 4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
31 
32 
33 The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
34  */
35 
36 #include <stdio.h>
39 #include "btDeformableBodySolver.h"
40 #include "LinearMath/btQuickprof.h"
41 #include "btSoftBodyInternals.h"
42 btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
43  : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
44  m_deformableBodySolver(deformableBodySolver),
45  m_solverCallback(0)
46 {
47  m_drawFlags = fDrawFlags::Std;
48  m_drawNodeTree = true;
49  m_drawFaceTree = false;
50  m_drawClusterTree = false;
51  m_sbi.m_broadphase = pairCache;
52  m_sbi.m_dispatcher = dispatcher;
53  m_sbi.m_sparsesdf.Initialize();
54  m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
55  m_sbi.m_sparsesdf.Reset();
56 
57  m_sbi.air_density = (btScalar)1.2;
58  m_sbi.water_density = 0;
59  m_sbi.water_offset = 0;
60  m_sbi.water_normal = btVector3(0, 0, 0);
61  m_sbi.m_gravity.setValue(0, -9.8, 0);
62  m_internalTime = 0.0;
63  m_implicit = false;
64  m_lineSearch = false;
65  m_useProjection = false;
66  m_ccdIterations = 5;
67  m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
68 }
69 
71 {
72  delete m_solverDeformableBodyIslandCallback;
73 }
74 
76 {
77  BT_PROFILE("internalSingleStepSimulation");
78  if (0 != m_internalPreTickCallback)
79  {
80  (*m_internalPreTickCallback)(this, timeStep);
81  }
82  reinitialize(timeStep);
83 
84  // add gravity to velocity of rigid and multi bodys
85  applyRigidBodyGravity(timeStep);
86 
88  predictUnconstraintMotion(timeStep);
89 
91  btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
92 
94 
95  beforeSolverCallbacks(timeStep);
96 
98  solveConstraints(timeStep);
99 
100  afterSolverCallbacks(timeStep);
101 
103 
104  applyRepulsionForce(timeStep);
105 
106  performGeometricCollisions(timeStep);
107 
108  integrateTransforms(timeStep);
109 
112 
113  updateActivationState(timeStep);
114  // End solver-wise simulation step
115  // ///////////////////////////////
116 }
117 
119 {
120  for (int i = 0; i < m_softBodies.size(); ++i)
121  {
122  m_softBodies[i]->m_softSoftCollision = true;
123  }
124 
125  for (int i = 0; i < m_softBodies.size(); ++i)
126  {
127  for (int j = i; j < m_softBodies.size(); ++j)
128  {
129  m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
130  }
131  }
132 
133  for (int i = 0; i < m_softBodies.size(); ++i)
134  {
135  m_softBodies[i]->m_softSoftCollision = false;
136  }
137 }
138 
140 {
141  for (int i = 0; i < m_softBodies.size(); i++)
142  {
143  btSoftBody* psb = m_softBodies[i];
144  psb->updateDeactivation(timeStep);
145  if (psb->wantsSleeping())
146  {
147  if (psb->getActivationState() == ACTIVE_TAG)
148  psb->setActivationState(WANTS_DEACTIVATION);
149  if (psb->getActivationState() == ISLAND_SLEEPING)
150  {
151  psb->setZeroVelocity();
152  }
153  }
154  else
155  {
156  if (psb->getActivationState() != DISABLE_DEACTIVATION)
157  psb->setActivationState(ACTIVE_TAG);
158  }
159  }
161 }
162 
164 {
165  BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
166  for (int i = 0; i < m_softBodies.size(); i++)
167  {
168  btSoftBody* psb = m_softBodies[i];
169  if (psb->isActive())
170  {
171  psb->applyRepulsionForce(timeStep, true);
172  }
173  }
174 }
175 
177 {
178  BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
179  // refit the BVH tree for CCD
180  for (int i = 0; i < m_softBodies.size(); ++i)
181  {
182  btSoftBody* psb = m_softBodies[i];
183  if (psb->isActive())
184  {
185  m_softBodies[i]->updateFaceTree(true, false);
186  m_softBodies[i]->updateNodeTree(true, false);
187  for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
188  {
189  btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
190  f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x);
191  }
192  }
193  }
194 
195  // clear contact points & update DBVT
196  for (int r = 0; r < m_ccdIterations; ++r)
197  {
198  for (int i = 0; i < m_softBodies.size(); ++i)
199  {
200  btSoftBody* psb = m_softBodies[i];
201  if (psb->isActive())
202  {
203  // clear contact points in the previous iteration
204  psb->m_faceNodeContacts.clear();
205 
206  // update m_q and normals for CCD calculation
207  for (int j = 0; j < psb->m_nodes.size(); ++j)
208  {
209  psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
210  }
211  for (int j = 0; j < psb->m_faces.size(); ++j)
212  {
213  btSoftBody::Face& f = psb->m_faces[j];
214  f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q);
215  f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
216  }
217  }
218  }
219 
220  // apply CCD to register new contact points
221  for (int i = 0; i < m_softBodies.size(); ++i)
222  {
223  for (int j = i; j < m_softBodies.size(); ++j)
224  {
225  btSoftBody* psb1 = m_softBodies[i];
226  btSoftBody* psb2 = m_softBodies[j];
227  if (psb1->isActive() && psb2->isActive())
228  {
229  m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
230  }
231  }
232  }
233 
234  int penetration_count = 0;
235  for (int i = 0; i < m_softBodies.size(); ++i)
236  {
237  btSoftBody* psb = m_softBodies[i];
238  if (psb->isActive())
239  {
240  penetration_count += psb->m_faceNodeContacts.size();
241  }
242  }
243  if (penetration_count == 0)
244  {
245  break;
246  }
247 
248  // apply inelastic impulse
249  for (int i = 0; i < m_softBodies.size(); ++i)
250  {
251  btSoftBody* psb = m_softBodies[i];
252  if (psb->isActive())
253  {
254  psb->applyRepulsionForce(timeStep, false);
255  }
256  }
257  }
258 }
259 
261 {
262  BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
263  for (int i = 0; i < m_softBodies.size(); i++)
264  {
265  btSoftBody* psb = m_softBodies[i];
266  if (psb->isActive())
267  {
268  psb->defaultCollisionHandler(psb);
269  }
270  }
271 }
272 
274 {
275  // correct the position of rigid bodies with temporary velocity generated from split impulse
277  btVector3 zero(0, 0, 0);
278  for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
279  {
281  //correct the position/orientation based on push/turn recovery
282  btTransform newTransform;
283  btVector3 pushVelocity = rb->getPushVelocity();
284  btVector3 turnVelocity = rb->getTurnVelocity();
285  if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
286  {
287  btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
288  rb->setWorldTransform(newTransform);
289  rb->setPushVelocity(zero);
290  rb->setTurnVelocity(zero);
291  }
292  }
293 }
294 
296 {
297  BT_PROFILE("integrateTransforms");
298  positionCorrection(timeStep);
300  for (int i = 0; i < m_softBodies.size(); ++i)
301  {
302  btSoftBody* psb = m_softBodies[i];
303  for (int j = 0; j < psb->m_nodes.size(); ++j)
304  {
305  btSoftBody::Node& node = psb->m_nodes[j];
306  btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
307  btScalar clampDeltaV = maxDisplacement / timeStep;
308  for (int c = 0; c < 3; c++)
309  {
310  if (node.m_v[c] > clampDeltaV)
311  {
312  node.m_v[c] = clampDeltaV;
313  }
314  if (node.m_v[c] < -clampDeltaV)
315  {
316  node.m_v[c] = -clampDeltaV;
317  }
318  }
319  node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
320  node.m_q = node.m_x;
321  node.m_vn = node.m_v;
322  }
323  // enforce anchor constraints
324  for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
325  {
327  btSoftBody::Node* n = a.m_node;
328  n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
329 
330  // update multibody anchor info
331  if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
332  {
334  if (multibodyLinkCol)
335  {
336  btVector3 nrm;
337  const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
338  const btTransform& wtr = multibodyLinkCol->getWorldTransform();
340  wtr.invXform(n->m_x),
341  shp,
342  nrm,
343  0);
344  a.m_cti.m_normal = wtr.getBasis() * nrm;
345  btVector3 normal = a.m_cti.m_normal;
347  btVector3 t2 = btCross(normal, t1);
348  btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
349  findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
350  findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
351  findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
352 
353  btScalar* J_n = &jacobianData_normal.m_jacobians[0];
354  btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
355  btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
356 
357  btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
358  btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
359  btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
360 
361  btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
362  t1.getX(), t1.getY(), t1.getZ(),
363  t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
364  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
365  btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
366  a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
367  a.jacobianData_normal = jacobianData_normal;
368  a.jacobianData_t1 = jacobianData_t1;
369  a.jacobianData_t2 = jacobianData_t2;
370  a.t1 = t1;
371  a.t2 = t2;
372  }
373  }
374  }
375  psb->interpolateRenderMesh();
376  }
377 }
378 
380 {
381  BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
382  // save v_{n+1}^* velocity after explicit forces
383  m_deformableBodySolver->backupVelocity();
384 
385  // set up constraints among multibodies and between multibodies and deformable bodies
387 
388  // solve contact constraints
390 
391  // set up the directions in which the velocity does not change in the momentum solve
392  if (m_useProjection)
393  m_deformableBodySolver->m_objective->m_projection.setProjection();
394  else
395  m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
396 
397  // for explicit scheme, m_backupVelocity = v_{n+1}^*
398  // for implicit scheme, m_backupVelocity = v_n
399  // Here, set dv = v_{n+1} - v_n for nodes in contact
400  m_deformableBodySolver->setupDeformableSolve(m_implicit);
401 
402  // At this point, dv should be golden for nodes in contact
403  // proceed to solve deformable momentum equation
404  m_deformableBodySolver->solveDeformableConstraints(timeStep);
405 }
406 
408 {
409  // set up constraints between multibody and deformable bodies
410  m_deformableBodySolver->setConstraints(m_solverInfo);
411 
412  // set up constraints among multibodies
413  {
414  sortConstraints();
415  // setup the solver callback
416  btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
417  btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
418  m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
419 
420  // build islands
422  }
423 }
424 
426 {
427  m_sortedConstraints.resize(m_constraints.size());
428  int i;
429  for (i = 0; i < getNumConstraints(); i++)
430  {
431  m_sortedConstraints[i] = m_constraints[i];
432  }
433  m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
434 
436  for (i = 0; i < m_multiBodyConstraints.size(); i++)
437  {
439  }
441 }
442 
444 {
445  // process constraints on each island
446  m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
447 
448  // process deferred
449  m_solverDeformableBodyIslandCallback->processConstraints();
450  m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
451 
452  // write joint feedback
453  {
454  for (int i = 0; i < this->m_multiBodies.size(); i++)
455  {
456  btMultiBody* bod = m_multiBodies[i];
457 
458  bool isSleeping = false;
459 
460  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
461  {
462  isSleeping = true;
463  }
464  for (int b = 0; b < bod->getNumLinks(); b++)
465  {
466  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
467  isSleeping = true;
468  }
469 
470  if (!isSleeping)
471  {
472  //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
473  m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd)
474  m_scratch_v.resize(bod->getNumLinks() + 1);
475  m_scratch_m.resize(bod->getNumLinks() + 1);
476 
477  if (bod->internalNeedsJointFeedback())
478  {
479  if (!bod->isUsingRK4Integration())
480  {
481  if (bod->internalNeedsJointFeedback())
482  {
483  bool isConstraintPass = true;
484  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
485  getSolverInfo().m_jointFeedbackInWorldSpace,
486  getSolverInfo().m_jointFeedbackInJointFrame);
487  }
488  }
489  }
490  }
491  }
492  }
493 
494  for (int i = 0; i < this->m_multiBodies.size(); i++)
495  {
496  btMultiBody* bod = m_multiBodies[i];
497  bod->processDeltaVeeMultiDof2();
498  }
499 }
500 
501 void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
502 {
503  m_softBodies.push_back(body);
504 
505  // Set the soft body solver that will deal with this body
506  // to be the world's solver
507  body->setSoftBodySolver(m_deformableBodySolver);
508 
510  collisionFilterGroup,
511  collisionFilterMask);
512 }
513 
515 {
516  BT_PROFILE("predictUnconstraintMotion");
518  m_deformableBodySolver->predictMotion(timeStep);
519 }
520 
522 {
523  m_internalTime += timeStep;
524  m_deformableBodySolver->setImplicit(m_implicit);
525  m_deformableBodySolver->setLineSearch(m_lineSearch);
526  m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
527  btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
528  dispatchInfo.m_timeStep = timeStep;
529  dispatchInfo.m_stepCount = 0;
530  dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
531  btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
532  if (m_useProjection)
533  {
534  m_deformableBodySolver->m_useProjection = true;
535  m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
536  m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
537  }
538  else
539  {
540  m_deformableBodySolver->m_useProjection = false;
541  m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
542  m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
543  }
544 }
545 
547 {
549 
550  for (int i = 0; i < getSoftBodyArray().size(); i++)
551  {
552  btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
553  {
554  btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer());
555  btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
556  }
557  }
558 }
559 
561 {
562  // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
563  // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
564  // when there are multiple substeps
566  // integrate rigid body gravity
567  for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
568  {
570  rb->integrateVelocities(timeStep);
571  }
572 
573  // integrate multibody gravity
574  {
577  {
578  for (int i = 0; i < this->m_multiBodies.size(); i++)
579  {
580  btMultiBody* bod = m_multiBodies[i];
581 
582  bool isSleeping = false;
583 
584  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
585  {
586  isSleeping = true;
587  }
588  for (int b = 0; b < bod->getNumLinks(); b++)
589  {
590  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
591  isSleeping = true;
592  }
593 
594  if (!isSleeping)
595  {
596  m_scratch_r.resize(bod->getNumLinks() + 1);
597  m_scratch_v.resize(bod->getNumLinks() + 1);
598  m_scratch_m.resize(bod->getNumLinks() + 1);
599  bool isConstraintPass = false;
600  {
601  if (!bod->isUsingRK4Integration())
602  {
603  bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
604  m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
605  getSolverInfo().m_jointFeedbackInWorldSpace,
606  getSolverInfo().m_jointFeedbackInJointFrame);
607  }
608  else
609  {
610  btAssert(" RK4Integration is not supported");
611  }
612  }
613  }
614  }
615  }
616  }
617  clearGravity();
618 }
619 
621 {
622  BT_PROFILE("btMultiBody clearGravity");
623  // clear rigid body gravity
624  for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
625  {
627  if (body->isActive())
628  {
629  body->clearGravity();
630  }
631  }
632  // clear multibody gravity
633  for (int i = 0; i < this->m_multiBodies.size(); i++)
634  {
635  btMultiBody* bod = m_multiBodies[i];
636 
637  bool isSleeping = false;
638 
639  if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
640  {
641  isSleeping = true;
642  }
643  for (int b = 0; b < bod->getNumLinks(); b++)
644  {
645  if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
646  isSleeping = true;
647  }
648 
649  if (!isSleeping)
650  {
651  bod->addBaseForce(-m_gravity * bod->getBaseMass());
652 
653  for (int j = 0; j < bod->getNumLinks(); ++j)
654  {
655  bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
656  }
657  }
658  }
659 }
660 
662 {
663  if (0 != m_internalTickCallback)
664  {
665  (*m_internalTickCallback)(this, timeStep);
666  }
667 
668  if (0 != m_solverCallback)
669  {
670  (*m_solverCallback)(m_internalTime, this);
671  }
672 }
673 
675 {
676  if (0 != m_solverCallback)
677  {
678  (*m_solverCallback)(m_internalTime, this);
679  }
680 }
681 
683 {
684  btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
685  bool added = false;
686  for (int i = 0; i < forces.size(); ++i)
687  {
688  if (forces[i]->getForceType() == force->getForceType())
689  {
690  forces[i]->addSoftBody(psb);
691  added = true;
692  break;
693  }
694  }
695  if (!added)
696  {
697  force->addSoftBody(psb);
698  force->setIndices(m_deformableBodySolver->m_objective->getIndices());
699  forces.push_back(force);
700  }
701 }
702 
704 {
705  btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
706  int removed_index = -1;
707  for (int i = 0; i < forces.size(); ++i)
708  {
709  if (forces[i]->getForceType() == force->getForceType())
710  {
711  forces[i]->removeSoftBody(psb);
712  if (forces[i]->m_softBodies.size() == 0)
713  removed_index = i;
714  break;
715  }
716  }
717  if (removed_index >= 0)
718  forces.removeAtIndex(removed_index);
719 }
720 
722 {
723  btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
724  for (int i = 0; i < forces.size(); ++i)
725  {
726  forces[i]->removeSoftBody(psb);
727  }
728 }
729 
731 {
732  removeSoftBodyForce(body);
733  m_softBodies.remove(body);
735  // force a reinitialize so that node indices get updated.
736  m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
737 }
738 
740 {
741  btSoftBody* body = btSoftBody::upcast(collisionObject);
742  if (body)
743  removeSoftBody(body);
744  else
746 }
747 
748 int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
749 {
750  startProfiling(timeStep);
751 
752  int numSimulationSubSteps = 0;
753 
754  if (maxSubSteps)
755  {
756  //fixed timestep with interpolation
757  m_fixedTimeStep = fixedTimeStep;
758  m_localTime += timeStep;
759  if (m_localTime >= fixedTimeStep)
760  {
761  numSimulationSubSteps = int(m_localTime / fixedTimeStep);
762  m_localTime -= numSimulationSubSteps * fixedTimeStep;
763  }
764  }
765  else
766  {
767  //variable timestep
768  fixedTimeStep = timeStep;
770  m_fixedTimeStep = 0;
771  if (btFuzzyZero(timeStep))
772  {
773  numSimulationSubSteps = 0;
774  maxSubSteps = 0;
775  }
776  else
777  {
778  numSimulationSubSteps = 1;
779  maxSubSteps = 1;
780  }
781  }
782 
783  //process some debugging flags
784  if (getDebugDrawer())
785  {
786  btIDebugDraw* debugDrawer = getDebugDrawer();
788  }
789  if (numSimulationSubSteps)
790  {
791  //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
792  int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
793 
794  saveKinematicState(fixedTimeStep * clampedSimulationSteps);
795 
796  for (int i = 0; i < clampedSimulationSteps; i++)
797  {
798  internalSingleStepSimulation(fixedTimeStep);
800  }
801  }
802  else
803  {
805  }
806 
807  clearForces();
808 
809 #ifndef BT_NO_PROFILE
810  CProfileManager::Increment_Frame_Counter();
811 #endif //BT_NO_PROFILE
812 
813  return numSimulationSubSteps;
814 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
btCollisionObject
#define ACTIVE_TAG
#define DISABLE_DEACTIVATION
@ CO_FEATHERSTONE_LINK
#define WANTS_DEACTIVATION
#define ISLAND_SLEEPING
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void updateActions(btScalar timeStep)
btSimulationIslandManager * m_islandManager
virtual void saveKinematicState(btScalar timeStep)
btAlignedObjectArray< btRigidBody * > m_nonStaticRigidBodies
virtual void synchronizeMotionStates()
virtual int getNumConstraints() const
btScalar m_fixedTimeStep
btAlignedObjectArray< btTypedConstraint * > m_constraints
virtual void removeCollisionObject(btCollisionObject *collisionObject)
removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise ca...
btCollisionWorld * getCollisionWorld()
bool m_latencyMotionStateInterpolation
btScalar m_localTime
btConstraintSolver * m_constraintSolver
void startProfiling(btScalar timeStep)
btScalar m_gravity
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btTransform.h:182
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMultiBodyConstraint
btMultiBody
Definition: btMultiBody.h:51
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
static SIMD_FORCE_INLINE btVector3 generateUnitOrthogonalVector(const btVector3 &u)
static SIMD_FORCE_INLINE void findJacobian(const btMultiBodyLinkCollider *multibodyLinkCol, btMultiBodyJacobianData &jacobianData, const btVector3 &contact_point, const btVector3 &dir)
btSoftBody implementation by Nathanael Presson
static btMatrix3x3 Diagonal(btScalar x)
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void removeAtIndex(int index)
void remove(const T &key)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual void removeCollisionObject(btCollisionObject *collisionObject)
virtual void addCollisionObject(btCollisionObject *collisionObject, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
virtual void allSolved(const btContactSolverInfo &, class btIDebugDraw *)
btAlignedObjectArray< btDeformableLagrangianForce * > m_lf
const btAlignedObjectArray< btSoftBody::Node * > * getIndices() const
virtual void solveDeformableConstraints(btScalar solverdt)
void setConstraints(const btContactSolverInfo &infoGlobal)
btDeformableBackwardEulerObjective * m_objective
void reinitialize(const btAlignedObjectArray< btSoftBody * > &softBodies, btScalar dt)
void setupDeformableSolve(bool implicit)
void setLineSearch(bool lineSearch)
virtual void predictMotion(btScalar solverdt)
virtual void setIndices(const btAlignedObjectArray< btSoftBody::Node * > *nodes)
virtual btDeformableLagrangianForceType getForceType()=0
virtual void addSoftBody(btSoftBody *psb)
btDeformableMultiBodyDynamicsWorld(btDispatcher *dispatcher, btBroadphaseInterface *pairCache, btDeformableMultiBodyConstraintSolver *constraintSolver, btCollisionConfiguration *collisionConfiguration, btDeformableBodySolver *deformableBodySolver=0)
void removeCollisionObject(btCollisionObject *collisionObject)
virtual void addSoftBody(btSoftBody *body, int collisionFilterGroup=btBroadphaseProxy::DefaultFilter, int collisionFilterMask=btBroadphaseProxy::AllFilter)
virtual void predictUnconstraintMotion(btScalar timeStep)
void addForce(btSoftBody *psb, btDeformableLagrangianForce *force)
void removeForce(btSoftBody *psb, btDeformableLagrangianForce *force)
virtual void internalSingleStepSimulation(btScalar timeStep)
virtual int stepSimulation(btScalar timeStep, int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))
virtual int getDebugMode() const =0
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
virtual void updateActivationState(btScalar timeStep)
btAlignedObjectArray< btMatrix3x3 > m_scratch_m
btAlignedObjectArray< btVector3 > m_scratch_v
virtual void predictUnconstraintMotion(btScalar timeStep)
virtual void integrateTransforms(btScalar timeStep)
btAlignedObjectArray< btMultiBody * > m_multiBodies
btAlignedObjectArray< btScalar > m_scratch_r
btAlignedObjectArray< btMultiBodyConstraint * > m_sortedMultiBodyConstraints
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
void integrateVelocities(btScalar step)
void setTurnVelocity(const btVector3 &v)
Definition: btRigidBody.h:391
void clearGravity()
void setPushVelocity(const btVector3 &v)
Definition: btRigidBody.h:369
btVector3 getPushVelocity() const
Definition: btRigidBody.h:359
btVector3 getTurnVelocity() const
Definition: btRigidBody.h:364
void processIslands(btDispatcher *dispatcher, btCollisionWorld *collisionWorld, IslandCallback *callback)
void buildIslands(btDispatcher *dispatcher, btCollisionWorld *colWorld)
void interpolateRenderMesh()
void defaultCollisionHandler(const btCollisionObjectWrapper *pcoWrap)
btSoftBodyWorldInfo * getWorldInfo()
Definition: btSoftBody.h:862
void setZeroVelocity()
btSoftBodyWorldInfo * m_worldInfo
Definition: btSoftBody.h:797
void setSoftBodySolver(btSoftBodySolver *softBodySolver)
Definition: btSoftBody.h:1103
bool wantsSleeping()
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:811
void updateDeactivation(btScalar timeStep)
tFaceArray m_faces
Definition: btSoftBody.h:802
void applyRepulsionForce(btScalar timeStep, bool applySpringForce)
Definition: btSoftBody.h:1287
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition: btSoftBody.h:808
static const btSoftBody * upcast(const btCollisionObject *colObj)
Definition: btSoftBody.h:1128
tNodeArray m_nodes
Definition: btSoftBody.h:799
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
OperationNode * node
#define rot(x, k)
IconTextureDrawCall normal
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
virtual SIMD_FORCE_INLINE void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
btScalar m_timeStep
Definition: btDispatcher.h:53
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
static void Draw(btSoftBody *psb, btIDebugDraw *idraw, int drawflags=fDrawFlags::Std)
static void DrawFrame(btSoftBody *psb, btIDebugDraw *idraw)
btScalar air_density
Definition: btSoftBody.h:49
btDispatcher * m_dispatcher
Definition: btSoftBody.h:55
btScalar water_density
Definition: btSoftBody.h:50
btSparseSdf< 3 > m_sparsesdf
Definition: btSoftBody.h:57
btVector3 m_gravity
Definition: btSoftBody.h:56
btVector3 water_normal
Definition: btSoftBody.h:53
btScalar m_maxDisplacement
Definition: btSoftBody.h:52
btScalar water_offset
Definition: btSoftBody.h:51
btBroadphaseInterface * m_broadphase
Definition: btSoftBody.h:54
btVector3 m_n0
Definition: btSoftBody.h:301
btVector3 m_n1
Definition: btSoftBody.h:301
btVector3 m_vn
Definition: btSoftBody.h:301
Node * m_n[3]
Definition: btSoftBody.h:296
btVector3 m_x
Definition: btSoftBody.h:263
btVector3 m_v
Definition: btSoftBody.h:265
btVector3 m_q
Definition: btSoftBody.h:264
void Reset()
Definition: btSparseSDF.h:116
btScalar Evaluate(const btVector3 &x, const btCollisionShape *shape, btVector3 &normal, btScalar margin)
Definition: btSparseSDF.h:196
void setDefaultVoxelsz(btScalar sz)
Definition: btSparseSDF.h:111
void Initialize(int hashsize=2383, int clampCells=256 *1024)
Definition: btSparseSDF.h:100