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;
btDeformableBodySolver()22 btDeformableBodySolver::btDeformableBodySolver()
23 : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
24 {
25 m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
26 }
27
~btDeformableBodySolver()28 btDeformableBodySolver::~btDeformableBodySolver()
29 {
30 delete m_objective;
31 }
32
solveDeformableConstraints(btScalar solverdt)33 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
34 {
35 BT_PROFILE("solveDeformableConstraints");
36 if (!m_implicit)
37 {
38 m_objective->computeResidual(solverdt, m_residual);
39 m_objective->applyDynamicFriction(m_residual);
40 if (m_useProjection)
41 {
42 computeStep(m_dv, m_residual);
43 }
44 else
45 {
46 TVStack rhs, x;
47 m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs);
48 m_objective->addLagrangeMultiplier(m_dv, x);
49 m_objective->m_preconditioner->reinitialize(true);
50 computeStep(x, rhs);
51 for (int i = 0; i < m_dv.size(); ++i)
52 {
53 m_dv[i] = x[i];
54 }
55 }
56 updateVelocity();
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
78 m_objective->computeResidual(solverdt, m_residual);
79 if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
80 {
81 break;
82 }
83 // todo xuchenhan@: this really only needs to be calculated once
84 m_objective->applyDynamicFriction(m_residual);
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 {
108 computeStep(m_ddv, m_residual);
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
kineticEnergy()121 btScalar btDeformableBodySolver::kineticEnergy()
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
backupDv()139 void btDeformableBodySolver::backupDv()
140 {
141 m_backup_dv.resize(m_dv.size());
142 for (int i = 0; i < m_backup_dv.size(); ++i)
143 {
144 m_backup_dv[i] = m_dv[i];
145 }
146 }
147
revertDv()148 void btDeformableBodySolver::revertDv()
149 {
150 for (int i = 0; i < m_backup_dv.size(); ++i)
151 {
152 m_dv[i] = m_backup_dv[i];
153 }
154 }
155
updateEnergy(btScalar scale)156 void btDeformableBodySolver::updateEnergy(btScalar scale)
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
computeDescentStep(TVStack & ddv,const TVStack & residual,bool verbose)165 btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
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
updateState()199 void btDeformableBodySolver::updateState()
200 {
201 updateVelocity();
202 updateTempPosition();
203 }
204
updateDv(btScalar scale)205 void btDeformableBodySolver::updateDv(btScalar scale)
206 {
207 for (int i = 0; i < m_numNodes; ++i)
208 {
209 m_dv[i] += scale * m_ddv[i];
210 }
211 }
212
computeStep(TVStack & ddv,const TVStack & residual)213 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
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
reinitialize(const btAlignedObjectArray<btSoftBody * > & softBodies,btScalar dt)221 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt)
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));
230 m_residual.resize(m_numNodes, btVector3(0, 0, 0));
231 m_backupVelocity.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);
247 updateSoftBodies();
248 }
249
setConstraints(const btContactSolverInfo & infoGlobal)250 void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
251 {
252 BT_PROFILE("setConstraint");
253 m_objective->setConstraints(infoGlobal);
254 }
255
solveContactConstraints(btCollisionObject ** deformableBodies,int numDeformableBodies,const btContactSolverInfo & infoGlobal)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
updateVelocity()263 void btDeformableBodySolver::updateVelocity()
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
updateTempPosition()296 void btDeformableBodySolver::updateTempPosition()
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
backupVelocity()316 void btDeformableBodySolver::backupVelocity()
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
setupDeformableSolve(bool implicit)329 void btDeformableBodySolver::setupDeformableSolve(bool implicit)
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
revertVelocity()361 void btDeformableBodySolver::revertVelocity()
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
updateNodes()374 bool btDeformableBodySolver::updateNodes()
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
predictMotion(btScalar solverdt)387 void btDeformableBodySolver::predictMotion(btScalar solverdt)
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 }
404 m_objective->applyExplicitForce(m_residual);
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
predictDeformableMotion(btSoftBody * psb,btScalar dt)417 void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
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();
428 if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
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
updateSoftBodies()484 void btDeformableBodySolver::updateSoftBodies()
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
setImplicit(bool implicit)497 void btDeformableBodySolver::setImplicit(bool implicit)
498 {
499 m_implicit = implicit;
500 m_objective->setImplicit(implicit);
501 }
502
setLineSearch(bool lineSearch)503 void btDeformableBodySolver::setLineSearch(bool lineSearch)
504 {
505 m_lineSearch = lineSearch;
506 }
507