Bullet Collision Detection & Physics Library: btDeformableLinearElasticityForce.h Source File

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
19 #include "btDeformableLagrangianForce.h"
20 #include "LinearMath/btQuickprof.h"
21 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce
22 {
23 public:
24 typedef btAlignedObjectArray<btVector3> TVStack;
25 btScalar m_mu, m_lambda;
26 btScalar m_mu_damp, m_lambda_damp;
27 btDeformableLinearElasticityForce(): m_mu(1), m_lambda(1)
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];
108 btSoftBody::TetraScratch& s = psb->m_tetraScratches[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
149 double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
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
311 void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
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
320 void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
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
328 void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP)
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
334 virtual btDeformableLagrangianForceType getForceType()
335 {
336 return BT_LINEAR_ELASTICITY_FORCE;
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:256
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:296
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:38
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:48
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:82
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:22
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
@ 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:73
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:24
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:310
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