Bullet Collision Detection & Physics Library
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:
30  {
31  btScalar damping = 0.05;
32  m_mu_damp = damping * m_mu;
33  m_lambda_damp = damping * m_lambda;
34  }
35 
36  btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
37  {
38  m_mu_damp = damping * m_mu;
39  m_lambda_damp = damping * m_lambda;
40  }
41 
42  virtual void addScaledForces(btScalar scale, TVStack& force)
43  {
44  addScaledDampingForce(scale, force);
45  addScaledElasticForce(scale, force);
46  }
47 
48  virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
49  {
50  addScaledElasticForce(scale, force);
51  }
52 
53  // 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
54  virtual void addScaledDampingForce(btScalar scale, TVStack& force)
55  {
56  if (m_mu_damp == 0 && m_lambda_damp == 0)
57  return;
58  int numNodes = getNumNodes();
59  btAssert(numNodes <= force.size());
60  btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
61  for (int i = 0; i < m_softBodies.size(); ++i)
62  {
63  btSoftBody* psb = m_softBodies[i];
64  if (!psb->isActive())
65  {
66  continue;
67  }
68  for (int j = 0; j < psb->m_tetras.size(); ++j)
69  {
70  btSoftBody::Tetra& tetra = psb->m_tetras[j];
71  btSoftBody::Node* node0 = tetra.m_n[0];
72  btSoftBody::Node* node1 = tetra.m_n[1];
73  btSoftBody::Node* node2 = tetra.m_n[2];
74  btSoftBody::Node* node3 = tetra.m_n[3];
75  size_t id0 = node0->index;
76  size_t id1 = node1->index;
77  size_t id2 = node2->index;
78  size_t id3 = node3->index;
79  btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
80  btMatrix3x3 I;
81  I.setIdentity();
82  btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
83 // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
84  btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
85  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
86 
87  // damping force differential
88  btScalar scale1 = scale * tetra.m_element_measure;
89  force[id0] -= scale1 * df_on_node0;
90  force[id1] -= scale1 * df_on_node123.getColumn(0);
91  force[id2] -= scale1 * df_on_node123.getColumn(1);
92  force[id3] -= scale1 * df_on_node123.getColumn(2);
93  }
94  }
95  }
96 
97  virtual double totalElasticEnergy(btScalar dt)
98  {
99  double energy = 0;
100  for (int i = 0; i < m_softBodies.size(); ++i)
101  {
102  btSoftBody* psb = m_softBodies[i];
103  if (!psb->isActive())
104  {
105  continue;
106  }
107  for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
108  {
109  btSoftBody::Tetra& tetra = psb->m_tetras[j];
111  energy += tetra.m_element_measure * elasticEnergyDensity(s);
112  }
113  }
114  return energy;
115  }
116 
117  // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
118  virtual double totalDampingEnergy(btScalar dt)
119  {
120  double energy = 0;
121  int sz = 0;
122  for (int i = 0; i < m_softBodies.size(); ++i)
123  {
124  btSoftBody* psb = m_softBodies[i];
125  if (!psb->isActive())
126  {
127  continue;
128  }
129  for (int j = 0; j < psb->m_nodes.size(); ++j)
130  {
131  sz = btMax(sz, psb->m_nodes[j].index);
132  }
133  }
134  TVStack dampingForce;
135  dampingForce.resize(sz+1);
136  for (int i = 0; i < dampingForce.size(); ++i)
137  dampingForce[i].setZero();
138  addScaledDampingForce(0.5, dampingForce);
139  for (int i = 0; i < m_softBodies.size(); ++i)
140  {
141  btSoftBody* psb = m_softBodies[i];
142  for (int j = 0; j < psb->m_nodes.size(); ++j)
143  {
144  const btSoftBody::Node& node = psb->m_nodes[j];
145  energy -= dampingForce[node.index].dot(node.m_v) / dt;
146  }
147  }
148  return energy;
149  }
150 
152  {
153  double density = 0;
154  density += m_mu * 0.5 * (s.m_trace - 3.);
155  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);
156  density -= m_mu * 0.5 * log(s.m_trace+1);
157  return density;
158  }
159 
160  virtual void addScaledElasticForce(btScalar scale, TVStack& force)
161  {
162  int numNodes = getNumNodes();
163  btAssert(numNodes <= force.size());
164  btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
165  for (int i = 0; i < m_softBodies.size(); ++i)
166  {
167  btSoftBody* psb = m_softBodies[i];
168  if (!psb->isActive())
169  {
170  continue;
171  }
172  btScalar max_p = psb->m_cfg.m_maxStress;
173  for (int j = 0; j < psb->m_tetras.size(); ++j)
174  {
175  btSoftBody::Tetra& tetra = psb->m_tetras[j];
176  btMatrix3x3 P;
177  firstPiola(psb->m_tetraScratches[j],P);
178 #ifdef USE_SVD
179  if (max_p > 0)
180  {
181  // since we want to clamp the principal stress to max_p, we only need to
182  // calculate SVD when sigma_0^2 + sigma_1^2 + sigma_2^2 > max_p * max_p
183  btScalar trPTP = (P[0].length2() + P[1].length2() + P[2].length2());
184  if (trPTP > max_p * max_p)
185  {
186  btMatrix3x3 U, V;
187  btVector3 sigma;
188  singularValueDecomposition(P, U, sigma, V);
189  sigma[0] = btMin(sigma[0], max_p);
190  sigma[1] = btMin(sigma[1], max_p);
191  sigma[2] = btMin(sigma[2], max_p);
192  sigma[0] = btMax(sigma[0], -max_p);
193  sigma[1] = btMax(sigma[1], -max_p);
194  sigma[2] = btMax(sigma[2], -max_p);
195  btMatrix3x3 Sigma;
196  Sigma.setIdentity();
197  Sigma[0][0] = sigma[0];
198  Sigma[1][1] = sigma[1];
199  Sigma[2][2] = sigma[2];
200  P = U * Sigma * V.transpose();
201  }
202  }
203 #endif
204 // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
205  btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
206  btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
207 
208  btSoftBody::Node* node0 = tetra.m_n[0];
209  btSoftBody::Node* node1 = tetra.m_n[1];
210  btSoftBody::Node* node2 = tetra.m_n[2];
211  btSoftBody::Node* node3 = tetra.m_n[3];
212  size_t id0 = node0->index;
213  size_t id1 = node1->index;
214  size_t id2 = node2->index;
215  size_t id3 = node3->index;
216 
217  // elastic force
218  btScalar scale1 = scale * tetra.m_element_measure;
219  force[id0] -= scale1 * force_on_node0;
220  force[id1] -= scale1 * force_on_node123.getColumn(0);
221  force[id2] -= scale1 * force_on_node123.getColumn(1);
222  force[id3] -= scale1 * force_on_node123.getColumn(2);
223  }
224  }
225  }
226 
227  // 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
228  virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
229  {
230  if (m_mu_damp == 0 && m_lambda_damp == 0)
231  return;
232  int numNodes = getNumNodes();
233  btAssert(numNodes <= df.size());
234  btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
235  for (int i = 0; i < m_softBodies.size(); ++i)
236  {
237  btSoftBody* psb = m_softBodies[i];
238  if (!psb->isActive())
239  {
240  continue;
241  }
242  for (int j = 0; j < psb->m_tetras.size(); ++j)
243  {
244  btSoftBody::Tetra& tetra = psb->m_tetras[j];
245  btSoftBody::Node* node0 = tetra.m_n[0];
246  btSoftBody::Node* node1 = tetra.m_n[1];
247  btSoftBody::Node* node2 = tetra.m_n[2];
248  btSoftBody::Node* node3 = tetra.m_n[3];
249  size_t id0 = node0->index;
250  size_t id1 = node1->index;
251  size_t id2 = node2->index;
252  size_t id3 = node3->index;
253  btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
254  btMatrix3x3 I;
255  I.setIdentity();
256  btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
257 // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
258 // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
259  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
260  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
261 
262  // damping force differential
263  btScalar scale1 = scale * tetra.m_element_measure;
264  df[id0] -= scale1 * df_on_node0;
265  df[id1] -= scale1 * df_on_node123.getColumn(0);
266  df[id2] -= scale1 * df_on_node123.getColumn(1);
267  df[id3] -= scale1 * df_on_node123.getColumn(2);
268  }
269  }
270  }
271 
272  virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
273  {
274  int numNodes = getNumNodes();
275  btAssert(numNodes <= df.size());
276  btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
277  for (int i = 0; i < m_softBodies.size(); ++i)
278  {
279  btSoftBody* psb = m_softBodies[i];
280  if (!psb->isActive())
281  {
282  continue;
283  }
284  for (int j = 0; j < psb->m_tetras.size(); ++j)
285  {
286  btSoftBody::Tetra& tetra = psb->m_tetras[j];
287  btSoftBody::Node* node0 = tetra.m_n[0];
288  btSoftBody::Node* node1 = tetra.m_n[1];
289  btSoftBody::Node* node2 = tetra.m_n[2];
290  btSoftBody::Node* node3 = tetra.m_n[3];
291  size_t id0 = node0->index;
292  size_t id1 = node1->index;
293  size_t id2 = node2->index;
294  size_t id3 = node3->index;
295  btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
296  btMatrix3x3 dP;
297  firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
298 // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
299  btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
300  btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
301 
302  // elastic force differential
303  btScalar scale1 = scale * tetra.m_element_measure;
304  df[id0] -= scale1 * df_on_node0;
305  df[id1] -= scale1 * df_on_node123.getColumn(0);
306  df[id2] -= scale1 * df_on_node123.getColumn(1);
307  df[id3] -= scale1 * df_on_node123.getColumn(2);
308  }
309  }
310  }
311 
313  {
314  btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
315  btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
316  P = s.m_F * c1 + s.m_cofF * c2;
317  }
318 
319  // Let P be the first piola stress.
320  // This function calculates the dP = dP/dF * dF
322  {
323  btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
324  btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
325  btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
326  dP = dF * c1 + s.m_F * c2;
327  addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
328  dP += s.m_cofF * c3;
329  }
330 
331  // Let Q be the damping stress.
332  // This function calculates the dP = dQ/dF * dF
334  {
335  btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
336  btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
337  btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
338  dP = dF * c1 + s.m_F * c2;
340  dP += s.m_cofF * c3;
341  }
342 
344  {
345  btScalar ans = 0;
346  for (int i = 0; i < 3; ++i)
347  {
348  ans += A[i].dot(B[i]);
349  }
350  return ans;
351  }
352 
353  // Let C(A) be the cofactor of the matrix A
354  // Let H = the derivative of C(A) with respect to A evaluated at F = A
355  // This function calculates H*dF
357  {
358  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]);
359  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]);
360  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]);
361  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]);
362  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]);
363  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]);
364  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]);
365  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]);
366  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]);
367  }
368 
370  {
371  return BT_NEOHOOKEAN_FORCE;
372  }
373 
374 };
375 #endif /* BT_NEOHOOKEAN_H */
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'
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
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:153
int size() const
return the number of elements in the array
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
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)
double elasticEnergyDensity(const btSoftBody::TetraScratch &s)
virtual btDeformableLagrangianForceType getForceType()
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:321
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:140
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
tTetraArray m_tetras
Definition: btSoftBody.h:782
Config m_cfg
Definition: btSoftBody.h:771
btAlignedObjectArray< TetraScratch > m_tetraScratches
Definition: btSoftBody.h:783
tNodeArray m_nodes
Definition: btSoftBody.h:777
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btScalar m_maxStress
Definition: btSoftBody.h:715
btVector3 m_v
Definition: btSoftBody.h:259
btScalar m_element_measure
Definition: btSoftBody.h:305
btMatrix3x3 m_Dm_inverse
Definition: btSoftBody.h:303