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"
21#include <iostream>
22
24{
30};
31
32static inline double randomDouble(double low, double high)
33{
34 return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
35}
36
38{
39public:
43
45 {
46 }
47
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
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 {
86 }
87
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
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
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
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
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 */
btDeformableLagrangianForceType
@ BT_LINEAR_ELASTICITY_FORCE
static double randomDouble(double low, double high)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
virtual double totalDampingEnergy(btScalar dt)
virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack &dx)
btAlignedObjectArray< btVector3 > TVStack
virtual double totalEnergy(btScalar dt)
virtual void setIndices(const btAlignedObjectArray< btSoftBody::Node * > *nodes)
virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node *n0, const btSoftBody::Node *n1, const btSoftBody::Node *n2, const btSoftBody::Node *n3)
const btAlignedObjectArray< btSoftBody::Node * > * m_nodes
virtual double totalElasticEnergy(btScalar dt)
virtual void reinitialize(bool nodeUpdated)
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)=0
virtual btDeformableLagrangianForceType getForceType()=0
virtual void addScaledForces(btScalar scale, TVStack &force)=0
virtual void addScaledDampingForce(btScalar scale, TVStack &force)=0
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)=0
btAlignedObjectArray< btSoftBody * > m_softBodies
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)=0
virtual void addSoftBody(btSoftBody *psb)
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
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
void updateDeformation()
tNodeArray m_nodes
Definition: btSoftBody.h:777
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3 m_v
Definition: btSoftBody.h:259