Bullet Collision Detection & Physics Library
btDeformableNeoHookeanForce.h
Go to the documentation of this file.
1/*
2Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
3
4Bullet Continuous Collision Detection and Physics Library
5Copyright (c) 2019 Google Inc. http://bulletphysics.org
6This software is provided 'as-is', without any express or implied warranty.
7In no event will the authors be held liable for any damages arising from the use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it freely,
10subject to the following restrictions:
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
22// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
24{
25public:
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;
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;
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