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