Blender  V3.3
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");
254 }
255 
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 
409  if (psb->isActive())
410  {
411  // predict motion for collision detection
412  predictDeformableMotion(psb, solverdt);
413  }
414  }
415 }
416 
418 {
419  BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
420  int i, ni;
421 
422  /* Update */
423  if (psb->m_bUpdateRtCst)
424  {
425  psb->m_bUpdateRtCst = false;
426  psb->updateConstants();
427  psb->m_fdbvt.clear();
429  {
430  psb->initializeFaceTree();
431  }
432  }
433 
434  /* Prepare */
435  psb->m_sst.sdt = dt * psb->m_cfg.timescale;
436  psb->m_sst.isdt = 1 / psb->m_sst.sdt;
437  psb->m_sst.velmrg = psb->m_sst.sdt * 3;
438  psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
439  psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
440  /* Bounds */
441  psb->updateBounds();
442 
443  /* Integrate */
444  // do not allow particles to move more than the bounding box size
445  btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
446  for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
447  {
448  btSoftBody::Node& n = psb->m_nodes[i];
449  // apply drag
450  n.m_v *= (1 - psb->m_cfg.drag);
451  // scale velocity back
452  if (m_implicit)
453  {
454  n.m_q = n.m_x;
455  }
456  else
457  {
458  if (n.m_v.norm() > max_v)
459  {
460  n.m_v.safeNormalize();
461  n.m_v *= max_v;
462  }
463  n.m_q = n.m_x + n.m_v * dt;
464  }
465  n.m_splitv.setZero();
466  n.m_constrained = false;
467  }
468 
469  /* Nodes */
470  psb->updateNodeTree(true, true);
471  if (!psb->m_fdbvt.empty())
472  {
473  psb->updateFaceTree(true, true);
474  }
475  /* Clear contacts */
476  psb->m_nodeRigidContacts.resize(0);
477  psb->m_faceRigidContacts.resize(0);
478  psb->m_faceNodeContacts.resize(0);
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 }
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
btCollisionObject
static const int kMaxConjugateGradientIterations
SIMD_FORCE_INLINE 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
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
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
static int verbose
Definition: cineonlib.c:29
virtual void reinitialize(bool nodeUpdated)=0
void copyFromArray(const btAlignedObjectArray &otherArray)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
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 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)
void computeStep(TVStack &ddv, const TVStack &residual)
void updateDv(btScalar scale=1)
virtual btScalar update(btCollisionObject **deformableBodies, int numDeformableBodies, const btContactSolverInfo &infoGlobal)
virtual SIMD_FORCE_INLINE btScalar dot(const TVStack &a, const TVStack &b)
bool m_bUpdateRtCst
Definition: btSoftBody.h:818
void updateFaceTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1270
SolverState m_sst
Definition: btSoftBody.h:794
void updateNodeTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1227
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:811
Config m_cfg
Definition: btSoftBody.h:793
void updateDeformation()
btVector3 m_bounds[2]
Definition: btSoftBody.h:817
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:826
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:812
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:810
void updateNormals()
btDbvt m_fdbvt
Definition: btSoftBody.h:820
tNodeArray m_nodes
Definition: btSoftBody.h:799
void updateConstants()
void updateBounds()
void initializeFaceTree()
OperationNode * node
ccl_gpu_kernel_postfix ccl_global int * counter
T abs(const T &a)
bool empty() const
Definition: btDbvt.h:314
void clear()
Definition: btDbvt.cpp:477
btScalar timescale
Definition: btSoftBody.h:718
btVector3 m_x
Definition: btSoftBody.h:263
btVector3 m_splitv
Definition: btSoftBody.h:275
btVector3 m_v
Definition: btSoftBody.h:265
btVector3 m_q
Definition: btSoftBody.h:264
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:166
ccl_device_inline float beta(float x, float y)
Definition: util/math.h:775