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