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 = 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 {
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 }
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{
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 {
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 {
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