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>
19#include "btSoftBodyInternals.h"
21static 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 {
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 {
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 {
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 }
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{
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
256btScalar 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 if (psb->isActive())
409 {
410 /* Clear contacts when softbody is active*/
411 psb->m_nodeRigidContacts.resize(0);
412 psb->m_faceRigidContacts.resize(0);
413 psb->m_faceNodeContacts.resize(0);
414 // predict motion for collision detection
415 predictDeformableMotion(psb, solverdt);
416 }
417 }
418}
419
421{
422 BT_PROFILE("btDeformableBodySolver::predictDeformableMotion");
423 int i, ni;
424
425 /* Update */
426 if (psb->m_bUpdateRtCst)
427 {
428 psb->m_bUpdateRtCst = false;
429 psb->updateConstants();
430 psb->m_fdbvt.clear();
432 {
433 psb->initializeFaceTree();
434 }
435 }
436
437 /* Prepare */
438 psb->m_sst.sdt = dt * psb->m_cfg.timescale;
439 psb->m_sst.isdt = 1 / psb->m_sst.sdt;
440 psb->m_sst.velmrg = psb->m_sst.sdt * 3;
441 psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
442 psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
443 /* Bounds */
444 psb->updateBounds();
445
446 /* Integrate */
447 // do not allow particles to move more than the bounding box size
448 btScalar max_v = (psb->m_bounds[1] - psb->m_bounds[0]).norm() / dt;
449 for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
450 {
451 btSoftBody::Node& n = psb->m_nodes[i];
452 // apply drag
453 n.m_v *= (1 - psb->m_cfg.drag);
454 // scale velocity back
455 if (m_implicit)
456 {
457 n.m_q = n.m_x;
458 }
459 else
460 {
461 if (n.m_v.norm() > max_v)
462 {
463 n.m_v.safeNormalize();
464 n.m_v *= max_v;
465 }
466 n.m_q = n.m_x + n.m_v * dt;
467 }
468 n.m_splitv.setZero();
469 n.m_constrained = false;
470 }
471
472 /* Nodes */
473 psb->updateNodeTree(true, true);
474 if (!psb->m_fdbvt.empty())
475 {
476 psb->updateFaceTree(true, true);
477 }
478 /* Optimize dbvt's */
479 // psb->m_ndbvt.optimizeIncremental(1);
480 // psb->m_fdbvt.optimizeIncremental(1);
481}
482
484{
485 BT_PROFILE("updateSoftBodies");
486 for (int i = 0; i < m_softBodies.size(); i++)
487 {
489 if (psb->isActive())
490 {
491 psb->updateNormals();
492 }
493 }
494}
495
497{
498 m_implicit = implicit;
499 m_objective->setImplicit(implicit);
500}
501
503{
504 m_lineSearch = lineSearch;
505}
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
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)
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
bool m_bUpdateRtCst
Definition: btSoftBody.h:833
void updateFaceTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1287
SolverState m_sst
Definition: btSoftBody.h:809
void updateNodeTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1244
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:826
Config m_cfg
Definition: btSoftBody.h:808
void updateDeformation()
btVector3 m_bounds[2]
Definition: btSoftBody.h:832
btScalar m_maxSpeedSquared
Definition: btSoftBody.h:841
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:827
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:825
void updateNormals()
btDbvt m_fdbvt
Definition: btSoftBody.h:835
tNodeArray m_nodes
Definition: btSoftBody.h:814
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:731
btVector3 m_x
Definition: btSoftBody.h:271
btVector3 m_splitv
Definition: btSoftBody.h:283
btVector3 m_v
Definition: btSoftBody.h:273
btVector3 m_q
Definition: btSoftBody.h:272
@ SDF_RD
Cluster vs convex rigid vs soft.
Definition: btSoftBody.h:166