Blender  V3.3
btDeformableNeoHookeanForce.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_NEOHOOKEAN_H
17 #define BT_NEOHOOKEAN_H
18 
20 #include "LinearMath/btQuickprof.h"
22 // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
24 {
25 public:
27  btScalar m_mu, m_lambda; // Lame Parameters
28  btScalar m_E, m_nu; // Young's modulus and Poisson ratio
31  {
32  btScalar damping = 0.05;
33  m_mu_damp = damping * m_mu;
34  m_lambda_damp = damping * m_lambda;
36  }
37 
38  btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda)
39  {
40  m_mu_damp = damping * m_mu;
41  m_lambda_damp = damping * m_lambda;
43  }
44 
46  {
47  // conversion from Lame Parameters to Young's modulus and Poisson ratio
48  // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
49  m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu);
50  m_nu = m_lambda * 0.5 / (m_mu + m_lambda);
51  }
52 
54  {
55  // conversion from Young's modulus and Poisson ratio to Lame Parameters
56  // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters
57  m_mu = m_E * 0.5 / (1 + m_nu);
58  m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu));
59  }
60 
62  {
63  m_E = E;
65  }
66 
68  {
69  m_nu = nu;
71  }
72 
73  void setDamping(btScalar damping)
74  {
75  m_mu_damp = damping * m_mu;
76  m_lambda_damp = damping * m_lambda;
77  }
78 
80  {
81  m_mu = mu;
82  m_lambda = lambda;
84  }
85 
86  virtual void addScaledForces(btScalar scale, TVStack& force)
87  {
88  addScaledDampingForce(scale, force);
89  addScaledElasticForce(scale, force);
90  }
91 
92  virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
93  {
94  addScaledElasticForce(scale, force);
95  }
96 
97  // 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
98  virtual void addScaledDampingForce(btScalar scale, TVStack& force)
99  {
100  if (m_mu_damp == 0 && m_lambda_damp == 0)
101  return;
102  int numNodes = getNumNodes();
103  btAssert(numNodes <= force.size());
104  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
105  for (int i = 0; i < m_softBodies.size(); ++i)
106  {
107  btSoftBody* psb = m_softBodies[i];
108  if (!psb->isActive())
109  {
110  continue;
111  }
112  for (int j = 0; j < psb->m_tetras.size(); ++j)
113  {
114  btSoftBody::Tetra& tetra = psb->m_tetras[j];
115  btSoftBody::Node* node0 = tetra.m_n[0];
116  btSoftBody::Node* node1 = tetra.m_n[1];
117  btSoftBody::Node* node2 = tetra.m_n[2];
118  btSoftBody::Node* node3 = tetra.m_n[3];
119  size_t id0 = node0->index;
120  size_t id1 = node1->index;
121  size_t id2 = node2->index;
122  size_t id3 = node3->index;
123  btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
124  btMatrix3x3 I;
125  I.setIdentity();
126  btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
127  // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
128  btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col);
129  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
130 
131  // damping force differential
132  btScalar scale1 = scale * tetra.m_element_measure;
133  force[id0] -= scale1 * df_on_node0;
134  force[id1] -= scale1 * df_on_node123.getColumn(0);
135  force[id2] -= scale1 * df_on_node123.getColumn(1);
136  force[id3] -= scale1 * df_on_node123.getColumn(2);
137  }
138  }
139  }
140 
141  virtual double totalElasticEnergy(btScalar dt)
142  {
143  double energy = 0;
144  for (int i = 0; i < m_softBodies.size(); ++i)
145  {
146  btSoftBody* psb = m_softBodies[i];
147  if (!psb->isActive())
148  {
149  continue;
150  }
151  for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
152  {
153  btSoftBody::Tetra& tetra = psb->m_tetras[j];
155  energy += tetra.m_element_measure * elasticEnergyDensity(s);
156  }
157  }
158  return energy;
159  }
160 
161  // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
162  virtual double totalDampingEnergy(btScalar dt)
163  {
164  double energy = 0;
165  int sz = 0;
166  for (int i = 0; i < m_softBodies.size(); ++i)
167  {
168  btSoftBody* psb = m_softBodies[i];
169  if (!psb->isActive())
170  {
171  continue;
172  }
173  for (int j = 0; j < psb->m_nodes.size(); ++j)
174  {
175  sz = btMax(sz, psb->m_nodes[j].index);
176  }
177  }
178  TVStack dampingForce;
179  dampingForce.resize(sz + 1);
180  for (int i = 0; i < dampingForce.size(); ++i)
181  dampingForce[i].setZero();
182  addScaledDampingForce(0.5, dampingForce);
183  for (int i = 0; i < m_softBodies.size(); ++i)
184  {
185  btSoftBody* psb = m_softBodies[i];
186  for (int j = 0; j < psb->m_nodes.size(); ++j)
187  {
188  const btSoftBody::Node& node = psb->m_nodes[j];
189  energy -= dampingForce[node.index].dot(node.m_v) / dt;
190  }
191  }
192  return energy;
193  }
194 
196  {
197  double density = 0;
198  density += m_mu * 0.5 * (s.m_trace - 3.);
199  density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda) * (s.m_J - 1. - 0.75 * m_mu / m_lambda);
200  density -= m_mu * 0.5 * log(s.m_trace + 1);
201  return density;
202  }
203 
204  virtual void addScaledElasticForce(btScalar scale, TVStack& force)
205  {
206  int numNodes = getNumNodes();
207  btAssert(numNodes <= force.size());
208  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
209  for (int i = 0; i < m_softBodies.size(); ++i)
210  {
211  btSoftBody* psb = m_softBodies[i];
212  if (!psb->isActive())
213  {
214  continue;
215  }
216  btScalar max_p = psb->m_cfg.m_maxStress;
217  for (int j = 0; j < psb->m_tetras.size(); ++j)
218  {
219  btSoftBody::Tetra& tetra = psb->m_tetras[j];
220  btMatrix3x3 P;
221  firstPiola(psb->m_tetraScratches[j], P);
222 #ifdef USE_SVD
223  if (max_p > 0)
224  {
225  // since we want to clamp the principal stress to max_p, we only need to
226  // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
227  btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
228  if (trPTP > max_p * max_p)
229  {
230  btMatrix3x3 U, V;
231  btVector3 sigma;
232  singularValueDecomposition(P, U, sigma, V);
233  sigma[0] = btMin(sigma[0], max_p);
234  sigma[1] = btMin(sigma[1], max_p);
235  sigma[2] = btMin(sigma[2], max_p);
236  sigma[0] = btMax(sigma[0], -max_p);
237  sigma[1] = btMax(sigma[1], -max_p);
238  sigma[2] = btMax(sigma[2], -max_p);
239  btMatrix3x3 Sigma;
240  Sigma.setIdentity();
241  Sigma[0][0] = sigma[0];
242  Sigma[1][1] = sigma[1];
243  Sigma[2][2] = sigma[2];
244  P = U * Sigma * V.transpose();
245  }
246  }
247 #endif
248  // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
249  btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
250  btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
251 
252  btSoftBody::Node* node0 = tetra.m_n[0];
253  btSoftBody::Node* node1 = tetra.m_n[1];
254  btSoftBody::Node* node2 = tetra.m_n[2];
255  btSoftBody::Node* node3 = tetra.m_n[3];
256  size_t id0 = node0->index;
257  size_t id1 = node1->index;
258  size_t id2 = node2->index;
259  size_t id3 = node3->index;
260 
261  // elastic force
262  btScalar scale1 = scale * tetra.m_element_measure;
263  force[id0] -= scale1 * force_on_node0;
264  force[id1] -= scale1 * force_on_node123.getColumn(0);
265  force[id2] -= scale1 * force_on_node123.getColumn(1);
266  force[id3] -= scale1 * force_on_node123.getColumn(2);
267  }
268  }
269  }
270 
271  // 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
272  virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
273  {
274  if (m_mu_damp == 0 && m_lambda_damp == 0)
275  return;
276  int numNodes = getNumNodes();
277  btAssert(numNodes <= df.size());
278  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
279  for (int i = 0; i < m_softBodies.size(); ++i)
280  {
281  btSoftBody* psb = m_softBodies[i];
282  if (!psb->isActive())
283  {
284  continue;
285  }
286  for (int j = 0; j < psb->m_tetras.size(); ++j)
287  {
288  btSoftBody::Tetra& tetra = psb->m_tetras[j];
289  btSoftBody::Node* node0 = tetra.m_n[0];
290  btSoftBody::Node* node1 = tetra.m_n[1];
291  btSoftBody::Node* node2 = tetra.m_n[2];
292  btSoftBody::Node* node3 = tetra.m_n[3];
293  size_t id0 = node0->index;
294  size_t id1 = node1->index;
295  size_t id2 = node2->index;
296  size_t id3 = node3->index;
297  btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
298  btMatrix3x3 I;
299  I.setIdentity();
300  btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp;
301  // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
302  // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
303  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
304  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
305 
306  // damping force differential
307  btScalar scale1 = scale * tetra.m_element_measure;
308  df[id0] -= scale1 * df_on_node0;
309  df[id1] -= scale1 * df_on_node123.getColumn(0);
310  df[id2] -= scale1 * df_on_node123.getColumn(1);
311  df[id3] -= scale1 * df_on_node123.getColumn(2);
312  }
313  }
314  }
315 
317 
318  virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
319  {
320  int numNodes = getNumNodes();
321  btAssert(numNodes <= df.size());
322  btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1);
323  for (int i = 0; i < m_softBodies.size(); ++i)
324  {
325  btSoftBody* psb = m_softBodies[i];
326  if (!psb->isActive())
327  {
328  continue;
329  }
330  for (int j = 0; j < psb->m_tetras.size(); ++j)
331  {
332  btSoftBody::Tetra& tetra = psb->m_tetras[j];
333  btSoftBody::Node* node0 = tetra.m_n[0];
334  btSoftBody::Node* node1 = tetra.m_n[1];
335  btSoftBody::Node* node2 = tetra.m_n[2];
336  btSoftBody::Node* node3 = tetra.m_n[3];
337  size_t id0 = node0->index;
338  size_t id1 = node1->index;
339  size_t id2 = node2->index;
340  size_t id3 = node3->index;
341  btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
342  btMatrix3x3 dP;
343  firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
344  // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
345  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
346  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
347 
348  // elastic force differential
349  btScalar scale1 = scale * tetra.m_element_measure;
350  df[id0] -= scale1 * df_on_node0;
351  df[id1] -= scale1 * df_on_node123.getColumn(0);
352  df[id2] -= scale1 * df_on_node123.getColumn(1);
353  df[id3] -= scale1 * df_on_node123.getColumn(2);
354  }
355  }
356  }
357 
359  {
360  btScalar c1 = (m_mu * (1. - 1. / (s.m_trace + 1.)));
361  btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
362  P = s.m_F * c1 + s.m_cofF * c2;
363  }
364 
365  // Let P be the first piola stress.
366  // This function calculates the dP = dP/dF * dF
368  {
369  btScalar c1 = m_mu * (1. - 1. / (s.m_trace + 1.));
370  btScalar c2 = (2. * m_mu) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace)));
371  btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
372  dP = dF * c1 + s.m_F * c2;
373  addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda * (s.m_J - 1.) - 0.75 * m_mu, dP);
374  dP += s.m_cofF * c3;
375  }
376 
377  // Let Q be the damping stress.
378  // This function calculates the dP = dQ/dF * dF
380  {
381  btScalar c1 = (m_mu_damp * (1. - 1. / (s.m_trace + 1.)));
382  btScalar c2 = ((2. * m_mu_damp) * DotProduct(s.m_F, dF) * (1. / ((1. + s.m_trace) * (1. + s.m_trace))));
383  btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
384  dP = dF * c1 + s.m_F * c2;
385  addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp * (s.m_J - 1.) - 0.75 * m_mu_damp, dP);
386  dP += s.m_cofF * c3;
387  }
388 
390  {
391  btScalar ans = 0;
392  for (int i = 0; i < 3; ++i)
393  {
394  ans += A[i].dot(B[i]);
395  }
396  return ans;
397  }
398 
399  // Let C(A) be the cofactor of the matrix A
400  // Let H = the derivative of C(A) with respect to A evaluated at F = A
401  // This function calculates H*dF
403  {
404  M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
405  M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
406  M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
407  M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
408  M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
409  M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
410  M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
411  M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
412  M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
413  }
414 
416  {
417  return BT_NEOHOOKEAN_FORCE;
418  }
419 };
420 #endif /* BT_NEOHOOKEAN_H */
#define U
#define A
btDeformableLagrangianForceType
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
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
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 void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)
btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping=0.05)
virtual void addScaledForces(btScalar scale, TVStack &force)
void firstPiola(const btSoftBody::TetraScratch &s, btMatrix3x3 &P)
btAlignedObjectArray< btVector3 > TVStack
void setLameParameters(btScalar mu, btScalar lambda)
virtual void addScaledDampingForce(btScalar scale, TVStack &force)
void addScaledCofactorMatrixDifferential(const btMatrix3x3 &F, const btMatrix3x3 &dF, btScalar scale, btMatrix3x3 &M)
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)
virtual double totalElasticEnergy(btScalar dt)
btScalar DotProduct(const btMatrix3x3 &A, const btMatrix3x3 &B)
virtual double totalDampingEnergy(btScalar dt)
void firstPiolaDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
virtual void addScaledElasticForce(btScalar scale, TVStack &force)
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack &diagA)
double elasticEnergyDensity(const btSoftBody::TetraScratch &s)
virtual btDeformableLagrangianForceType getForceType()
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
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
ccl_device_inline float3 log(float3 v)
Definition: math_float3.h:397
static float P(float k)
Definition: math_interp.c:25
#define M
#define B
#define F
static const pxr::TfToken density("density", pxr::TfToken::Immortal)
#define I
btScalar m_maxStress
Definition: btSoftBody.h:728
btScalar m_element_measure
Definition: btSoftBody.h:315
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:313
CCL_NAMESPACE_BEGIN struct Window V