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

Bullet Collision Detection & Physics Library

btPreconditioner.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_PRECONDITIONER_H
17 #define BT_PRECONDITIONER_H
18
19 class Preconditioner
20 {
21 public:
22 typedef btAlignedObjectArray<btVector3> TVStack;
23 virtual void operator()(const TVStack& x, TVStack& b) = 0;
24 virtual void reinitialize(bool nodeUpdated) = 0;
25 virtual ~Preconditioner() {}
26 };
27
28 class DefaultPreconditioner : public Preconditioner
29 {
30 public:
31 virtual void operator()(const TVStack& x, TVStack& b)
32 {
33 btAssert(b.size() == x.size());
34 for (int i = 0; i < b.size(); ++i)
35 b[i] = x[i];
36 }
37 virtual void reinitialize(bool nodeUpdated)
38 {
39 }
40
41 virtual ~DefaultPreconditioner() {}
42 };
43
44 class MassPreconditioner : public Preconditioner
45 {
46 btAlignedObjectArray<btScalar> m_inv_mass;
47 const btAlignedObjectArray<btSoftBody*>& m_softBodies;
48
49 public:
50 MassPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies)
51 : m_softBodies(softBodies)
52 {
53 }
54
55 virtual void reinitialize(bool nodeUpdated)
56 {
57 if (nodeUpdated)
58 {
59 m_inv_mass.clear();
60 for (int i = 0; i < m_softBodies.size(); ++i)
61 {
62 btSoftBody* psb = m_softBodies[i];
63 for (int j = 0; j < psb->m_nodes.size(); ++j)
64 m_inv_mass.push_back(psb->m_nodes[j].m_im);
65 }
66 }
67 }
68
69 virtual void operator()(const TVStack& x, TVStack& b)
70 {
71 btAssert(b.size() == x.size());
72 btAssert(m_inv_mass.size() <= x.size());
73 for (int i = 0; i < m_inv_mass.size(); ++i)
74 {
75 b[i] = x[i] * m_inv_mass[i];
76 }
77 for (int i = m_inv_mass.size(); i < b.size(); ++i)
78 {
79 b[i] = x[i];
80 }
81 }
82 };
83
84 class KKTPreconditioner : public Preconditioner
85 {
86 const btAlignedObjectArray<btSoftBody*>& m_softBodies;
87 const btDeformableContactProjection& m_projections;
88 const btAlignedObjectArray<btDeformableLagrangianForce*>& m_lf;
89 TVStack m_inv_A, m_inv_S;
90 const btScalar& m_dt;
91 const bool& m_implicit;
92
93 public:
94 KKTPreconditioner(const btAlignedObjectArray<btSoftBody*>& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray<btDeformableLagrangianForce*>& lf, const btScalar& dt, const bool& implicit)
95 : m_softBodies(softBodies), m_projections(projections), m_lf(lf), m_dt(dt), m_implicit(implicit)
96 {
97 }
98
99 virtual void reinitialize(bool nodeUpdated)
100 {
101 if (nodeUpdated)
102 {
103 int num_nodes = 0;
104 for (int i = 0; i < m_softBodies.size(); ++i)
105 {
106 btSoftBody* psb = m_softBodies[i];
107 num_nodes += psb->m_nodes.size();
108 }
109 m_inv_A.resize(num_nodes);
110 }
111 buildDiagonalA(m_inv_A);
112 for (int i = 0; i < m_inv_A.size(); ++i)
113 {
114 // printf("A[%d] = %f, %f, %f \n", i, m_inv_A[i][0], m_inv_A[i][1], m_inv_A[i][2]);
115 for (int d = 0; d < 3; ++d)
116 {
117 m_inv_A[i][d] = (m_inv_A[i][d] == 0) ? 0.0 : 1.0 / m_inv_A[i][d];
118 }
119 }
120 m_inv_S.resize(m_projections.m_lagrangeMultipliers.size());
121 // printf("S.size() = %d \n", m_inv_S.size());
122 buildDiagonalS(m_inv_A, m_inv_S);
123 for (int i = 0; i < m_inv_S.size(); ++i)
124 {
125 // printf("S[%d] = %f, %f, %f \n", i, m_inv_S[i][0], m_inv_S[i][1], m_inv_S[i][2]);
126 for (int d = 0; d < 3; ++d)
127 {
128 m_inv_S[i][d] = (m_inv_S[i][d] == 0) ? 0.0 : 1.0 / m_inv_S[i][d];
129 }
130 }
131 }
132
133 void buildDiagonalA(TVStack& diagA) const
134 {
135 size_t counter = 0;
136 for (int i = 0; i < m_softBodies.size(); ++i)
137 {
138 btSoftBody* psb = m_softBodies[i];
139 for (int j = 0; j < psb->m_nodes.size(); ++j)
140 {
141 const btSoftBody::Node& node = psb->m_nodes[j];
142 diagA[counter] = (node.m_im == 0) ? btVector3(0, 0, 0) : btVector3(1.0 / node.m_im, 1.0 / node.m_im, 1.0 / node.m_im);
143 ++counter;
144 }
145 }
146 if (m_implicit)
147 {
148 printf("implicit not implemented\n");
149 btAssert(false);
150 }
151 for (int i = 0; i < m_lf.size(); ++i)
152 {
153 // add damping matrix
154 m_lf[i]->buildDampingForceDifferentialDiagonal(-m_dt, diagA);
155 }
156 }
157
158 void buildDiagonalS(const TVStack& inv_A, TVStack& diagS)
159 {
160 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
161 {
162 // S[k,k] = e_k^T * C A_d^-1 C^T * e_k
163 const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
164 btVector3& t = diagS[c];
165 t.setZero();
166 for (int j = 0; j < lm.m_num_constraints; ++j)
167 {
168 for (int i = 0; i < lm.m_num_nodes; ++i)
169 {
170 for (int d = 0; d < 3; ++d)
171 {
172 t[j] += inv_A[lm.m_indices[i]][d] * lm.m_dirs[j][d] * lm.m_dirs[j][d] * lm.m_weights[i] * lm.m_weights[i];
173 }
174 }
175 }
176 }
177 }
178 //#define USE_FULL_PRECONDITIONER
179 #ifndef USE_FULL_PRECONDITIONER
180 virtual void operator()(const TVStack& x, TVStack& b)
181 {
182 btAssert(b.size() == x.size());
183 for (int i = 0; i < m_inv_A.size(); ++i)
184 {
185 b[i] = x[i] * m_inv_A[i];
186 }
187 int offset = m_inv_A.size();
188 for (int i = 0; i < m_inv_S.size(); ++i)
189 {
190 b[i + offset] = x[i + offset] * m_inv_S[i];
191 }
192 }
193 #else
194 virtual void operator()(const TVStack& x, TVStack& b)
195 {
196 btAssert(b.size() == x.size());
197 int offset = m_inv_A.size();
198
199 for (int i = 0; i < m_inv_A.size(); ++i)
200 {
201 b[i] = x[i] * m_inv_A[i];
202 }
203
204 for (int i = 0; i < m_inv_S.size(); ++i)
205 {
206 b[i + offset].setZero();
207 }
208
209 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
210 {
211 const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
212 // C * x
213 for (int d = 0; d < lm.m_num_constraints; ++d)
214 {
215 for (int i = 0; i < lm.m_num_nodes; ++i)
216 {
217 b[offset + c][d] += lm.m_weights[i] * b[lm.m_indices[i]].dot(lm.m_dirs[d]);
218 }
219 }
220 }
221
222 for (int i = 0; i < m_inv_S.size(); ++i)
223 {
224 b[i + offset] = b[i + offset] * m_inv_S[i];
225 }
226
227 for (int i = 0; i < m_inv_A.size(); ++i)
228 {
229 b[i].setZero();
230 }
231
232 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
233 {
234 // C^T * lambda
235 const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
236 for (int i = 0; i < lm.m_num_nodes; ++i)
237 {
238 for (int j = 0; j < lm.m_num_constraints; ++j)
239 {
240 b[lm.m_indices[i]] += b[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
241 }
242 }
243 }
244
245 for (int i = 0; i < m_inv_A.size(); ++i)
246 {
247 b[i] = (x[i] - b[i]) * m_inv_A[i];
248 }
249
250 TVStack t;
251 t.resize(b.size());
252 for (int i = 0; i < m_inv_S.size(); ++i)
253 {
254 t[i + offset] = x[i + offset] * m_inv_S[i];
255 }
256 for (int i = 0; i < m_inv_A.size(); ++i)
257 {
258 t[i].setZero();
259 }
260 for (int c = 0; c < m_projections.m_lagrangeMultipliers.size(); ++c)
261 {
262 // C^T * lambda
263 const LagrangeMultiplier& lm = m_projections.m_lagrangeMultipliers[c];
264 for (int i = 0; i < lm.m_num_nodes; ++i)
265 {
266 for (int j = 0; j < lm.m_num_constraints; ++j)
267 {
268 t[lm.m_indices[i]] += t[offset + c][j] * lm.m_weights[i] * lm.m_dirs[j];
269 }
270 }
271 }
272 for (int i = 0; i < m_inv_A.size(); ++i)
273 {
274 b[i] += t[i] * m_inv_A[i];
275 }
276
277 for (int i = 0; i < m_inv_S.size(); ++i)
278 {
279 b[i + offset] -= x[i + offset] * m_inv_S[i];
280 }
281 }
282 #endif
283 };
284
285 #endif /* BT_PRECONDITIONER_H */
LagrangeMultiplier::m_num_constraints
int m_num_constraints
Definition: btDeformableContactProjection.h:30
Preconditioner
Definition: btPreconditioner.h:20
MassPreconditioner::MassPreconditioner
MassPreconditioner(const btAlignedObjectArray< btSoftBody * > &softBodies)
Definition: btPreconditioner.h:50
Preconditioner::reinitialize
virtual void reinitialize(bool nodeUpdated)=0
MassPreconditioner::operator()
virtual void operator()(const TVStack &x, TVStack &b)
Definition: btPreconditioner.h:69
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
Preconditioner::operator()
virtual void operator()(const TVStack &x, TVStack &b)=0
btDeformableContactProjection
Definition: btDeformableContactProjection.h:38
MassPreconditioner::m_softBodies
const btAlignedObjectArray< btSoftBody * > & m_softBodies
Definition: btPreconditioner.h:47
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition: btAlignedObjectArray.h:176
LagrangeMultiplier::m_dirs
btVector3 m_dirs[3]
Definition: btDeformableContactProjection.h:33
btVector3::setZero
void setZero()
Definition: btVector3.h:671
KKTPreconditioner::reinitialize
virtual void reinitialize(bool nodeUpdated)
Definition: btPreconditioner.h:99
btSoftBody::Node
Definition: btSoftBody.h:268
Preconditioner::~Preconditioner
virtual ~Preconditioner()
Definition: btPreconditioner.h:25
KKTPreconditioner::m_projections
const btDeformableContactProjection & m_projections
Definition: btPreconditioner.h:87
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btDeformableContactProjection::m_lagrangeMultipliers
btAlignedObjectArray< LagrangeMultiplier > m_lagrangeMultipliers
Definition: btDeformableContactProjection.h:52
LagrangeMultiplier::m_num_nodes
int m_num_nodes
Definition: btDeformableContactProjection.h:31
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
KKTPreconditioner::m_dt
const btScalar & m_dt
Definition: btPreconditioner.h:90
MassPreconditioner::m_inv_mass
btAlignedObjectArray< btScalar > m_inv_mass
Definition: btPreconditioner.h:46
KKTPreconditioner::m_inv_A
TVStack m_inv_A
Definition: btPreconditioner.h:89
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
KKTPreconditioner::buildDiagonalS
void buildDiagonalS(const TVStack &inv_A, TVStack &diagS)
Definition: btPreconditioner.h:158
KKTPreconditioner::KKTPreconditioner
KKTPreconditioner(const btAlignedObjectArray< btSoftBody * > &softBodies, const btDeformableContactProjection &projections, const btAlignedObjectArray< btDeformableLagrangianForce * > &lf, const btScalar &dt, const bool &implicit)
Definition: btPreconditioner.h:94
KKTPreconditioner::operator()
virtual void operator()(const TVStack &x, TVStack &b)
Definition: btPreconditioner.h:180
DefaultPreconditioner::reinitialize
virtual void reinitialize(bool nodeUpdated)
Definition: btPreconditioner.h:37
DefaultPreconditioner::~DefaultPreconditioner
virtual ~DefaultPreconditioner()
Definition: btPreconditioner.h:41
Preconditioner::TVStack
btAlignedObjectArray< btVector3 > TVStack
Definition: btPreconditioner.h:22
btAlignedObjectArray< btVector3 >
KKTPreconditioner::m_inv_S
TVStack m_inv_S
Definition: btPreconditioner.h:89
LagrangeMultiplier::m_indices
int m_indices[3]
Definition: btDeformableContactProjection.h:34
KKTPreconditioner
Definition: btPreconditioner.h:85
KKTPreconditioner::buildDiagonalA
void buildDiagonalA(TVStack &diagA) const
Definition: btPreconditioner.h:133
btSoftBody
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:75
LagrangeMultiplier::m_weights
btScalar m_weights[3]
Definition: btDeformableContactProjection.h:32
KKTPreconditioner::m_softBodies
const btAlignedObjectArray< btSoftBody * > & m_softBodies
Definition: btPreconditioner.h:86
KKTPreconditioner::m_lf
const btAlignedObjectArray< btDeformableLagrangianForce * > & m_lf
Definition: btPreconditioner.h:88
KKTPreconditioner::m_implicit
const bool & m_implicit
Definition: btPreconditioner.h:91
MassPreconditioner
Definition: btPreconditioner.h:45
MassPreconditioner::reinitialize
virtual void reinitialize(bool nodeUpdated)
Definition: btPreconditioner.h:55
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
LagrangeMultiplier
Definition: btDeformableContactProjection.h:29
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:275
DefaultPreconditioner::operator()
virtual void operator()(const TVStack &x, TVStack &b)
Definition: btPreconditioner.h:31
DefaultPreconditioner
Definition: btPreconditioner.h:29
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:812