Bullet Collision Detection & Physics Library
btDeformableBodySolver.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 #include <stdio.h>
17 #include <limits>
18 #include "btDeformableBodySolver.h"
19 #include "btSoftBodyInternals.h"
20 #include "LinearMath/btQuickprof.h"
21 static const int kMaxConjugateGradientIterations = 300;
23  : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
24 {
26 }
27 
29 {
30  delete m_objective;
31 }
32 
34 {
35  BT_PROFILE("solveDeformableConstraints");
36  if (!m_implicit)
37  {
40  if (m_useProjection)
41  {
43  }
44  else
45  {
46  TVStack rhs, x;
50  computeStep(x, rhs);
51  for (int i = 0; i < m_dv.size(); ++i)
52  {
53  m_dv[i] = x[i];
54  }
55  }
57  }
58  else
59  {
60  for (int i = 0; i < m_maxNewtonIterations; ++i)
61  {
62  updateState();
63  // add the inertia term in the residual
64  int counter = 0;
65  for (int k = 0; k < m_softBodies.size(); ++k)
66  {
67  btSoftBody* psb = m_softBodies[k];
68  for (int j = 0; j < psb->m_nodes.size(); ++j)
69  {
70  if (psb->m_nodes[j].m_im > 0)
71  {
72  m_residual[counter] = (-1. / psb->m_nodes[j].m_im) * m_dv[counter];
73  }
74  ++counter;
75  }
76  }
77 
80  {
81  break;
82  }
83  // todo xuchenhan@: this really only needs to be calculated once
85  if (m_lineSearch)
86  {
87  btScalar inner_product = computeDescentStep(m_ddv, m_residual);
88  btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
89  btScalar scale = 2;
90  btScalar f0 = m_objective->totalEnergy(solverdt) + kineticEnergy(), f1, f2;
91  backupDv();
92  do
93  {
94  scale *= beta;
95  if (scale < 1e-8)
96  {
97  return;
98  }
99  updateEnergy(scale);
100  f1 = m_objective->totalEnergy(solverdt) + kineticEnergy();
101  f2 = f0 - alpha * scale * inner_product;
102  } while (!(f1 < f2 + SIMD_EPSILON)); // if anything here is nan then the search continues
103  revertDv();
104  updateDv(scale);
105  }
106  else
107  {
109  updateDv();
110  }
111  for (int j = 0; j < m_numNodes; ++j)
112  {
113  m_ddv[j].setZero();
114  m_residual[j].setZero();
115  }
116  }
117  updateVelocity();
118  }
119 }
120 
122 {
123  btScalar ke = 0;
124  for (int i = 0; i < m_softBodies.size(); ++i)
125  {
126  btSoftBody* psb = m_softBodies[i];
127  for (int j = 0; j < psb->m_nodes.size(); ++j)
128  {
129  btSoftBody::Node& node = psb->m_nodes[j];
130  if (node.m_im > 0)
131  {
132  ke += m_dv[node.index].length2() * 0.5 / node.m_im;
133  }
134  }
135  }
136  return ke;
137 }
138 
140 {
142  for (int i = 0; i < m_backup_dv.size(); ++i)
143  {
144  m_backup_dv[i] = m_dv[i];
145  }
146 }
147 
149 {
150  for (int i = 0; i < m_backup_dv.size(); ++i)
151  {
152  m_dv[i] = m_backup_dv[i];
153  }
154 }
155 
157 {
158  for (int i = 0; i < m_dv.size(); ++i)
159  {
160  m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
161  }
162  updateState();
163 }
164 
166 {
167  m_cg.solve(*m_objective, ddv, residual, false);
168  btScalar inner_product = m_cg.dot(residual, m_ddv);
169  btScalar res_norm = m_objective->computeNorm(residual);
170  btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
171  if (inner_product < -tol)
172  {
173  if (verbose)
174  {
175  std::cout << "Looking backwards!" << std::endl;
176  }
177  for (int i = 0; i < m_ddv.size(); ++i)
178  {
179  m_ddv[i] = -m_ddv[i];
180  }
181  inner_product = -inner_product;
182  }
183  else if (std::abs(inner_product) < tol)
184  {
185  if (verbose)
186  {
187  std::cout << "Gradient Descent!" << std::endl;
188  }
189  btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
190  for (int i = 0; i < m_ddv.size(); ++i)
191  {
192  m_ddv[i] = scale * residual[i];
193  }
194  inner_product = scale * res_norm * res_norm;
195  }
196  return inner_product;
197 }
198 
200 {
201  updateVelocity();
203 }
204 
206 {
207  for (int i = 0; i < m_numNodes; ++i)
208  {
209  m_dv[i] += scale * m_ddv[i];
210  }
211 }
212 
214 {
215  if (m_useProjection)
216  m_cg.solve(*m_objective, ddv, residual, false);
217  else
218  m_cr.solve(*m_objective, ddv, residual, false);
219 }
220 
222 {
223  m_softBodies.copyFromArray(softBodies);
224  bool nodeUpdated = updateNodes();
225 
226  if (nodeUpdated)
227  {
228  m_dv.resize(m_numNodes, btVector3(0, 0, 0));
229  m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
232  }
233 
234  // need to setZero here as resize only set value for newly allocated items
235  for (int i = 0; i < m_numNodes; ++i)
236  {
237  m_dv[i].setZero();
238  m_ddv[i].setZero();
239  m_residual[i].setZero();
240  }
241 
242  if (dt > 0)
243  {
244  m_dt = dt;
245  }
246  m_objective->reinitialize(nodeUpdated, dt);
248 }
249 
251 {
252  BT_PROFILE("setConstraint");
253  m_objective->setConstraints(infoGlobal);
254 }
255 
256 btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
257 {
258  BT_PROFILE("solveContactConstraints");
259  btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies, numDeformableBodies, infoGlobal);
260  return maxSquaredResidual;
261 }
262 
264 {
265  int counter = 0;
266  for (int i = 0; i < m_softBodies.size(); ++i)
267  {
268  btSoftBody* psb = m_softBodies[i];
269  psb->m_maxSpeedSquared = 0;
270  if (!psb->isActive())
271  {
272  counter += psb->m_nodes.size();
273  continue;
274  }
275  for (int j = 0; j < psb->m_nodes.size(); ++j)
276  {
277  // set NaN to zero;
278  if (m_dv[counter] != m_dv[counter])
279  {
280  m_dv[counter].setZero();
281  }
282  if (m_implicit)
283  {
284  psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
285  }
286  else
287  {
288  psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
289  }
290  psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
291  ++counter;
292  }
293  }
294 }
295 
297 {
298  int counter = 0;
299  for (int i = 0; i < m_softBodies.size(); ++i)
300  {
301  btSoftBody* psb = m_softBodies[i];
302  if (!psb->isActive())
303  {
304  counter += psb->m_nodes.size();
305  continue;
306  }
307  for (int j = 0; j < psb->m_nodes.size(); ++j)
308  {
309  psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv);
310  ++counter;
311  }
312  psb->updateDeformation();
313  }
314 }
315 
317 {
318  int counter = 0;
319  for (int i = 0; i < m_softBodies.size(); ++i)
320  {
321  btSoftBody* psb = m_softBodies[i];
322  for (int j = 0; j < psb->m_nodes.size(); ++j)
323  {
324  m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
325  }
326  }
327 }
328 
330 {
331  int counter = 0;
332  for (int i = 0; i < m_softBodies.size(); ++i)
333  {
334  btSoftBody* psb = m_softBodies[i];
335  if (!psb->isActive())
336  {
337  counter += psb->m_nodes.size();
338  continue;
339  }
340  for (int j = 0; j < psb->m_nodes.size(); ++j)
341  {
342  if (implicit)
343  {
344  // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint.
345  if (psb->m_nodes[j].m_v == m_backupVelocity[counter])
346  m_dv[counter].setZero();
347  else
348  m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
349  m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
350  }
351  else
352  {
353  m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
354  }
355  psb->m_nodes[j].m_v = m_backupVelocity[counter];
356  ++counter;
357  }
358  }
359 }
360 
362 {
363  int counter = 0;
364  for (int i = 0; i < m_softBodies.size(); ++i)
365  {
366  btSoftBody* psb = m_softBodies[i];
367  for (int j = 0; j < psb->m_nodes.size(); ++j)
368  {
369  psb->m_nodes[j].m_v = m_backupVelocity[counter++];
370  }
371  }
372 }
373 
375 {
376  int numNodes = 0;
377  for (int i = 0; i < m_softBodies.size(); ++i)
378  numNodes += m_softBodies[i]->m_nodes.size();
379  if (numNodes != m_numNodes)
380  {
381  m_numNodes = numNodes;
382  return true;
383  }
384  return false;
385 }
386 
388 {
389  // apply explicit forces to velocity
390  if (m_implicit)
391  {
392  for (int i = 0; i < m_softBodies.size(); ++i)
393  {
394  btSoftBody* psb = m_softBodies[i];
395  if (psb->isActive())
396  {
397  for (int j = 0; j < psb->m_nodes.size(); ++j)
398  {
399  psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt;
400  }
401  }
402  }
403  }
405  for (int i = 0; i < m_softBodies.size(); ++i)
406  {
407  btSoftBody* psb = m_softBodies[i];
408  /* Clear contacts */
409  psb->m_nodeRigidContacts.resize(0);
410  psb->m_faceRigidContacts.resize(0);
411  psb->m_faceNodeContacts.resize(0);
412 
413  if (psb->isActive())
414  {
415  // predict motion for collision detection
416  predictDeformableMotion(psb, solverdt);
417  }
418  }
419 }
420 
422 {
423  BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
424  int i, ni;
425 
426  /* Update */
427  if (psb->m_bUpdateRtCst)
428  {
429  psb->m_bUpdateRtCst = false;
430  psb->updateConstants();
431  psb->m_fdbvt.clear();
433  {
434  psb->initializeFaceTree();
435  }
436  }
437 
438  /* Prepare */
439  psb->m_sst.sdt = dt * psb->m_cfg.timescale;
440  psb->m_sst.isdt = 1 / psb->m_sst.sdt;
441  psb->m_sst.velmrg = psb->m_sst.sdt * 3;
442  psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
443  psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
444  /* Bounds */
445  psb->updateBounds();
446 
447  /* Integrate */
448  // do not allow particles to move more than the bounding box size
449  btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
450  for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
451  {
452  btSoftBody::Node& n = psb->m_nodes[i];
453  // apply drag
454  n.m_v *= (1 - psb->m_cfg.drag);
455  // scale velocity back
456  if (m_implicit)
457  {
458  n.m_q = n.m_x;
459  }
460  else
461  {
462  if (n.m_v.norm() > max_v)
463  {
464  n.m_v.safeNormalize();
465  n.m_v *= max_v;
466  }
467  n.m_q = n.m_x + n.m_v * dt;
468  }
469  n.m_splitv.setZero();
470  n.m_constrained = false;
471  }
472 
473  /* Nodes */
474  psb->updateNodeTree(true, true);
475  if (!psb->m_fdbvt.empty())
476  {
477  psb->updateFaceTree(true, true);
478  }
479  /* Optimize dbvt's */
480  // psb->m_ndbvt.optimizeIncremental(1);
481  // psb->m_fdbvt.optimizeIncremental(1);
482 }
483 
485 {
486  BT_PROFILE("updateSoftBodies");
487  for (int i = 0; i < m_softBodies.size(); i++)
488  {
489  btSoftBody* psb = (btSoftBody*)m_softBodies[i];
490  if (psb->isActive())
491  {
492  psb->updateNormals();
493  }
494  }
495 }
496 
498 {
499  m_implicit = implicit;
500  m_objective->setImplicit(implicit);
501 }
502 
504 {
505  m_lineSearch = lineSearch;
506 }
static const int kMaxConjugateGradientIterations
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
virtual void reinitialize(bool nodeUpdated)=0
int size() const
return the number of elements in the array
void copyFromArray(const btAlignedObjectArray &otherArray)
void resize(int newsize, const T &fillData=T())
btCollisionObject can be used to manage collision detection objects.
const btCollisionShape * getCollisionShape() const
virtual btScalar getMargin() const =0
int solve(MatrixX &A, TVStack &x, const TVStack &b, bool verbose=false)
int solve(MatrixX &A, TVStack &x, const TVStack &b, bool verbose=false)
void setConstraints(const btContactSolverInfo &infoGlobal)
void computeResidual(btScalar dt, TVStack &residual)
void addLagrangeMultiplier(const TVStack &vec, TVStack &extended_vec)
btScalar computeNorm(const TVStack &residual) const
void reinitialize(bool nodeUpdated, btScalar dt)
void addLagrangeMultiplierRHS(const TVStack &residual, const TVStack &m_dv, TVStack &extended_residual)
virtual void updateSoftBodies()
Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes.
virtual void solveDeformableConstraints(btScalar solverdt)
void updateEnergy(btScalar scale)
void setConstraints(const btContactSolverInfo &infoGlobal)
btDeformableBackwardEulerObjective * m_objective
btScalar computeDescentStep(TVStack &ddv, const TVStack &residual, bool verbose=false)
void predictDeformableMotion(btSoftBody *psb, btScalar dt)
btConjugateResidual< btDeformableBackwardEulerObjective > m_cr
btConjugateGradient< btDeformableBackwardEulerObjective > m_cg
void reinitialize(const btAlignedObjectArray< btSoftBody * > &softBodies, btScalar dt)
void setupDeformableSolve(bool implicit)
void setLineSearch(bool lineSearch)
btAlignedObjectArray< btSoftBody * > m_softBodies
virtual btScalar solveContactConstraints(btCollisionObject **deformableBodies, int numDeformableBodies, const btContactSolverInfo &infoGlobal)
virtual void predictMotion(btScalar solverdt)
Predict motion of soft bodies into next timestep.
void computeStep(TVStack &ddv, const TVStack &residual)
void updateDv(btScalar scale=1)
virtual btScalar update(btCollisionObject **deformableBodies, int numDeformableBodies, const btContactSolverInfo &infoGlobal)
virtual btScalar dot(const TVStack &a, const TVStack &b)
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
bool m_bUpdateRtCst
Definition: btSoftBody.h:831
void updateFaceTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1285
SolverState m_sst
Definition: btSoftBody.h:807
void updateNodeTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1242
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:824
Config m_cfg
Definition: btSoftBody.h:806
void updateDeformation()
btVector3 m_bounds[2]
Definition: btSoftBody.h:830
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:839
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:825
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:823
void updateNormals()
btDbvt m_fdbvt
Definition: btSoftBody.h:833
tNodeArray m_nodes
Definition: btSoftBody.h:812
void updateConstants()
void updateBounds()
void initializeFaceTree()
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 & safeNormalize()
Definition: btVector3.h:286
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
void setZero()
Definition: btVector3.h:671
bool empty() const
Definition: btDbvt.h:314
void clear()
Definition: btDbvt.cpp:477
btScalar timescale
Definition: btSoftBody.h:729
btVector3 m_x
Definition: btSoftBody.h:269
btVector3 m_splitv
Definition: btSoftBody.h:281
btVector3 m_v
Definition: btSoftBody.h:271
btVector3 m_q
Definition: btSoftBody.h:270
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:166