Bullet Collision Detection & Physics Library
btDeformableMassSpringForce.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_MASS_SPRING_H
17 #define BT_MASS_SPRING_H
18 
20 
22 {
23  // If true, the damping force will be in the direction of the spring
24  // If false, the damping force will be in the direction of the velocity
27 public:
30  {
31  }
32  btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k)
33  {
35  {
37  }
38  }
39 
40  virtual void addScaledForces(btScalar scale, TVStack& force)
41  {
42  addScaledDampingForce(scale, force);
43  addScaledElasticForce(scale, force);
44  }
45 
46  virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
47  {
48  addScaledElasticForce(scale, force);
49  }
50 
51  virtual void addScaledDampingForce(btScalar scale, TVStack& force)
52  {
53  int numNodes = getNumNodes();
54  btAssert(numNodes <= force.size());
55  for (int i = 0; i < m_softBodies.size(); ++i)
56  {
57  const btSoftBody* psb = m_softBodies[i];
58  if (!psb->isActive())
59  {
60  continue;
61  }
62  for (int j = 0; j < psb->m_links.size(); ++j)
63  {
64  const btSoftBody::Link& link = psb->m_links[j];
65  btSoftBody::Node* node1 = link.m_n[0];
66  btSoftBody::Node* node2 = link.m_n[1];
67  size_t id1 = node1->index;
68  size_t id2 = node2->index;
69 
70  // damping force
71  btVector3 v_diff = (node2->m_v - node1->m_v);
72  btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
74  {
75  if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
76  {
77  btVector3 dir = (node2->m_x - node1->m_x).normalized();
78  scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
79  }
80  }
81  force[id1] += scaled_force;
82  force[id2] -= scaled_force;
83  }
84  }
85  }
86 
87  virtual void addScaledElasticForce(btScalar scale, TVStack& force)
88  {
89  int numNodes = getNumNodes();
90  btAssert(numNodes <= force.size());
91  for (int i = 0; i < m_softBodies.size(); ++i)
92  {
93  const btSoftBody* psb = m_softBodies[i];
94  if (!psb->isActive())
95  {
96  continue;
97  }
98  for (int j = 0; j < psb->m_links.size(); ++j)
99  {
100  const btSoftBody::Link& link = psb->m_links[j];
101  btSoftBody::Node* node1 = link.m_n[0];
102  btSoftBody::Node* node2 = link.m_n[1];
103  btScalar r = link.m_rl;
104  size_t id1 = node1->index;
105  size_t id2 = node2->index;
106 
107  // elastic force
108  btVector3 dir = (node2->m_q - node1->m_q);
109  btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
110  btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
111  btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
112  force[id1] += scaled_force;
113  force[id2] -= scaled_force;
114  }
115  }
116  }
117 
118  virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
119  {
120  // implicit damping force differential
121  for (int i = 0; i < m_softBodies.size(); ++i)
122  {
123  btSoftBody* psb = m_softBodies[i];
124  if (!psb->isActive())
125  {
126  continue;
127  }
128  btScalar scaled_k_damp = m_dampingStiffness * scale;
129  for (int j = 0; j < psb->m_links.size(); ++j)
130  {
131  const btSoftBody::Link& link = psb->m_links[j];
132  btSoftBody::Node* node1 = link.m_n[0];
133  btSoftBody::Node* node2 = link.m_n[1];
134  size_t id1 = node1->index;
135  size_t id2 = node2->index;
136 
137  btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
139  {
140  if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
141  {
142  btVector3 dir = (node2->m_x - node1->m_x).normalized();
143  local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
144  }
145  }
146  df[id1] += local_scaled_df;
147  df[id2] -= local_scaled_df;
148  }
149  }
150  }
151 
152  virtual double totalElasticEnergy(btScalar dt)
153  {
154  double energy = 0;
155  for (int i = 0; i < m_softBodies.size(); ++i)
156  {
157  const btSoftBody* psb = m_softBodies[i];
158  if (!psb->isActive())
159  {
160  continue;
161  }
162  for (int j = 0; j < psb->m_links.size(); ++j)
163  {
164  const btSoftBody::Link& link = psb->m_links[j];
165  btSoftBody::Node* node1 = link.m_n[0];
166  btSoftBody::Node* node2 = link.m_n[1];
167  btScalar r = link.m_rl;
168 
169  // elastic force
170  btVector3 dir = (node2->m_q - node1->m_q);
171  energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r);
172  }
173  }
174  return energy;
175  }
176 
177  virtual double totalDampingEnergy(btScalar dt)
178  {
179  double energy = 0;
180  int sz = 0;
181  for (int i = 0; i < m_softBodies.size(); ++i)
182  {
183  btSoftBody* psb = m_softBodies[i];
184  if (!psb->isActive())
185  {
186  continue;
187  }
188  for (int j = 0; j < psb->m_nodes.size(); ++j)
189  {
190  sz = btMax(sz, psb->m_nodes[j].index);
191  }
192  }
193  TVStack dampingForce;
194  dampingForce.resize(sz+1);
195  for (int i = 0; i < dampingForce.size(); ++i)
196  dampingForce[i].setZero();
197  addScaledDampingForce(0.5, dampingForce);
198  for (int i = 0; i < m_softBodies.size(); ++i)
199  {
200  btSoftBody* psb = m_softBodies[i];
201  for (int j = 0; j < psb->m_nodes.size(); ++j)
202  {
203  const btSoftBody::Node& node = psb->m_nodes[j];
204  energy -= dampingForce[node.index].dot(node.m_v) / dt;
205  }
206  }
207  return energy;
208  }
209 
210  virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
211  {
212  // implicit damping force differential
213  for (int i = 0; i < m_softBodies.size(); ++i)
214  {
215  const btSoftBody* psb = m_softBodies[i];
216  if (!psb->isActive())
217  {
218  continue;
219  }
220  for (int j = 0; j < psb->m_links.size(); ++j)
221  {
222  const btSoftBody::Link& link = psb->m_links[j];
223  btSoftBody::Node* node1 = link.m_n[0];
224  btSoftBody::Node* node2 = link.m_n[1];
225  size_t id1 = node1->index;
226  size_t id2 = node2->index;
227  btScalar r = link.m_rl;
228 
229  btVector3 dir = (node1->m_q - node2->m_q);
230  btScalar dir_norm = dir.norm();
231  btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
232  btVector3 dx_diff = dx[id1] - dx[id2];
233  btVector3 scaled_df = btVector3(0,0,0);
234  btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
235  if (dir_norm > SIMD_EPSILON)
236  {
237  scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
238  scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
239  scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
240  }
241 
242  df[id1] += scaled_df;
243  df[id2] -= scaled_df;
244  }
245  }
246  }
247 
249  {
250  return BT_MASSSPRING_FORCE;
251  }
252 
253 };
254 
255 #endif /* btMassSpring_h */
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btDeformableMassSpringForce::addScaledForces
virtual void addScaledForces(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:40
btSoftBody::m_links
tLinkArray m_links
Definition: btSoftBody.h:779
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btDeformableMassSpringForce::addScaledElasticForceDifferential
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)
Definition: btDeformableMassSpringForce.h:210
BT_MASSSPRING_FORCE
Definition: btDeformableLagrangianForce.h:26
btDeformableMassSpringForce::addScaledDampingForceDifferential
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)
Definition: btDeformableMassSpringForce.h:118
btSoftBody::Node
Definition: btSoftBody.h:255
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btDeformableMassSpringForce::TVStack
btAlignedObjectArray< btVector3 > TVStack
Definition: btDeformableMassSpringForce.h:28
btSoftBody::Node::m_q
btVector3 m_q
Definition: btSoftBody.h:258
btDeformableMassSpringForce
Definition: btDeformableMassSpringForce.h:21
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSoftBody::Node::m_x
btVector3 m_x
Definition: btSoftBody.h:257
btDeformableLagrangianForce::m_softBodies
btAlignedObjectArray< btSoftBody * > m_softBodies
Definition: btDeformableLagrangianForce.h:41
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btDeformableMassSpringForce::m_momentum_conserving
bool m_momentum_conserving
Definition: btDeformableMassSpringForce.h:25
btDeformableLagrangianForce
Definition: btDeformableLagrangianForce.h:37
btDeformableMassSpringForce::btDeformableMassSpringForce
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular=true, double bending_k=-1)
Definition: btDeformableMassSpringForce.h:32
btDeformableMassSpringForce::totalDampingEnergy
virtual double totalDampingEnergy(btScalar dt)
Definition: btDeformableMassSpringForce.h:177
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btDeformableMassSpringForce::addScaledElasticForce
virtual void addScaledElasticForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:87
btSoftBody::Node::index
int index
Definition: btSoftBody.h:268
btDeformableMassSpringForce::m_dampingStiffness
btScalar m_dampingStiffness
Definition: btDeformableMassSpringForce.h:26
btAlignedObjectArray< btVector3 >
btDeformableMassSpringForce::m_bendingStiffness
btScalar m_bendingStiffness
Definition: btDeformableMassSpringForce.h:26
btDeformableMassSpringForce::addScaledDampingForce
virtual void addScaledDampingForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:51
btDeformableMassSpringForce::btDeformableMassSpringForce
btDeformableMassSpringForce()
Definition: btDeformableMassSpringForce.h:29
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:72
btCollisionObject::isActive
bool isActive() const
Definition: btCollisionObject.h:294
btDeformableLagrangianForce::getNumNodes
virtual int getNumNodes()
Definition: btDeformableLagrangianForce.h:72
btDeformableLagrangianForceType
btDeformableLagrangianForceType
Definition: btDeformableLagrangianForce.h:23
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btDeformableMassSpringForce::getForceType
virtual btDeformableLagrangianForceType getForceType()
Definition: btDeformableMassSpringForce.h:248
btDeformableMassSpringForce::totalElasticEnergy
virtual double totalElasticEnergy(btScalar dt)
Definition: btDeformableMassSpringForce.h:152
btDeformableLagrangianForce.h
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:259
btDeformableMassSpringForce::m_elasticStiffness
btScalar m_elasticStiffness
Definition: btDeformableMassSpringForce.h:26
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btDeformableMassSpringForce::addScaledExplicitForce
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:46
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