Blender  V3.3
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 {
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 
39 {
40 public:
44 
46  {
47  }
48 
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 
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  {
92  }
93 
94  virtual void removeSoftBody(btSoftBody* psb)
95  {
96  m_softBodies.remove(psb);
97  }
98 
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
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 
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 
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
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 */
__forceinline ssef low(const avxf &a)
Definition: avxf.h:264
__forceinline ssef high(const avxf &a)
Definition: avxf.h:268
btDeformableLagrangianForceType
@ BT_LINEAR_ELASTICITY_FORCE
@ BT_MOUSE_PICKING_FORCE
static double randomDouble(double low, double high)
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void remove(const T &key)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE 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 buildDampingForceDifferentialDiagonal(btScalar scale, TVStack &diagA)=0
virtual void reinitialize(bool nodeUpdated)
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)=0
virtual btDeformableLagrangianForceType getForceType()=0
virtual void addScaledHessian(btScalar scale)
virtual void removeSoftBody(btSoftBody *psb)
btAlignedObjectArray< btSoftBody * > m_softBodies
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
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)=0
virtual void addSoftBody(btSoftBody *psb)
void updateDeformation()
tNodeArray m_nodes
Definition: btSoftBody.h:799
ccl_gpu_kernel_postfix ccl_global int * counter
static void error(const char *str)
Definition: meshlaplacian.c:51
btVector3 m_v
Definition: btSoftBody.h:265