2 Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
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.
16 #ifndef BT_MASS_SPRING_H
17 #define BT_MASS_SPRING_H
19 #include "btDeformableLagrangianForce.h"
21 class btDeformableMassSpringForce : public btDeformableLagrangianForce
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;
29 typedef btAlignedObjectArray<btVector3> TVStack;
30 btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05)
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)
35 if (m_bendingStiffness < btScalar(0))
37 m_bendingStiffness = m_elasticStiffness;
41 virtual void addScaledForces(btScalar scale, TVStack& force)
43 addScaledDampingForce(scale, force);
44 addScaledElasticForce(scale, force);
47 virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
49 addScaledElasticForce(scale, force);
52 virtual void addScaledDampingForce(btScalar scale, TVStack& force)
54 int numNodes = getNumNodes();
55 btAssert(numNodes <= force.size());
56 for (int i = 0; i < m_softBodies.size(); ++i)
58 const btSoftBody* psb = m_softBodies[i];
63 for (int j = 0; j < psb->m_links.size(); ++j)
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;
72 btVector3 v_diff = (node2->m_v - node1->m_v);
73 btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
74 if (m_momentum_conserving)
76 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
78 btVector3 dir = (node2->m_x - node1->m_x).normalized();
79 scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
82 force[id1] += scaled_force;
83 force[id2] -= scaled_force;
88 virtual void addScaledElasticForce(btScalar scale, TVStack& force)
90 int numNodes = getNumNodes();
91 btAssert(numNodes <= force.size());
92 for (int i = 0; i < m_softBodies.size(); ++i)
94 const btSoftBody* psb = m_softBodies[i];
99 for (int j = 0; j < psb->m_links.size(); ++j)
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;
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;
119 virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
121 // implicit damping force differential
122 for (int i = 0; i < m_softBodies.size(); ++i)
124 btSoftBody* psb = m_softBodies[i];
125 if (!psb->isActive())
129 btScalar scaled_k_damp = m_dampingStiffness * scale;
130 for (int j = 0; j < psb->m_links.size(); ++j)
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;
138 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
139 if (m_momentum_conserving)
141 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
143 btVector3 dir = (node2->m_x - node1->m_x).normalized();
144 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
147 df[id1] += local_scaled_df;
148 df[id2] -= local_scaled_df;
153 virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA)
155 // implicit damping force differential
156 for (int i = 0; i < m_softBodies.size(); ++i)
158 btSoftBody* psb = m_softBodies[i];
159 if (!psb->isActive())
163 btScalar scaled_k_damp = m_dampingStiffness * scale;
164 for (int j = 0; j < psb->m_links.size(); ++j)
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)
173 if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
175 btVector3 dir = (node2->m_x - node1->m_x).normalized();
176 for (int d = 0; d < 3; ++d)
179 diagA[id1][d] -= scaled_k_damp * dir[d] * dir[d];
181 diagA[id2][d] -= scaled_k_damp * dir[d] * dir[d];
187 for (int d = 0; d < 3; ++d)
190 diagA[id1][d] -= scaled_k_damp;
192 diagA[id2][d] -= scaled_k_damp;
199 virtual double totalElasticEnergy(btScalar dt)
202 for (int i = 0; i < m_softBodies.size(); ++i)
204 const btSoftBody* psb = m_softBodies[i];
205 if (!psb->isActive())
209 for (int j = 0; j < psb->m_links.size(); ++j)
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;
217 btVector3 dir = (node2->m_q - node1->m_q);
218 energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() - r);
224 virtual double totalDampingEnergy(btScalar dt)
228 for (int i = 0; i < m_softBodies.size(); ++i)
230 btSoftBody* psb = m_softBodies[i];
231 if (!psb->isActive())
235 for (int j = 0; j < psb->m_nodes.size(); ++j)
237 sz = btMax(sz, psb->m_nodes[j].index);
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)
247 btSoftBody* psb = m_softBodies[i];
248 for (int j = 0; j < psb->m_nodes.size(); ++j)
250 const btSoftBody::Node& node = psb->m_nodes[j];
251 energy -= dampingForce[node.index].dot(node.m_v) / dt;
257 virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
259 // implicit damping force differential
260 for (int i = 0; i < m_softBodies.size(); ++i)
262 const btSoftBody* psb = m_softBodies[i];
263 if (!psb->isActive())
267 for (int j = 0; j < psb->m_links.size(); ++j)
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;
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)
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;
289 df[id1] += scaled_df;
290 df[id2] -= scaled_df;
295 virtual btDeformableLagrangianForceType getForceType()
297 return BT_MASSSPRING_FORCE;
301 #endif /* btMassSpring_h */