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 #include "btDeformableContactProjection.h"
17 #include "btDeformableMultiBodyDynamicsWorld.h"
18 #include <algorithm>
19 #include <cmath>
update(btCollisionObject ** deformableBodies,int numDeformableBodies,const btContactSolverInfo & infoGlobal)20 btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
21 {
22 btScalar residualSquare = 0;
23 for (int i = 0; i < numDeformableBodies; ++i)
24 {
25 for (int j = 0; j < m_softBodies.size(); ++j)
26 {
27 btCollisionObject* psb = m_softBodies[j];
28 if (psb != deformableBodies[i])
29 {
30 continue;
31 }
32 for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
33 {
34 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
35 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
36 residualSquare = btMax(residualSquare, localResidualSquare);
37 }
38 for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
39 {
40 btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
41 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
42 residualSquare = btMax(residualSquare, localResidualSquare);
43 }
44 for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
45 {
46 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
47 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
48 residualSquare = btMax(residualSquare, localResidualSquare);
49 }
50 for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
51 {
52 btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
53 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
54 residualSquare = btMax(residualSquare, localResidualSquare);
55 }
56 }
57 }
58 return residualSquare;
59 }
60
solveSplitImpulse(btCollisionObject ** deformableBodies,int numDeformableBodies,const btContactSolverInfo & infoGlobal)61 btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
62 {
63 btScalar residualSquare = 0;
64 for (int i = 0; i < numDeformableBodies; ++i)
65 {
66 for (int j = 0; j < m_softBodies.size(); ++j)
67 {
68 btCollisionObject* psb = m_softBodies[j];
69 if (psb != deformableBodies[i])
70 {
71 continue;
72 }
73 for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
74 {
75 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
76 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
77 residualSquare = btMax(residualSquare, localResidualSquare);
78 }
79 for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
80 {
81 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
82 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
83 residualSquare = btMax(residualSquare, localResidualSquare);
84 }
85 }
86 }
87 return residualSquare;
88 }
89
setConstraints(const btContactSolverInfo & infoGlobal)90 void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
91 {
92 BT_PROFILE("setConstraints");
93 for (int i = 0; i < m_softBodies.size(); ++i)
94 {
95 btSoftBody* psb = m_softBodies[i];
96 if (!psb->isActive())
97 {
98 continue;
99 }
100
101 // set Dirichlet constraint
102 for (int j = 0; j < psb->m_nodes.size(); ++j)
103 {
104 if (psb->m_nodes[j].m_im == 0)
105 {
106 btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
107 m_staticConstraints[i].push_back(static_constraint);
108 }
109 }
110
111 // set up deformable anchors
112 for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
113 {
114 btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
115 // skip fixed points
116 if (anchor.m_node->m_im == 0)
117 {
118 continue;
119 }
120 anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
121 btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
122 m_nodeAnchorConstraints[i].push_back(constraint);
123 }
124
125 // set Deformable Node vs. Rigid constraint
126 for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
127 {
128 const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
129 // skip fixed points
130 if (contact.m_node->m_im == 0)
131 {
132 continue;
133 }
134 btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
135 m_nodeRigidConstraints[i].push_back(constraint);
136 }
137
138 // set Deformable Face vs. Rigid constraint
139 for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
140 {
141 const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
142 // skip fixed faces
143 if (contact.m_c2 == 0)
144 {
145 continue;
146 }
147 btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting);
148 m_faceRigidConstraints[i].push_back(constraint);
149 }
150 }
151 }
152
project(TVStack & x)153 void btDeformableContactProjection::project(TVStack& x)
154 {
155 #ifndef USE_MGS
156 const int dim = 3;
157 for (int index = 0; index < m_projectionsDict.size(); ++index)
158 {
159 btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
160 size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
161 if (projectionDirs.size() >= dim)
162 {
163 // static node
164 x[i].setZero();
165 continue;
166 }
167 else if (projectionDirs.size() == 2)
168 {
169 btVector3 dir0 = projectionDirs[0];
170 btVector3 dir1 = projectionDirs[1];
171 btVector3 free_dir = btCross(dir0, dir1);
172 if (free_dir.safeNorm() < SIMD_EPSILON)
173 {
174 x[i] -= x[i].dot(dir0) * dir0;
175 }
176 else
177 {
178 free_dir.normalize();
179 x[i] = x[i].dot(free_dir) * free_dir;
180 }
181 }
182 else
183 {
184 btAssert(projectionDirs.size() == 1);
185 btVector3 dir0 = projectionDirs[0];
186 x[i] -= x[i].dot(dir0) * dir0;
187 }
188 }
189 #else
190 btReducedVector p(x.size());
191 for (int i = 0; i < m_projections.size(); ++i)
192 {
193 p += (m_projections[i].dot(x) * m_projections[i]);
194 }
195 for (int i = 0; i < p.m_indices.size(); ++i)
196 {
197 x[p.m_indices[i]] -= p.m_vecs[i];
198 }
199 #endif
200 }
201
setProjection()202 void btDeformableContactProjection::setProjection()
203 {
204 #ifndef USE_MGS
205 BT_PROFILE("btDeformableContactProjection::setProjection");
206 btAlignedObjectArray<btVector3> units;
207 units.push_back(btVector3(1, 0, 0));
208 units.push_back(btVector3(0, 1, 0));
209 units.push_back(btVector3(0, 0, 1));
210 for (int i = 0; i < m_softBodies.size(); ++i)
211 {
212 btSoftBody* psb = m_softBodies[i];
213 if (!psb->isActive())
214 {
215 continue;
216 }
217 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
218 {
219 int index = m_staticConstraints[i][j].m_node->index;
220 m_staticConstraints[i][j].m_node->m_constrained = true;
221 if (m_projectionsDict.find(index) == NULL)
222 {
223 m_projectionsDict.insert(index, units);
224 }
225 else
226 {
227 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
228 for (int k = 0; k < 3; ++k)
229 {
230 projections.push_back(units[k]);
231 }
232 }
233 }
234 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
235 {
236 int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
237 m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
238 if (m_projectionsDict.find(index) == NULL)
239 {
240 m_projectionsDict.insert(index, units);
241 }
242 else
243 {
244 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
245 for (int k = 0; k < 3; ++k)
246 {
247 projections.push_back(units[k]);
248 }
249 }
250 }
251 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
252 {
253 int index = m_nodeRigidConstraints[i][j].m_node->index;
254 m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
255 if (m_nodeRigidConstraints[i][j].m_binding)
256 {
257 if (m_nodeRigidConstraints[i][j].m_static)
258 {
259 if (m_projectionsDict.find(index) == NULL)
260 {
261 m_projectionsDict.insert(index, units);
262 }
263 else
264 {
265 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
266 for (int k = 0; k < 3; ++k)
267 {
268 projections.push_back(units[k]);
269 }
270 }
271 }
272 else
273 {
274 if (m_projectionsDict.find(index) == NULL)
275 {
276 btAlignedObjectArray<btVector3> projections;
277 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
278 m_projectionsDict.insert(index, projections);
279 }
280 else
281 {
282 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
283 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
284 }
285 }
286 }
287 }
288 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
289 {
290 const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
291 if (m_faceRigidConstraints[i][j].m_binding)
292 {
293 for (int k = 0; k < 3; ++k)
294 {
295 face->m_n[k]->m_constrained = true;
296 }
297 }
298 for (int k = 0; k < 3; ++k)
299 {
300 btSoftBody::Node* node = face->m_n[k];
301 int index = node->index;
302 if (m_faceRigidConstraints[i][j].m_static)
303 {
304 if (m_projectionsDict.find(index) == NULL)
305 {
306 m_projectionsDict.insert(index, units);
307 }
308 else
309 {
310 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
311 for (int l = 0; l < 3; ++l)
312 {
313 projections.push_back(units[l]);
314 }
315 }
316 }
317 else
318 {
319 if (m_projectionsDict.find(index) == NULL)
320 {
321 btAlignedObjectArray<btVector3> projections;
322 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
323 m_projectionsDict.insert(index, projections);
324 }
325 else
326 {
327 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
328 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
329 }
330 }
331 }
332 }
333 }
334 #else
335 int dof = 0;
336 for (int i = 0; i < m_softBodies.size(); ++i)
337 {
338 dof += m_softBodies[i]->m_nodes.size();
339 }
340 for (int i = 0; i < m_softBodies.size(); ++i)
341 {
342 btSoftBody* psb = m_softBodies[i];
343 if (!psb->isActive())
344 {
345 continue;
346 }
347 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
348 {
349 int index = m_staticConstraints[i][j].m_node->index;
350 m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
351 btAlignedObjectArray<int> indices;
352 btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
353 indices.push_back(index);
354 vecs1.push_back(btVector3(1, 0, 0));
355 vecs2.push_back(btVector3(0, 1, 0));
356 vecs3.push_back(btVector3(0, 0, 1));
357 m_projections.push_back(btReducedVector(dof, indices, vecs1));
358 m_projections.push_back(btReducedVector(dof, indices, vecs2));
359 m_projections.push_back(btReducedVector(dof, indices, vecs3));
360 }
361
362 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
363 {
364 int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
365 m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
366 btAlignedObjectArray<int> indices;
367 btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
368 indices.push_back(index);
369 vecs1.push_back(btVector3(1, 0, 0));
370 vecs2.push_back(btVector3(0, 1, 0));
371 vecs3.push_back(btVector3(0, 0, 1));
372 m_projections.push_back(btReducedVector(dof, indices, vecs1));
373 m_projections.push_back(btReducedVector(dof, indices, vecs2));
374 m_projections.push_back(btReducedVector(dof, indices, vecs3));
375 }
376 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
377 {
378 int index = m_nodeRigidConstraints[i][j].m_node->index;
379 m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
380 btAlignedObjectArray<int> indices;
381 indices.push_back(index);
382 btAlignedObjectArray<btVector3> vecs1, vecs2, vecs3;
383 if (m_nodeRigidConstraints[i][j].m_static)
384 {
385 vecs1.push_back(btVector3(1, 0, 0));
386 vecs2.push_back(btVector3(0, 1, 0));
387 vecs3.push_back(btVector3(0, 0, 1));
388 m_projections.push_back(btReducedVector(dof, indices, vecs1));
389 m_projections.push_back(btReducedVector(dof, indices, vecs2));
390 m_projections.push_back(btReducedVector(dof, indices, vecs3));
391 }
392 else
393 {
394 vecs1.push_back(m_nodeRigidConstraints[i][j].m_normal);
395 m_projections.push_back(btReducedVector(dof, indices, vecs1));
396 }
397 }
398 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
399 {
400 const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
401 btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
402 btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
403 for (int k = 0; k < 3; ++k)
404 {
405 face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
406 }
407 if (m_faceRigidConstraints[i][j].m_static)
408 {
409 for (int l = 0; l < 3; ++l)
410 {
411 btReducedVector rv(dof);
412 for (int k = 0; k < 3; ++k)
413 {
414 rv.m_indices.push_back(face->m_n[k]->index);
415 btVector3 v(0, 0, 0);
416 v[l] = bary[k];
417 rv.m_vecs.push_back(v);
418 rv.sort();
419 }
420 m_projections.push_back(rv);
421 }
422 }
423 else
424 {
425 btReducedVector rv(dof);
426 for (int k = 0; k < 3; ++k)
427 {
428 rv.m_indices.push_back(face->m_n[k]->index);
429 rv.m_vecs.push_back(bary[k] * m_faceRigidConstraints[i][j].m_normal);
430 rv.sort();
431 }
432 m_projections.push_back(rv);
433 }
434 }
435 }
436 btModifiedGramSchmidt<btReducedVector> mgs(m_projections);
437 mgs.solve();
438 m_projections = mgs.m_out;
439 #endif
440 }
441
checkConstraints(const TVStack & x)442 void btDeformableContactProjection::checkConstraints(const TVStack& x)
443 {
444 for (int i = 0; i < m_lagrangeMultipliers.size(); ++i)
445 {
446 btVector3 d(0, 0, 0);
447 const LagrangeMultiplier& lm = m_lagrangeMultipliers[i];
448 for (int j = 0; j < lm.m_num_constraints; ++j)
449 {
450 for (int k = 0; k < lm.m_num_nodes; ++k)
451 {
452 d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
453 }
454 }
455 // printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
456 // printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
457 }
458 }
459
setLagrangeMultiplier()460 void btDeformableContactProjection::setLagrangeMultiplier()
461 {
462 for (int i = 0; i < m_softBodies.size(); ++i)
463 {
464 btSoftBody* psb = m_softBodies[i];
465 if (!psb->isActive())
466 {
467 continue;
468 }
469 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
470 {
471 int index = m_staticConstraints[i][j].m_node->index;
472 m_staticConstraints[i][j].m_node->m_constrained = true;
473 LagrangeMultiplier lm;
474 lm.m_num_nodes = 1;
475 lm.m_indices[0] = index;
476 lm.m_weights[0] = 1.0;
477 lm.m_num_constraints = 3;
478 lm.m_dirs[0] = btVector3(1, 0, 0);
479 lm.m_dirs[1] = btVector3(0, 1, 0);
480 lm.m_dirs[2] = btVector3(0, 0, 1);
481 m_lagrangeMultipliers.push_back(lm);
482 }
483 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
484 {
485 int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
486 m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
487 LagrangeMultiplier lm;
488 lm.m_num_nodes = 1;
489 lm.m_indices[0] = index;
490 lm.m_weights[0] = 1.0;
491 lm.m_num_constraints = 3;
492 lm.m_dirs[0] = btVector3(1, 0, 0);
493 lm.m_dirs[1] = btVector3(0, 1, 0);
494 lm.m_dirs[2] = btVector3(0, 0, 1);
495 m_lagrangeMultipliers.push_back(lm);
496 }
497
498 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
499 {
500 if (!m_nodeRigidConstraints[i][j].m_binding)
501 {
502 continue;
503 }
504 int index = m_nodeRigidConstraints[i][j].m_node->index;
505 m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
506 LagrangeMultiplier lm;
507 lm.m_num_nodes = 1;
508 lm.m_indices[0] = index;
509 lm.m_weights[0] = 1.0;
510 if (m_nodeRigidConstraints[i][j].m_static)
511 {
512 lm.m_num_constraints = 3;
513 lm.m_dirs[0] = btVector3(1, 0, 0);
514 lm.m_dirs[1] = btVector3(0, 1, 0);
515 lm.m_dirs[2] = btVector3(0, 0, 1);
516 }
517 else
518 {
519 lm.m_num_constraints = 1;
520 lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal;
521 }
522 m_lagrangeMultipliers.push_back(lm);
523 }
524
525 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
526 {
527 if (!m_faceRigidConstraints[i][j].m_binding)
528 {
529 continue;
530 }
531 btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
532
533 btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
534 LagrangeMultiplier lm;
535 lm.m_num_nodes = 3;
536
537 for (int k = 0; k < 3; ++k)
538 {
539 face->m_n[k]->m_constrained = true;
540 lm.m_indices[k] = face->m_n[k]->index;
541 lm.m_weights[k] = bary[k];
542 }
543 if (m_faceRigidConstraints[i][j].m_static)
544 {
545 face->m_pcontact[3] = 1;
546 lm.m_num_constraints = 3;
547 lm.m_dirs[0] = btVector3(1, 0, 0);
548 lm.m_dirs[1] = btVector3(0, 1, 0);
549 lm.m_dirs[2] = btVector3(0, 0, 1);
550 }
551 else
552 {
553 face->m_pcontact[3] = 0;
554 lm.m_num_constraints = 1;
555 lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
556 }
557 m_lagrangeMultipliers.push_back(lm);
558 }
559 }
560 }
561
562 //
applyDynamicFriction(TVStack & f)563 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
564 {
565 for (int i = 0; i < m_softBodies.size(); ++i)
566 {
567 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
568 {
569 const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
570 const btSoftBody::Node* node = constraint.m_node;
571 if (node->m_im != 0)
572 {
573 int index = node->index;
574 f[index] += constraint.getDv(node) * (1. / node->m_im);
575 }
576 }
577 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
578 {
579 const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
580 const btSoftBody::Face* face = constraint.getContact()->m_face;
581 for (int k = 0; k < 3; ++k)
582 {
583 const btSoftBody::Node* node = face->m_n[k];
584 if (node->m_im != 0)
585 {
586 int index = node->index;
587 f[index] += constraint.getDv(node) * (1. / node->m_im);
588 }
589 }
590 }
591 for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
592 {
593 const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
594 const btSoftBody::Face* face = constraint.getContact()->m_face;
595 const btSoftBody::Node* node = constraint.getContact()->m_node;
596 if (node->m_im != 0)
597 {
598 int index = node->index;
599 f[index] += constraint.getDv(node) * (1. / node->m_im);
600 }
601 for (int k = 0; k < 3; ++k)
602 {
603 const btSoftBody::Node* node = face->m_n[k];
604 if (node->m_im != 0)
605 {
606 int index = node->index;
607 f[index] += constraint.getDv(node) * (1. / node->m_im);
608 }
609 }
610 }
611 }
612 }
613
reinitialize(bool nodeUpdated)614 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
615 {
616 int N = m_softBodies.size();
617 if (nodeUpdated)
618 {
619 m_staticConstraints.resize(N);
620 m_nodeAnchorConstraints.resize(N);
621 m_nodeRigidConstraints.resize(N);
622 m_faceRigidConstraints.resize(N);
623 m_deformableConstraints.resize(N);
624 }
625 for (int i = 0; i < N; ++i)
626 {
627 m_staticConstraints[i].clear();
628 m_nodeAnchorConstraints[i].clear();
629 m_nodeRigidConstraints[i].clear();
630 m_faceRigidConstraints[i].clear();
631 m_deformableConstraints[i].clear();
632 }
633 #ifndef USE_MGS
634 m_projectionsDict.clear();
635 #else
636 m_projections.clear();
637 #endif
638 m_lagrangeMultipliers.clear();
639 }
640