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
22{
23public:
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;
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;
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 */
btDeformableLagrangianForceType
@ BT_LINEAR_ELASTICITY_FORCE
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 btDeformableLagrangianForceType getForceType()
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)
void firstPiolaDampingDifferential(const btSoftBody::TetraScratch &s, const btMatrix3x3 &dF, btMatrix3x3 &dP)
virtual void addScaledElasticForce(btScalar scale, TVStack &force)
void firstPiola(const btSoftBody::TetraScratch &s, btMatrix3x3 &P)
virtual void addScaledForces(btScalar scale, TVStack &force)
btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping=0.05)
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)
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
static const btMatrix3x3 & getIdentity()
Definition: btMatrix3x3.h:334
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