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 = 50;
23 : m_numNodes(0)
25 , m_maxNewtonIterations(5)
26 , m_newtonTolerance(1e-4)
27 , m_lineSearch(false)
28 {
30 }
31 
33 {
34  delete m_objective;
35 }
36 
38 {
39  BT_PROFILE("solveDeformableConstraints");
40  if (!m_implicit)
41  {
46  }
47  else
48  {
49  for (int i = 0; i < m_maxNewtonIterations; ++i)
50  {
51  updateState();
52  // add the inertia term in the residual
53  int counter = 0;
54  for (int k = 0; k < m_softBodies.size(); ++k)
55  {
56  btSoftBody* psb = m_softBodies[k];
57  for (int j = 0; j < psb->m_nodes.size(); ++j)
58  {
59  if (psb->m_nodes[j].m_im > 0)
60  {
61  m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter];
62  }
63  ++counter;
64  }
65  }
66 
69  {
70  break;
71  }
72  // todo xuchenhan@: this really only needs to be calculated once
74  if (m_lineSearch)
75  {
77  btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
78  btScalar scale = 2;
79  btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
80  backupDv();
81  do {
82  scale *= beta;
83  if (scale < 1e-8) {
84  return;
85  }
86  updateEnergy(scale);
87  f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
88  f2 = f0 - alpha * scale * inner_product;
89  } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
90  revertDv();
91  updateDv(scale);
92  }
93  else
94  {
96  updateDv();
97  }
98  for (int j = 0; j < m_numNodes; ++j)
99  {
100  m_ddv[j].setZero();
101  m_residual[j].setZero();
102  }
103  }
104  updateVelocity();
105  }
106 }
107 
109 {
110  btScalar ke = 0;
111  for (int i = 0; i < m_softBodies.size();++i)
112  {
113  btSoftBody* psb = m_softBodies[i];
114  for (int j = 0; j < psb->m_nodes.size();++j)
115  {
116  btSoftBody::Node& node = psb->m_nodes[j];
117  if (node.m_im > 0)
118  {
119  ke += m_dv[node.index].length2() * 0.5 / node.m_im;
120  }
121  }
122  }
123  return ke;
124 }
125 
127 {
129  for (int i = 0; i<m_backup_dv.size(); ++i)
130  {
131  m_backup_dv[i] = m_dv[i];
132  }
133 }
134 
136 {
137  for (int i = 0; i<m_backup_dv.size(); ++i)
138  {
139  m_dv[i] = m_backup_dv[i];
140  }
141 }
142 
144 {
145  for (int i = 0; i<m_dv.size(); ++i)
146  {
147  m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
148  }
149  updateState();
150 }
151 
152 
154 {
155  m_cg.solve(*m_objective, ddv, residual, false);
156  btScalar inner_product = m_cg.dot(residual, m_ddv);
157  btScalar res_norm = m_objective->computeNorm(residual);
158  btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
159  if (inner_product < -tol)
160  {
161  if (verbose)
162  {
163  std::cout << "Looking backwards!" << std::endl;
164  }
165  for (int i = 0; i < m_ddv.size();++i)
166  {
167  m_ddv[i] = -m_ddv[i];
168  }
169  inner_product = -inner_product;
170  }
171  else if (std::abs(inner_product) < tol)
172  {
173  if (verbose)
174  {
175  std::cout << "Gradient Descent!" << std::endl;
176  }
177  btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
178  for (int i = 0; i < m_ddv.size();++i)
179  {
180  m_ddv[i] = scale * residual[i];
181  }
182  inner_product = scale * res_norm * res_norm;
183  }
184  return inner_product;
185 }
186 
188 {
189  updateVelocity();
191 }
192 
194 {
195  for (int i = 0; i < m_numNodes; ++i)
196  {
197  m_dv[i] += scale * m_ddv[i];
198  }
199 }
200 
202 {
203  m_cg.solve(*m_objective, ddv, residual);
204 }
205 
207 {
208  m_softBodies.copyFromArray(softBodies);
209  bool nodeUpdated = updateNodes();
210 
211  if (nodeUpdated)
212  {
213  m_dv.resize(m_numNodes, btVector3(0,0,0));
214  m_ddv.resize(m_numNodes, btVector3(0,0,0));
217  }
218 
219  // need to setZero here as resize only set value for newly allocated items
220  for (int i = 0; i < m_numNodes; ++i)
221  {
222  m_dv[i].setZero();
223  m_ddv[i].setZero();
224  m_residual[i].setZero();
225  }
226 
227  m_dt = dt;
228  m_objective->reinitialize(nodeUpdated, dt);
229 }
230 
232 {
233  BT_PROFILE("setConstraint");
235 }
236 
238 {
239  BT_PROFILE("solveContactConstraints");
240  btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
241  return maxSquaredResidual;
242 }
243 
245 {
246  BT_PROFILE("solveSplitImpulse");
247  return m_objective->m_projection.solveSplitImpulse(infoGlobal);
248 }
249 
251 {
253 }
254 
256 {
257  int counter = 0;
258  for (int i = 0; i < m_softBodies.size(); ++i)
259  {
260  btSoftBody* psb = m_softBodies[i];
261  psb->m_maxSpeedSquared = 0;
262  if (!psb->isActive())
263  {
264  counter += psb->m_nodes.size();
265  continue;
266  }
267  for (int j = 0; j < psb->m_nodes.size(); ++j)
268  {
269  // set NaN to zero;
270  if (m_dv[counter] != m_dv[counter])
271  {
272  m_dv[counter].setZero();
273  }
274  psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
275  psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
276  ++counter;
277  }
278  }
279 }
280 
282 {
283  int counter = 0;
284  for (int i = 0; i < m_softBodies.size(); ++i)
285  {
286  btSoftBody* psb = m_softBodies[i];
287  if (!psb->isActive())
288  {
289  counter += psb->m_nodes.size();
290  continue;
291  }
292  for (int j = 0; j < psb->m_nodes.size(); ++j)
293  {
294  psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
295  ++counter;
296  }
297  psb->updateDeformation();
298  }
299 }
300 
302 {
303  int counter = 0;
304  for (int i = 0; i < m_softBodies.size(); ++i)
305  {
306  btSoftBody* psb = m_softBodies[i];
307  for (int j = 0; j < psb->m_nodes.size(); ++j)
308  {
309  m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
310  }
311  }
312 }
313 
315 {
316  int counter = 0;
317  for (int i = 0; i < m_softBodies.size(); ++i)
318  {
319  btSoftBody* psb = m_softBodies[i];
320  if (!psb->isActive())
321  {
322  counter += psb->m_nodes.size();
323  continue;
324  }
325  for (int j = 0; j < psb->m_nodes.size(); ++j)
326  {
327  if (implicit)
328  {
329  if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
330  m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
331  else
332  m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
333  m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
334  }
335  else
336  m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
337  psb->m_nodes[j].m_v = m_backupVelocity[counter] + psb->m_nodes[j].m_vsplit;
338  ++counter;
339  }
340  }
341 }
342 
344 {
345  int counter = 0;
346  for (int i = 0; i < m_softBodies.size(); ++i)
347  {
348  btSoftBody* psb = m_softBodies[i];
349  for (int j = 0; j < psb->m_nodes.size(); ++j)
350  {
351  psb->m_nodes[j].m_v = m_backupVelocity[counter++];
352  }
353  }
354 }
355 
357 {
358  int numNodes = 0;
359  for (int i = 0; i < m_softBodies.size(); ++i)
360  numNodes += m_softBodies[i]->m_nodes.size();
361  if (numNodes != m_numNodes)
362  {
363  m_numNodes = numNodes;
364  return true;
365  }
366  return false;
367 }
368 
369 
371 {
372  // apply explicit forces to velocity
374  for (int i = 0; i < m_softBodies.size(); ++i)
375  {
376  btSoftBody *psb = m_softBodies[i];
377 
378  if (psb->isActive())
379  {
380  // predict motion for collision detection
381  predictDeformableMotion(psb, solverdt);
382  }
383  }
384 }
385 
387 {
388  int i, ni;
389 
390  /* Update */
391  if (psb->m_bUpdateRtCst)
392  {
393  psb->m_bUpdateRtCst = false;
394  psb->updateConstants();
395  psb->m_fdbvt.clear();
397  {
398  psb->initializeFaceTree();
399  }
400  }
401 
402  /* Prepare */
403  psb->m_sst.sdt = dt * psb->m_cfg.timescale;
404  psb->m_sst.isdt = 1 / psb->m_sst.sdt;
405  psb->m_sst.velmrg = psb->m_sst.sdt * 3;
406  psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
407  psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
408  /* Bounds */
409  psb->updateBounds();
410 
411  /* Integrate */
412  // do not allow particles to move more than the bounding box size
413  btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt;
414  for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
415  {
416  btSoftBody::Node& n = psb->m_nodes[i];
417  // apply drag
418  n.m_v *= (1 - psb->m_cfg.drag);
419  // scale velocity back
420  if (n.m_v.norm() > max_v)
421  {
422  n.m_v.safeNormalize();
423  n.m_v *= max_v;
424  }
425  n.m_q = n.m_x + n.m_v * dt;
426  }
427 
428  /* Nodes */
430  vol;
431  for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
432  {
433  btSoftBody::Node& n = psb->m_nodes[i];
434  btVector3 points[2] = {n.m_x, n.m_q};
435  vol = btDbvtVolume::FromPoints(points, 2);
436  vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
437  psb->m_ndbvt.update(n.m_leaf, vol);
438  }
439 
440  if (!psb->m_fdbvt.empty())
441  {
442  for (int i = 0; i < psb->m_faces.size(); ++i)
443  {
444  btSoftBody::Face& f = psb->m_faces[i];
445  btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q,
446  f.m_n[1]->m_x, f.m_n[1]->m_q,
447  f.m_n[2]->m_x, f.m_n[2]->m_q};
448  vol = btDbvtVolume::FromPoints(points, 6);
449  vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
450  psb->m_fdbvt.update(f.m_leaf, vol);
451  }
452  }
453  /* Clear contacts */
454  psb->m_nodeRigidContacts.resize(0);
455  psb->m_faceRigidContacts.resize(0);
456  psb->m_faceNodeContacts.resize(0);
457  /* Optimize dbvt's */
460 }
461 
462 
464 {
465  BT_PROFILE("updateSoftBodies");
466  for (int i = 0; i < m_softBodies.size(); i++)
467  {
468  btSoftBody *psb = (btSoftBody *)m_softBodies[i];
469  if (psb->isActive())
470  {
471  psb->updateNormals();
472  }
473  }
474 }
475 
477 {
478  m_implicit = implicit;
479  m_objective->setImplicit(implicit);
480 }
481 
483 {
484  m_lineSearch = lineSearch;
485 }
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 ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
#define SIMD_EPSILON
Definition: btScalar.h:543
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
btScalar dot(const TVStack &a, const TVStack &b)
int solve(MatrixX &A, TVStack &x, const TVStack &b, bool verbose=false)
void computeResidual(btScalar dt, TVStack &residual)
btScalar computeNorm(const TVStack &residual) const
void reinitialize(bool nodeUpdated, btScalar dt)
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)
btDeformableBackwardEulerObjective * m_objective
btAlignedObjectArray< btSoftBody * > m_softBodies
void splitImpulseSetup(const btContactSolverInfo &infoGlobal)
btScalar computeDescentStep(TVStack &ddv, const TVStack &residual, bool verbose=false)
void predictDeformableMotion(btSoftBody *psb, btScalar dt)
btConjugateGradient< btDeformableBackwardEulerObjective > m_cg
btScalar solveContactConstraints(btCollisionObject **deformableBodies, int numDeformableBodies)
void reinitialize(const btAlignedObjectArray< btSoftBody * > &softBodies, btScalar dt)
void setupDeformableSolve(bool implicit)
void setLineSearch(bool lineSearch)
virtual void predictMotion(btScalar solverdt)
Predict motion of soft bodies into next timestep.
void computeStep(TVStack &ddv, const TVStack &residual)
btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
void updateDv(btScalar scale=1)
virtual btScalar update(btCollisionObject **deformableBodies, int numDeformableBodies)
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
virtual void splitImpulseSetup(const btContactSolverInfo &infoGlobal)
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
bool m_bUpdateRtCst
Definition: btSoftBody.h:796
SolverState m_sst
Definition: btSoftBody.h:772
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:789
Config m_cfg
Definition: btSoftBody.h:771
tFaceArray m_faces
Definition: btSoftBody.h:780
void updateDeformation()
btVector3 m_bounds[2]
Definition: btSoftBody.h:795
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:803
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:790
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:788
void updateNormals()
btDbvt m_fdbvt
Definition: btSoftBody.h:798
tNodeArray m_nodes
Definition: btSoftBody.h:777
void updateConstants()
void updateBounds()
void initializeFaceTree()
btDbvt m_ndbvt
Definition: btSoftBody.h:797
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
static btDbvtAabbMm FromPoints(const btVector3 *pts, int n)
Definition: btDbvt.h:488
void optimizeIncremental(int passes)
Definition: btDbvt.cpp:514
void update(btDbvtNode *leaf, int lookahead=-1)
Definition: btDbvt.cpp:544
bool empty() const
Definition: btDbvt.h:314
void clear()
Definition: btDbvt.cpp:477
btScalar timescale
Definition: btSoftBody.h:705
Node * m_n[3]
Definition: btSoftBody.h:287
btDbvtNode * m_leaf
Definition: btSoftBody.h:290
btVector3 m_x
Definition: btSoftBody.h:257
btVector3 m_v
Definition: btSoftBody.h:259
btVector3 m_q
Definition: btSoftBody.h:258
btDbvtNode * m_leaf
Definition: btSoftBody.h:266
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:164