Blender  V3.3
btDeformableLinearElasticityForce.h
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 #ifndef BT_LINEAR_ELASTICITY_H
17 #define BT_LINEAR_ELASTICITY_H
18 
20 #include "LinearMath/btQuickprof.h"
21 #include "btSoftBodyInternals.h"
22 #define TETRA_FLAT_THRESHOLD 0.01
24 {
25 public:
28  btScalar m_E, m_nu; // Young's modulus and Poisson ratio
31  {
33  }
34 
35  btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta)
36  {
38  }
39 
41  {
42  // conversion from Lame Parameters to Young's modulus and Poisson ratio
43  // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
44  m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
45  m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
46  }
47 
49  {
50  // conversion from Young's modulus and Poisson ratio to Lame Parameters
51  // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
52  m_mu = m_E * 0.5 / (1 + m_nu);
53  m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
54  }
55 
57  {
58  m_E = E;
60  }
61 
63  {
64  m_nu = nu;
66  }
67 
68  void setDamping(btScalar damping_alpha, btScalar damping_beta)
69  {
70  m_damping_alpha = damping_alpha;
71  m_damping_beta = damping_beta;
72  }
73 
75  {
76  m_mu = mu;
77  m_lambda = lambda;
79  }
80 
81  virtual void addScaledForces(btScalar scale, TVStack& force)
82  {
83  addScaledDampingForce(scale, force);
84  addScaledElasticForce(scale, force);
85  }
86 
87  virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
88  {
89  addScaledElasticForce(scale, force);
90  }
91 
92  // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
93  virtual void addScaledDampingForce(btScalar scale, TVStack& force)
94  {
95  if (m_damping_alpha == 0 && m_damping_beta == 0)
96  return;
97  btScalar mu_damp = m_damping_beta * m_mu;
98  btScalar lambda_damp = m_damping_beta * m_lambda;
99  int numNodes = getNumNodes();
100  btAssert(numNodes <= force.size());
101  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
102  for (int i = 0; i < m_softBodies.size(); ++i)
103  {
104  btSoftBody* psb = m_softBodies[i];
105  if (!psb->isActive())
106  {
107  continue;
108  }
109  for (int j = 0; j < psb->m_tetras.size(); ++j)
110  {
111  bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
112  btSoftBody::Tetra& tetra = psb->m_tetras[j];
113  btSoftBody::Node* node0 = tetra.m_n[0];
114  btSoftBody::Node* node1 = tetra.m_n[1];
115  btSoftBody::Node* node2 = tetra.m_n[2];
116  btSoftBody::Node* node3 = tetra.m_n[3];
117  size_t id0 = node0->index;
118  size_t id1 = node1->index;
119  size_t id2 = node2->index;
120  size_t id3 = node3->index;
121  btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
122  if (!close_to_flat)
123  {
124  dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
125  }
126  btMatrix3x3 I;
127  I.setIdentity();
128  btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
129  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
130  if (!close_to_flat)
131  {
132  df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
133  }
134  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
135  // damping force differential
136  btScalar scale1 = scale * tetra.m_element_measure;
137  force[id0] -= scale1 * df_on_node0;
138  force[id1] -= scale1 * df_on_node123.getColumn(0);
139  force[id2] -= scale1 * df_on_node123.getColumn(1);
140  force[id3] -= scale1 * df_on_node123.getColumn(2);
141  }
142  for (int j = 0; j < psb->m_nodes.size(); ++j)
143  {
144  const btSoftBody::Node& node = psb->m_nodes[j];
145  size_t id = node.index;
146  if (node.m_im > 0)
147  {
148  force[id] -= scale * node.m_v / node.m_im * m_damping_alpha;
149  }
150  }
151  }
152  }
153 
154  virtual double totalElasticEnergy(btScalar dt)
155  {
156  double energy = 0;
157  for (int i = 0; i < m_softBodies.size(); ++i)
158  {
159  btSoftBody* psb = m_softBodies[i];
160  if (!psb->isActive())
161  {
162  continue;
163  }
164  for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
165  {
166  btSoftBody::Tetra& tetra = psb->m_tetras[j];
168  energy += tetra.m_element_measure * elasticEnergyDensity(s);
169  }
170  }
171  return energy;
172  }
173 
174  // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
175  virtual double totalDampingEnergy(btScalar dt)
176  {
177  double energy = 0;
178  int sz = 0;
179  for (int i = 0; i < m_softBodies.size(); ++i)
180  {
181  btSoftBody* psb = m_softBodies[i];
182  if (!psb->isActive())
183  {
184  continue;
185  }
186  for (int j = 0; j < psb->m_nodes.size(); ++j)
187  {
188  sz = btMax(sz, psb->m_nodes[j].index);
189  }
190  }
191  TVStack dampingForce;
192  dampingForce.resize(sz + 1);
193  for (int i = 0; i < dampingForce.size(); ++i)
194  dampingForce[i].setZero();
195  addScaledDampingForce(0.5, dampingForce);
196  for (int i = 0; i < m_softBodies.size(); ++i)
197  {
198  btSoftBody* psb = m_softBodies[i];
199  for (int j = 0; j < psb->m_nodes.size(); ++j)
200  {
201  const btSoftBody::Node& node = psb->m_nodes[j];
202  energy -= dampingForce[node.index].dot(node.m_v) / dt;
203  }
204  }
205  return energy;
206  }
207 
209  {
210  double density = 0;
211  btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
212  btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
213  density += m_mu * (epsilon[0].length2() + epsilon[1].length2() + epsilon[2].length2());
214  density += m_lambda * trace * trace * 0.5;
215  return density;
216  }
217 
218  virtual void addScaledElasticForce(btScalar scale, TVStack& force)
219  {
220  int numNodes = getNumNodes();
221  btAssert(numNodes <= force.size());
222  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
223  for (int i = 0; i < m_softBodies.size(); ++i)
224  {
225  btSoftBody* psb = m_softBodies[i];
226  if (!psb->isActive())
227  {
228  continue;
229  }
230  btScalar max_p = psb->m_cfg.m_maxStress;
231  for (int j = 0; j < psb->m_tetras.size(); ++j)
232  {
233  btSoftBody::Tetra& tetra = psb->m_tetras[j];
234  btMatrix3x3 P;
235  firstPiola(psb->m_tetraScratches[j], P);
236 #if USE_SVD
237  if (max_p > 0)
238  {
239  // since we want to clamp the principal stress to max_p, we only need to
240  // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
241  btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
242  if (trPTP > max_p * max_p)
243  {
244  btMatrix3x3 U, V;
245  btVector3 sigma;
246  singularValueDecomposition(P, U, sigma, V);
247  sigma[0] = btMin(sigma[0], max_p);
248  sigma[1] = btMin(sigma[1], max_p);
249  sigma[2] = btMin(sigma[2], max_p);
250  sigma[0] = btMax(sigma[0], -max_p);
251  sigma[1] = btMax(sigma[1], -max_p);
252  sigma[2] = btMax(sigma[2], -max_p);
253  btMatrix3x3 Sigma;
254  Sigma.setIdentity();
255  Sigma[0][0] = sigma[0];
256  Sigma[1][1] = sigma[1];
257  Sigma[2][2] = sigma[2];
258  P = U * Sigma * V.transpose();
259  }
260  }
261 #endif
262  // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
263  btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
264  btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
265 
266  btSoftBody::Node* node0 = tetra.m_n[0];
267  btSoftBody::Node* node1 = tetra.m_n[1];
268  btSoftBody::Node* node2 = tetra.m_n[2];
269  btSoftBody::Node* node3 = tetra.m_n[3];
270  size_t id0 = node0->index;
271  size_t id1 = node1->index;
272  size_t id2 = node2->index;
273  size_t id3 = node3->index;
274 
275  // elastic force
276  btScalar scale1 = scale * tetra.m_element_measure;
277  force[id0] -= scale1 * force_on_node0;
278  force[id1] -= scale1 * force_on_node123.getColumn(0);
279  force[id2] -= scale1 * force_on_node123.getColumn(1);
280  force[id3] -= scale1 * force_on_node123.getColumn(2);
281  }
282  }
283  }
284 
286 
287  // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
288  virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
289  {
290  if (m_damping_alpha == 0 && m_damping_beta == 0)
291  return;
292  btScalar mu_damp = m_damping_beta * m_mu;
293  btScalar lambda_damp = m_damping_beta * m_lambda;
294  int numNodes = getNumNodes();
295  btAssert(numNodes <= df.size());
296  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
297  for (int i = 0; i < m_softBodies.size(); ++i)
298  {
299  btSoftBody* psb = m_softBodies[i];
300  if (!psb->isActive())
301  {
302  continue;
303  }
304  for (int j = 0; j < psb->m_tetras.size(); ++j)
305  {
306  bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD);
307  btSoftBody::Tetra& tetra = psb->m_tetras[j];
308  btSoftBody::Node* node0 = tetra.m_n[0];
309  btSoftBody::Node* node1 = tetra.m_n[1];
310  btSoftBody::Node* node2 = tetra.m_n[2];
311  btSoftBody::Node* node3 = tetra.m_n[3];
312  size_t id0 = node0->index;
313  size_t id1 = node1->index;
314  size_t id2 = node2->index;
315  size_t id3 = node3->index;
316  btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
317  if (!close_to_flat)
318  {
319  dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF;
320  }
321  btMatrix3x3 I;
322  I.setIdentity();
323  btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp);
324  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
325  if (!close_to_flat)
326  {
327  df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123;
328  }
329  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
330 
331  // damping force differential
332  btScalar scale1 = scale * tetra.m_element_measure;
333  df[id0] -= scale1 * df_on_node0;
334  df[id1] -= scale1 * df_on_node123.getColumn(0);
335  df[id2] -= scale1 * df_on_node123.getColumn(1);
336  df[id3] -= scale1 * df_on_node123.getColumn(2);
337  }
338  for (int j = 0; j < psb->m_nodes.size(); ++j)
339  {
340  const btSoftBody::Node& node = psb->m_nodes[j];
341  size_t id = node.index;
342  if (node.m_im > 0)
343  {
344  df[id] -= scale * dv[id] / node.m_im * m_damping_alpha;
345  }
346  }
347  }
348  }
349 
350  virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
351  {
352  int numNodes = getNumNodes();
353  btAssert(numNodes <= df.size());
354  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
355  for (int i = 0; i < m_softBodies.size(); ++i)
356  {
357  btSoftBody* psb = m_softBodies[i];
358  if (!psb->isActive())
359  {
360  continue;
361  }
362  for (int j = 0; j < psb->m_tetras.size(); ++j)
363  {
364  btSoftBody::Tetra& tetra = psb->m_tetras[j];
365  btSoftBody::Node* node0 = tetra.m_n[0];
366  btSoftBody::Node* node1 = tetra.m_n[1];
367  btSoftBody::Node* node2 = tetra.m_n[2];
368  btSoftBody::Node* node3 = tetra.m_n[3];
369  size_t id0 = node0->index;
370  size_t id1 = node1->index;
371  size_t id2 = node2->index;
372  size_t id3 = node3->index;
373  btMatrix3x3 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
374  btMatrix3x3 dP;
375  firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
376  // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
377  btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose();
378  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
379 
380  // elastic force differential
381  btScalar scale1 = scale * tetra.m_element_measure;
382  df[id0] -= scale1 * df_on_node0;
383  df[id1] -= scale1 * df_on_node123.getColumn(0);
384  df[id2] -= scale1 * df_on_node123.getColumn(1);
385  df[id3] -= scale1 * df_on_node123.getColumn(2);
386  }
387  }
388  }
389 
391  {
392  btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F;
393 
394  btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity();
395  btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2];
396  P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
397  }
398 
399  // Let P be the first piola stress.
400  // This function calculates the dP = dP/dF * dF
402  {
403  btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
404  dP = (dF + dF.transpose()) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace;
405  }
406 
407  // Let Q be the damping stress.
408  // This function calculates the dP = dQ/dF * dF
410  {
411  btScalar mu_damp = m_damping_beta * m_mu;
412  btScalar lambda_damp = m_damping_beta * m_lambda;
413  btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]);
414  dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace;
415  }
416 
417  virtual void addScaledHessian(btScalar scale)
418  {
419  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
420  for (int i = 0; i < m_softBodies.size(); ++i)
421  {
422  btSoftBody* psb = m_softBodies[i];
423  if (!psb->isActive())
424  {
425  continue;
426  }
427  for (int j = 0; j < psb->m_tetras.size(); ++j)
428  {
429  btSoftBody::Tetra& tetra = psb->m_tetras[j];
430  btMatrix3x3 P;
431  firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn
432  btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose();
433  btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
434  btSoftBody::Node* node0 = tetra.m_n[0];
435  btSoftBody::Node* node1 = tetra.m_n[1];
436  btSoftBody::Node* node2 = tetra.m_n[2];
437  btSoftBody::Node* node3 = tetra.m_n[3];
438  btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms;
439  node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1;
440  node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1;
441  node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1;
442  node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1;
443  }
444  for (int j = 0; j < psb->m_nodes.size(); ++j)
445  {
446  btSoftBody::Node& node = psb->m_nodes[j];
447  if (node.m_im > 0)
448  {
449  btMatrix3x3 I;
450  I.setIdentity();
451  node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha);
452  }
453  }
454  }
455  }
456 
458  {
460  }
461 };
462 #endif /* BT_LINEAR_ELASTICITY_H */
#define U
btDeformableLagrangianForceType
@ BT_LINEAR_ELASTICITY_FORCE
#define TETRA_FLAT_THRESHOLD
unsigned int U
Definition: btGjkEpa3.h:78
void singularValueDecomposition(const btMatrix2x2 &A, GivensRotation &U, const btMatrix2x2 &Sigma, GivensRotation &V, const btScalar tol=64 *std::numeric_limits< btScalar >::epsilon())
2x2 SVD (singular value decomposition) A=USV'
void setZero()
Set the matrix to the identity.
Definition: btMatrix3x3.h:337
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
static const btMatrix3x3 & getIdentity()
Definition: btMatrix3x3.h:350
SIMD_FORCE_INLINE const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:295
static btMatrix3x3 OuterProduct(const btScalar *v1, const btScalar *v2, const btScalar *v3, const btScalar *u1, const btScalar *u2, const btScalar *u3, int ndof)
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
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())
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack &dx)
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node *n0, const btSoftBody::Node *n1, const btSoftBody::Node *n2, const btSoftBody::Node *n3)
btAlignedObjectArray< btSoftBody * > m_softBodies
virtual btDeformableLagrangianForceType getForceType()
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack &diagA)
void setDamping(btScalar damping_alpha, btScalar damping_beta)
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha=0.01, btScalar damping_beta=0.01)
virtual void addScaledElasticForce(btScalar scale, TVStack &force)
void firstPiola(const btSoftBody::TetraScratch &s, btMatrix3x3 &P)
virtual void addScaledForces(btScalar scale, TVStack &force)
double elasticEnergyDensity(const btSoftBody::TetraScratch &s)
virtual void addScaledDampingForce(btScalar scale, TVStack &force)
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)
void firstPiolaDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
void setLameParameters(btScalar mu, btScalar lambda)
tTetraArray m_tetras
Definition: btSoftBody.h:804
Config m_cfg
Definition: btSoftBody.h:793
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition: btSoftBody.h:805
tNodeArray m_nodes
Definition: btSoftBody.h:799
OperationNode * node
static float P(float k)
Definition: math_interp.c:25
static double epsilon
static const pxr::TfToken density("density", pxr::TfToken::Immortal)
#define I
btScalar m_maxStress
Definition: btSoftBody.h:728
btMatrix3x3 m_effectiveMass
Definition: btSoftBody.h:276
btMatrix3x3 m_corotation
Definition: btSoftBody.h:326
btScalar m_element_measure
Definition: btSoftBody.h:315
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:313
CCL_NAMESPACE_BEGIN struct Window V