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 
24 {
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 
38 {
39 public:
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 */
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
Definition: btDeformableLagrangianForce.h:28
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
Definition: btDeformableLagrangianForce.h:26
btSoftBody::Node
Definition: btSoftBody.h:255
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:37
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
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:80
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
Definition: btDeformableLagrangianForce.h:29
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:72
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:23
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