Bullet Collision Detection & Physics Library: btDeformableMassSpringForce.h Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
19#include "btDeformableLagrangianForce.h"
20
21 class btDeformableMassSpringForce : public btDeformableLagrangianForce
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
25 bool m_momentum_conserving;
26 btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness;
27
28public:
29 typedef btAlignedObjectArray<btVector3> TVStack;
30 btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
31 {
32 }
33 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)
34 {
35 if (m_bendingStiffness < btScalar(0))
36 {
37 m_bendingStiffness = m_elasticStiffness;
38 }
39 }
40
41 virtual void addScaledForces(btScalar scale, TVStack& force)
42 {
43 addScaledDampingForce(scale, force);
44 addScaledElasticForce(scale, force);
45 }
46
47 virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
48 {
49 addScaledElasticForce(scale, force);
50 }
51
52 virtual void addScaledDampingForce(btScalar scale, TVStack& force)
53 {
54 int numNodes = getNumNodes();
55 btAssert(numNodes <= force.size());
56 for (int i = 0; i < m_softBodies.size(); ++i)
57 {
58 const btSoftBody* psb = m_softBodies[i];
59 if (!psb->isActive())
60 {
61 continue;
62 }
63 for (int j = 0; j < psb->m_links.size(); ++j)
64 {
65 const btSoftBody::Link& link = psb->m_links[j];
66 btSoftBody::Node* node1 = link.m_n[0];
67 btSoftBody::Node* node2 = link.m_n[1];
68 size_t id1 = node1->index;
69 size_t id2 = node2->index;
70
71 // damping force
72 btVector3 v_diff = (node2->m_v - node1->m_v);
73 btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
74 if (m_momentum_conserving)
75 {
76 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
77 {
78 btVector3 dir = (node2->m_x - node1->m_x).normalized();
79 scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
80 }
81 }
82 force[id1] += scaled_force;
83 force[id2] -= scaled_force;
84 }
85 }
86 }
87
88 virtual void addScaledElasticForce(btScalar scale, TVStack& force)
89 {
90 int numNodes = getNumNodes();
91 btAssert(numNodes <= force.size());
92 for (int i = 0; i < m_softBodies.size(); ++i)
93 {
94 const btSoftBody* psb = m_softBodies[i];
95 if (!psb->isActive())
96 {
97 continue;
98 }
99 for (int j = 0; j < psb->m_links.size(); ++j)
100 {
101 const btSoftBody::Link& link = psb->m_links[j];
102 btSoftBody::Node* node1 = link.m_n[0];
103 btSoftBody::Node* node2 = link.m_n[1];
104 btScalar r = link.m_rl;
105 size_t id1 = node1->index;
106 size_t id2 = node2->index;
107
108 // elastic force
109 btVector3 dir = (node2->m_q - node1->m_q);
110 btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
111 btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
112 btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r);
113 force[id1] += scaled_force;
114 force[id2] -= scaled_force;
115 }
116 }
117 }
118
119 virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
120 {
121 // implicit damping force differential
122 for (int i = 0; i < m_softBodies.size(); ++i)
123 {
124 btSoftBody* psb = m_softBodies[i];
125 if (!psb->isActive())
126 {
127 continue;
128 }
129 btScalar scaled_k_damp = m_dampingStiffness * scale;
130 for (int j = 0; j < psb->m_links.size(); ++j)
131 {
132 const btSoftBody::Link& link = psb->m_links[j];
133 btSoftBody::Node* node1 = link.m_n[0];
134 btSoftBody::Node* node2 = link.m_n[1];
135 size_t id1 = node1->index;
136 size_t id2 = node2->index;
137
138 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
139 if (m_momentum_conserving)
140 {
141 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
142 {
143 btVector3 dir = (node2->m_x - node1->m_x).normalized();
144 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
145 }
146 }
147 df[id1] += local_scaled_df;
148 df[id2] -= local_scaled_df;
149 }
150 }
151 }
152
153 virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
154 {
155 // implicit damping force differential
156 for (int i = 0; i < m_softBodies.size(); ++i)
157 {
158 btSoftBody* psb = m_softBodies[i];
159 if (!psb->isActive())
160 {
161 continue;
162 }
163 btScalar scaled_k_damp = m_dampingStiffness * scale;
164 for (int j = 0; j < psb->m_links.size(); ++j)
165 {
166 const btSoftBody::Link& link = psb->m_links[j];
167 btSoftBody::Node* node1 = link.m_n[0];
168 btSoftBody::Node* node2 = link.m_n[1];
169 size_t id1 = node1->index;
170 size_t id2 = node2->index;
171 if (m_momentum_conserving)
172 {
173 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
174 {
175 btVector3 dir = (node2->m_x - node1->m_x).normalized();
176 for (int d = 0; d < 3; ++d)
177 {
178 if (node1->m_im > 0)
179 diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
180 if (node2->m_im > 0)
181 diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
182 }
183 }
184 }
185 else
186 {
187 for (int d = 0; d < 3; ++d)
188 {
189 if (node1->m_im > 0)
190 diagA[id1][d] -= scaled_k_damp;
191 if (node2->m_im > 0)
192 diagA[id2][d] -= scaled_k_damp;
193 }
194 }
195 }
196 }
197 }
198
199 virtual double totalElasticEnergy(btScalar dt)
200 {
201 double energy = 0;
202 for (int i = 0; i < m_softBodies.size(); ++i)
203 {
204 const btSoftBody* psb = m_softBodies[i];
205 if (!psb->isActive())
206 {
207 continue;
208 }
209 for (int j = 0; j < psb->m_links.size(); ++j)
210 {
211 const btSoftBody::Link& link = psb->m_links[j];
212 btSoftBody::Node* node1 = link.m_n[0];
213 btSoftBody::Node* node2 = link.m_n[1];
214 btScalar r = link.m_rl;
215
216 // elastic force
217 btVector3 dir = (node2->m_q - node1->m_q);
218 energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
219 }
220 }
221 return energy;
222 }
223
224 virtual double totalDampingEnergy(btScalar dt)
225 {
226 double energy = 0;
227 int sz = 0;
228 for (int i = 0; i < m_softBodies.size(); ++i)
229 {
230 btSoftBody* psb = m_softBodies[i];
231 if (!psb->isActive())
232 {
233 continue;
234 }
235 for (int j = 0; j < psb->m_nodes.size(); ++j)
236 {
237 sz = btMax(sz, psb->m_nodes[j].index);
238 }
239 }
240 TVStack dampingForce;
241 dampingForce.resize(sz + 1);
242 for (int i = 0; i < dampingForce.size(); ++i)
243 dampingForce[i].setZero();
244 addScaledDampingForce(0.5, dampingForce);
245 for (int i = 0; i < m_softBodies.size(); ++i)
246 {
247 btSoftBody* psb = m_softBodies[i];
248 for (int j = 0; j < psb->m_nodes.size(); ++j)
249 {
250 const btSoftBody::Node& node = psb->m_nodes[j];
251 energy -= dampingForce[node.index].dot(node.m_v) / dt;
252 }
253 }
254 return energy;
255 }
256
257 virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
258 {
259 // implicit damping force differential
260 for (int i = 0; i < m_softBodies.size(); ++i)
261 {
262 const btSoftBody* psb = m_softBodies[i];
263 if (!psb->isActive())
264 {
265 continue;
266 }
267 for (int j = 0; j < psb->m_links.size(); ++j)
268 {
269 const btSoftBody::Link& link = psb->m_links[j];
270 btSoftBody::Node* node1 = link.m_n[0];
271 btSoftBody::Node* node2 = link.m_n[1];
272 size_t id1 = node1->index;
273 size_t id2 = node2->index;
274 btScalar r = link.m_rl;
275
276 btVector3 dir = (node1->m_q - node2->m_q);
277 btScalar dir_norm = dir.norm();
278 btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0);
279 btVector3 dx_diff = dx[id1] - dx[id2];
280 btVector3 scaled_df = btVector3(0, 0, 0);
281 btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness);
282 if (dir_norm > SIMD_EPSILON)
283 {
284 scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
285 scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized;
286 scaled_df -= scaled_k * ((dir_norm - r) / dir_norm) * dx_diff;
287 }
288
289 df[id1] += scaled_df;
290 df[id2] -= scaled_df;
291 }
292 }
293 }
294
295 virtual btDeformableLagrangianForceType getForceType()
296 {
297 return BT_MASSSPRING_FORCE;
298 }
299};
300
301#endif /* btMassSpring_h */
btDeformableLagrangianForce.h
btDeformableLagrangianForceType
btDeformableLagrangianForceType
Definition: btDeformableLagrangianForce.h:24
BT_MASSSPRING_FORCE
@ BT_MASSSPRING_FORCE
Definition: btDeformableLagrangianForce.h:26
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
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
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:46
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
btCollisionObject::isActive
bool isActive() const
Definition: btCollisionObject.h:306
btDeformableLagrangianForce
Definition: btDeformableLagrangianForce.h:39
btDeformableLagrangianForce::getNumNodes
virtual int getNumNodes()
Definition: btDeformableLagrangianForce.h:78
btDeformableLagrangianForce::m_softBodies
btAlignedObjectArray< btSoftBody * > m_softBodies
Definition: btDeformableLagrangianForce.h:42
btDeformableMassSpringForce
Definition: btDeformableMassSpringForce.h:22
btDeformableMassSpringForce::getForceType
virtual btDeformableLagrangianForceType getForceType()
Definition: btDeformableMassSpringForce.h:295
btDeformableMassSpringForce::m_dampingStiffness
btScalar m_dampingStiffness
Definition: btDeformableMassSpringForce.h:26
btDeformableMassSpringForce::addScaledElasticForceDifferential
virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack &dx, TVStack &df)
Definition: btDeformableMassSpringForce.h:257
btDeformableMassSpringForce::m_momentum_conserving
bool m_momentum_conserving
Definition: btDeformableMassSpringForce.h:25
btDeformableMassSpringForce::TVStack
btAlignedObjectArray< btVector3 > TVStack
Definition: btDeformableMassSpringForce.h:29
btDeformableMassSpringForce::m_bendingStiffness
btScalar m_bendingStiffness
Definition: btDeformableMassSpringForce.h:26
btDeformableMassSpringForce::addScaledForces
virtual void addScaledForces(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:41
btDeformableMassSpringForce::addScaledDampingForce
virtual void addScaledDampingForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:52
btDeformableMassSpringForce::addScaledDampingForceDifferential
virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack &dv, TVStack &df)
Definition: btDeformableMassSpringForce.h:119
btDeformableMassSpringForce::m_elasticStiffness
btScalar m_elasticStiffness
Definition: btDeformableMassSpringForce.h:26
btDeformableMassSpringForce::addScaledElasticForce
virtual void addScaledElasticForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:88
btDeformableMassSpringForce::buildDampingForceDifferentialDiagonal
virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack &diagA)
Definition: btDeformableMassSpringForce.h:153
btDeformableMassSpringForce::btDeformableMassSpringForce
btDeformableMassSpringForce()
Definition: btDeformableMassSpringForce.h:30
btDeformableMassSpringForce::totalDampingEnergy
virtual double totalDampingEnergy(btScalar dt)
Definition: btDeformableMassSpringForce.h:224
btDeformableMassSpringForce::btDeformableMassSpringForce
btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular=true, double bending_k=-1)
Definition: btDeformableMassSpringForce.h:33
btDeformableMassSpringForce::totalElasticEnergy
virtual double totalElasticEnergy(btScalar dt)
Definition: btDeformableMassSpringForce.h:199
btDeformableMassSpringForce::addScaledExplicitForce
virtual void addScaledExplicitForce(btScalar scale, TVStack &force)
Definition: btDeformableMassSpringForce.h:47
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
btSoftBody::m_links
tLinkArray m_links
Definition: btSoftBody.h:816
btSoftBody::m_nodes
tNodeArray m_nodes
Definition: btSoftBody.h:814
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btSoftBody::Link
Definition: btSoftBody.h:288
btSoftBody::Link::m_bbending
int m_bbending
Definition: btSoftBody.h:292
btSoftBody::Link::m_n
Node * m_n[2]
Definition: btSoftBody.h:290
btSoftBody::Link::m_rl
btScalar m_rl
Definition: btSoftBody.h:291
btSoftBody::Node
Definition: btSoftBody.h:268
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:275
btSoftBody::Node::m_x
btVector3 m_x
Definition: btSoftBody.h:269
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:271
btSoftBody::Node::m_q
btVector3 m_q
Definition: btSoftBody.h:270
btSoftBody::Node::index
int index
Definition: btSoftBody.h:280