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

Bullet Collision Detection & Physics Library

btDeformableLagrangianForce.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_DEFORMABLE_LAGRANGIAN_FORCE_H
17 #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
18
19 #include "btSoftBody.h"
20 #include <LinearMath/btHashMap.h>
21 #include <iostream>
22
23 enum btDeformableLagrangianForceType
24 {
25 BT_GRAVITY_FORCE = 1,
26 BT_MASSSPRING_FORCE = 2,
27 BT_COROTATED_FORCE = 3,
28 BT_NEOHOOKEAN_FORCE = 4,
29 BT_LINEAR_ELASTICITY_FORCE = 5
30 };
31
32 static inline double randomDouble(double low, double high)
33 {
34 return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
35 }
36
37 class btDeformableLagrangianForce
38 {
39 public:
40 typedef btAlignedObjectArray<btVector3> TVStack;
41 btAlignedObjectArray<btSoftBody *> m_softBodies;
42 const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
43
44 btDeformableLagrangianForce()
45 {
46 }
47
48 virtual ~btDeformableLagrangianForce(){}
49
50 // add all forces
51 virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
52
53 // add damping df
54 virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
55
56 // add elastic df
57 virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
58
59 // add all forces that are explicit in explicit solve
60 virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
61
62 // add all damping forces
63 virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
64
65 virtual btDeformableLagrangianForceType getForceType() = 0;
66
67 virtual void reinitialize(bool nodeUpdated)
68 {
69 }
70
71 // get number of nodes that have the force
72 virtual int getNumNodes()
73 {
74 int numNodes = 0;
75 for (int i = 0; i < m_softBodies.size(); ++i)
76 {
77 numNodes += m_softBodies[i]->m_nodes.size();
78 }
79 return numNodes;
80 }
81
82 // add a soft body to be affected by the particular lagrangian force
83 virtual void addSoftBody(btSoftBody* psb)
84 {
85 m_softBodies.push_back(psb);
86 }
87
88 virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
89 {
90 m_nodes = nodes;
91 }
92
93 // Calculate the incremental deformable generated from the input dx
94 virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
95 {
96 btVector3 c1 = dx[id1] - dx[id0];
97 btVector3 c2 = dx[id2] - dx[id0];
98 btVector3 c3 = dx[id3] - dx[id0];
99 return btMatrix3x3(c1,c2,c3).transpose();
100 }
101
102 // Calculate the incremental deformable generated from the current velocity
103 virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
104 {
105 btVector3 c1 = n1->m_v - n0->m_v;
106 btVector3 c2 = n2->m_v - n0->m_v;
107 btVector3 c3 = n3->m_v - n0->m_v;
108 return btMatrix3x3(c1,c2,c3).transpose();
109 }
110
111 // test for addScaledElasticForce function
112 virtual void testDerivative()
113 {
114 for (int i = 0; i<m_softBodies.size();++i)
115 {
116 btSoftBody* psb = m_softBodies[i];
117 for (int j = 0; j < psb->m_nodes.size(); ++j)
118 {
119 psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
120 }
121 psb->updateDeformation();
122 }
123
124 TVStack dx;
125 dx.resize(getNumNodes());
126 TVStack dphi_dx;
127 dphi_dx.resize(dx.size());
128 for (int i =0; i < dphi_dx.size();++i)
129 {
130 dphi_dx[i].setZero();
131 }
132 addScaledForces(-1, dphi_dx);
133
134 // write down the current position
135 TVStack x;
136 x.resize(dx.size());
137 int counter = 0;
138 for (int i = 0; i<m_softBodies.size();++i)
139 {
140 btSoftBody* psb = m_softBodies[i];
141 for (int j = 0; j < psb->m_nodes.size(); ++j)
142 {
143 x[counter] = psb->m_nodes[j].m_q;
144 counter++;
145 }
146 }
147 counter = 0;
148
149 // populate dx with random vectors
150 for (int i = 0; i < dx.size(); ++i)
151 {
152 dx[i].setX(randomDouble(-1, 1));
153 dx[i].setY(randomDouble(-1, 1));
154 dx[i].setZ(randomDouble(-1, 1));
155 }
156
157 btAlignedObjectArray<double> errors;
158 for (int it = 0; it < 10; ++it)
159 {
160 for (int i = 0; i < dx.size(); ++i)
161 {
162 dx[i] *= 0.5;
163 }
164
165 // get dphi/dx * dx
166 double dphi = 0;
167 for (int i = 0; i < dx.size(); ++i)
168 {
169 dphi += dphi_dx[i].dot(dx[i]);
170 }
171
172
173 for (int i = 0; i<m_softBodies.size();++i)
174 {
175 btSoftBody* psb = m_softBodies[i];
176 for (int j = 0; j < psb->m_nodes.size(); ++j)
177 {
178 psb->m_nodes[j].m_q = x[counter] + dx[counter];
179 counter++;
180 }
181 psb->updateDeformation();
182 }
183 counter = 0;
184 double f1 = totalElasticEnergy(0);
185
186 for (int i = 0; i<m_softBodies.size();++i)
187 {
188 btSoftBody* psb = m_softBodies[i];
189 for (int j = 0; j < psb->m_nodes.size(); ++j)
190 {
191 psb->m_nodes[j].m_q = x[counter] - dx[counter];
192 counter++;
193 }
194 psb->updateDeformation();
195 }
196 counter = 0;
197
198 double f2 = totalElasticEnergy(0);
199
200 //restore m_q
201 for (int i = 0; i<m_softBodies.size();++i)
202 {
203 btSoftBody* psb = m_softBodies[i];
204 for (int j = 0; j < psb->m_nodes.size(); ++j)
205 {
206 psb->m_nodes[j].m_q = x[counter];
207 counter++;
208 }
209 psb->updateDeformation();
210 }
211 counter = 0;
212 double error = f1-f2-2*dphi;
213 errors.push_back(error);
214 std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
215 }
216 for (int i = 1; i < errors.size(); ++i)
217 {
218 std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
219 }
220 }
221
222 // test for addScaledElasticForce function
223 virtual void testHessian()
224 {
225 for (int i = 0; i<m_softBodies.size();++i)
226 {
227 btSoftBody* psb = m_softBodies[i];
228 for (int j = 0; j < psb->m_nodes.size(); ++j)
229 {
230 psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
231 }
232 psb->updateDeformation();
233 }
234
235
236 TVStack dx;
237 dx.resize(getNumNodes());
238 TVStack df;
239 df.resize(dx.size());
240 TVStack f1;
241 f1.resize(dx.size());
242 TVStack f2;
243 f2.resize(dx.size());
244
245
246 // write down the current position
247 TVStack x;
248 x.resize(dx.size());
249 int counter = 0;
250 for (int i = 0; i<m_softBodies.size();++i)
251 {
252 btSoftBody* psb = m_softBodies[i];
253 for (int j = 0; j < psb->m_nodes.size(); ++j)
254 {
255 x[counter] = psb->m_nodes[j].m_q;
256 counter++;
257 }
258 }
259 counter = 0;
260
261 // populate dx with random vectors
262 for (int i = 0; i < dx.size(); ++i)
263 {
264 dx[i].setX(randomDouble(-1, 1));
265 dx[i].setY(randomDouble(-1, 1));
266 dx[i].setZ(randomDouble(-1, 1));
267 }
268
269 btAlignedObjectArray<double> errors;
270 for (int it = 0; it < 10; ++it)
271 {
272 for (int i = 0; i < dx.size(); ++i)
273 {
274 dx[i] *= 0.5;
275 }
276
277 // get df
278 for (int i =0; i < df.size();++i)
279 {
280 df[i].setZero();
281 f1[i].setZero();
282 f2[i].setZero();
283 }
284
285 //set df
286 addScaledElasticForceDifferential(-1, dx, df);
287
288 for (int i = 0; i<m_softBodies.size();++i)
289 {
290 btSoftBody* psb = m_softBodies[i];
291 for (int j = 0; j < psb->m_nodes.size(); ++j)
292 {
293 psb->m_nodes[j].m_q = x[counter] + dx[counter];
294 counter++;
295 }
296 psb->updateDeformation();
297 }
298 counter = 0;
299
300 //set f1
301 addScaledForces(-1, f1);
302
303 for (int i = 0; i<m_softBodies.size();++i)
304 {
305 btSoftBody* psb = m_softBodies[i];
306 for (int j = 0; j < psb->m_nodes.size(); ++j)
307 {
308 psb->m_nodes[j].m_q = x[counter] - dx[counter];
309 counter++;
310 }
311 psb->updateDeformation();
312 }
313 counter = 0;
314
315 //set f2
316 addScaledForces(-1, f2);
317
318 //restore m_q
319 for (int i = 0; i<m_softBodies.size();++i)
320 {
321 btSoftBody* psb = m_softBodies[i];
322 for (int j = 0; j < psb->m_nodes.size(); ++j)
323 {
324 psb->m_nodes[j].m_q = x[counter];
325 counter++;
326 }
327 psb->updateDeformation();
328 }
329 counter = 0;
330 double error = 0;
331 for (int i = 0; i < df.size();++i)
332 {
333 btVector3 error_vector = f1[i]-f2[i]-2*df[i];
334 error += error_vector.length2();
335 }
336 error = btSqrt(error);
337 errors.push_back(error);
338 std::cout << "Iteration = " << it << ", error = " << error << std::endl;
339 }
340 for (int i = 1; i < errors.size(); ++i)
341 {
342 std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
343 }
344 }
345
346 //
347 virtual double totalElasticEnergy(btScalar dt)
348 {
349 return 0;
350 }
351
352 //
353 virtual double totalDampingEnergy(btScalar dt)
354 {
355 return 0;
356 }
357
358 // total Energy takes dt as input because certain energies depend on dt
359 virtual double totalEnergy(btScalar dt)
360 {
361 return totalElasticEnergy(dt) + totalDampingEnergy(dt);
362 }
363 };
364 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
BT_COROTATED_FORCE
@ BT_COROTATED_FORCE
Definition: btDeformableLagrangianForce.h:27
btDeformableLagrangianForce::addScaledDampingForce
virtual void addScaledDampingForce(btScalar scale, TVStack &force)=0
btDeformableLagrangianForce::setIndices
virtual void setIndices(const btAlignedObjectArray< btSoftBody::Node * > *nodes)
Definition: btDeformableLagrangianForce.h:88
btDeformableLagrangianForce::addScaledExplicitForce
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)=0
btDeformableLagrangianForce::Ds
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack &dx)
Definition: btDeformableLagrangianForce.h:94
btDeformableLagrangianForce::getForceType
virtual btDeformableLagrangianForceType getForceType()=0
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
BT_NEOHOOKEAN_FORCE
@ BT_NEOHOOKEAN_FORCE
Definition: btDeformableLagrangianForce.h:28
BT_GRAVITY_FORCE
@ BT_GRAVITY_FORCE
Definition: btDeformableLagrangianForce.h:25
btDeformableLagrangianForce::btDeformableLagrangianForce
btDeformableLagrangianForce()
Definition: btDeformableLagrangianForce.h:44
btDeformableLagrangianForce::totalEnergy
virtual double totalEnergy(btScalar dt)
Definition: btDeformableLagrangianForce.h:359
btDeformableLagrangianForce::totalElasticEnergy
virtual double totalElasticEnergy(btScalar dt)
Definition: btDeformableLagrangianForce.h:347
BT_MASSSPRING_FORCE
@ BT_MASSSPRING_FORCE
Definition: btDeformableLagrangianForce.h:26
btSoftBody::Node
Definition: btSoftBody.h:256
btDeformableLagrangianForce::addScaledDampingForceDifferential
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)=0
btDeformableLagrangianForce::addScaledElasticForceDifferential
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)=0
btDeformableLagrangianForce::~btDeformableLagrangianForce
virtual ~btDeformableLagrangianForce()
Definition: btDeformableLagrangianForce.h:48
randomDouble
static double randomDouble(double low, double high)
Definition: btDeformableLagrangianForce.h:32
btDeformableLagrangianForce::m_softBodies
btAlignedObjectArray< btSoftBody * > m_softBodies
Definition: btDeformableLagrangianForce.h:41
btDeformableLagrangianForce::addSoftBody
virtual void addSoftBody(btSoftBody *psb)
Definition: btDeformableLagrangianForce.h:83
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btSoftBody::updateDeformation
void updateDeformation()
Definition: btSoftBody.cpp:3098
btDeformableLagrangianForce::TVStack
btAlignedObjectArray< btVector3 > TVStack
Definition: btDeformableLagrangianForce.h:40
btDeformableLagrangianForce
Definition: btDeformableLagrangianForce.h:38
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1033
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btDeformableLagrangianForce::testHessian
virtual void testHessian()
Definition: btDeformableLagrangianForce.h:223
btAlignedObjectArray< btVector3 >
btHashMap.h
btDeformableLagrangianForce::totalDampingEnergy
virtual double totalDampingEnergy(btScalar dt)
Definition: btDeformableLagrangianForce.h:353
BT_LINEAR_ELASTICITY_FORCE
@ BT_LINEAR_ELASTICITY_FORCE
Definition: btDeformableLagrangianForce.h:29
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
btDeformableLagrangianForce::m_nodes
const btAlignedObjectArray< btSoftBody::Node * > * m_nodes
Definition: btDeformableLagrangianForce.h:42
btDeformableLagrangianForce::getNumNodes
virtual int getNumNodes()
Definition: btDeformableLagrangianForce.h:72
btDeformableLagrangianForce::reinitialize
virtual void reinitialize(bool nodeUpdated)
Definition: btDeformableLagrangianForce.h:67
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
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btDeformableLagrangianForce::addScaledForces
virtual void addScaledForces(btScalar scale, TVStack &force)=0
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:259
btDeformableLagrangianForce::testDerivative
virtual void testDerivative()
Definition: btDeformableLagrangianForce.h:112
btSoftBody.h
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251