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)20 btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
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();
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();
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();
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();
54 residualSquare = btMax(residualSquare, localResidualSquare);
55 }
56 }
57 }
58 return residualSquare;
59 }
60
splitImpulseSetup(const btContactSolverInfo & infoGlobal)61 void btDeformableContactProjection::splitImpulseSetup(const btContactSolverInfo& infoGlobal)
62 {
63 for (int i = 0; i < m_softBodies.size(); ++i)
64 {
65 // node constraints
66 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
67 {
68 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
69 constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
70 }
71 // face constraints
72 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
73 {
74 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
75 constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
76 }
77 }
78 }
79
solveSplitImpulse(const btContactSolverInfo & infoGlobal)80 btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
81 {
82 btScalar residualSquare = 0;
83 for (int i = 0; i < m_softBodies.size(); ++i)
84 {
85 // node constraints
86 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
87 {
88 btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
89 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
90 residualSquare = btMax(residualSquare, localResidualSquare);
91 }
92 // anchor constraints
93 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
94 {
95 btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[i][j];
96 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
97 residualSquare = btMax(residualSquare, localResidualSquare);
98 }
99 // face constraints
100 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
101 {
102 btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
103 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
104 residualSquare = btMax(residualSquare, localResidualSquare);
105 }
106
107 }
108 return residualSquare;
109 }
110
setConstraints()111 void btDeformableContactProjection::setConstraints()
112 {
113 BT_PROFILE("setConstraints");
114 for (int i = 0; i < m_softBodies.size(); ++i)
115 {
116 btSoftBody* psb = m_softBodies[i];
117 if (!psb->isActive())
118 {
119 continue;
120 }
121
122 // set Dirichlet constraint
123 for (int j = 0; j < psb->m_nodes.size(); ++j)
124 {
125 if (psb->m_nodes[j].m_im == 0)
126 {
127 btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
128 m_staticConstraints[i].push_back(static_constraint);
129 }
130 }
131
132 // set up deformable anchors
133 for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
134 {
135 btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
136 // skip fixed points
137 if (anchor.m_node->m_im == 0)
138 {
139 continue;
140 }
141 anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
142 btDeformableNodeAnchorConstraint constraint(anchor);
143 m_nodeAnchorConstraints[i].push_back(constraint);
144 }
145
146 // set Deformable Node vs. Rigid constraint
147 for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
148 {
149 const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
150 // skip fixed points
151 if (contact.m_node->m_im == 0)
152 {
153 continue;
154 }
155 btDeformableNodeRigidContactConstraint constraint(contact);
156 btVector3 va = constraint.getVa();
157 btVector3 vb = constraint.getVb();
158 const btVector3 vr = vb - va;
159 const btSoftBody::sCti& cti = contact.m_cti;
160 const btScalar dn = btDot(vr, cti.m_normal);
161 if (dn < SIMD_EPSILON)
162 {
163 m_nodeRigidConstraints[i].push_back(constraint);
164 }
165 }
166
167 // set Deformable Face vs. Rigid constraint
168 for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
169 {
170 const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
171 // skip fixed faces
172 if (contact.m_c2 == 0)
173 {
174 continue;
175 }
176 btDeformableFaceRigidContactConstraint constraint(contact);
177 btVector3 va = constraint.getVa();
178 btVector3 vb = constraint.getVb();
179 const btVector3 vr = vb - va;
180 const btSoftBody::sCti& cti = contact.m_cti;
181 const btScalar dn = btDot(vr, cti.m_normal);
182 if (dn < SIMD_EPSILON)
183 {
184 m_faceRigidConstraints[i].push_back(constraint);
185 }
186 }
187
188 // set Deformable Face vs. Deformable Node constraint
189 for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
190 {
191 const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
192
193 btDeformableFaceNodeContactConstraint constraint(contact);
194 btVector3 va = constraint.getVa();
195 btVector3 vb = constraint.getVb();
196 const btVector3 vr = vb - va;
197 const btScalar dn = btDot(vr, contact.m_normal);
198 if (dn > -SIMD_EPSILON)
199 {
200 m_deformableConstraints[i].push_back(constraint);
201 }
202 }
203 }
204 }
205
project(TVStack & x)206 void btDeformableContactProjection::project(TVStack& x)
207 {
208 const int dim = 3;
209 for (int index = 0; index < m_projectionsDict.size(); ++index)
210 {
211 btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
212 size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
213 if (projectionDirs.size() >= dim)
214 {
215 // static node
216 x[i].setZero();
217 continue;
218 }
219 else if (projectionDirs.size() == 2)
220 {
221 btVector3 dir0 = projectionDirs[0];
222 btVector3 dir1 = projectionDirs[1];
223 btVector3 free_dir = btCross(dir0, dir1);
224 if (free_dir.safeNorm() < SIMD_EPSILON)
225 {
226 x[i] -= x[i].dot(dir0) * dir0;
227 x[i] -= x[i].dot(dir1) * dir1;
228 }
229 else
230 {
231 free_dir.normalize();
232 x[i] = x[i].dot(free_dir) * free_dir;
233 }
234 }
235 else
236 {
237 btAssert(projectionDirs.size() == 1);
238 btVector3 dir0 = projectionDirs[0];
239 x[i] -= x[i].dot(dir0) * dir0;
240 }
241 }
242 }
243
setProjection()244 void btDeformableContactProjection::setProjection()
245 {
246 btAlignedObjectArray<btVector3> units;
247 units.push_back(btVector3(1,0,0));
248 units.push_back(btVector3(0,1,0));
249 units.push_back(btVector3(0,0,1));
250 for (int i = 0; i < m_softBodies.size(); ++i)
251 {
252 btSoftBody* psb = m_softBodies[i];
253 if (!psb->isActive())
254 {
255 continue;
256 }
257 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
258 {
259 int index = m_staticConstraints[i][j].m_node->index;
260 if (m_projectionsDict.find(index) == NULL)
261 {
262 m_projectionsDict.insert(index, units);
263 }
264 else
265 {
266 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
267 for (int k = 0; k < 3; ++k)
268 {
269 projections.push_back(units[k]);
270 }
271 }
272 }
273 for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
274 {
275 int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
276 if (m_projectionsDict.find(index) == NULL)
277 {
278 m_projectionsDict.insert(index, units);
279 }
280 else
281 {
282 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
283 for (int k = 0; k < 3; ++k)
284 {
285 projections.push_back(units[k]);
286 }
287 }
288 }
289 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
290 {
291 int index = m_nodeRigidConstraints[i][j].m_node->index;
292 if (m_nodeRigidConstraints[i][j].m_static)
293 {
294 if (m_projectionsDict.find(index) == NULL)
295 {
296 m_projectionsDict.insert(index, units);
297 }
298 else
299 {
300 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
301 for (int k = 0; k < 3; ++k)
302 {
303 projections.push_back(units[k]);
304 }
305 }
306 }
307 else
308 {
309 if (m_projectionsDict.find(index) == NULL)
310 {
311 btAlignedObjectArray<btVector3> projections;
312 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
313 m_projectionsDict.insert(index, projections);
314 }
315 else
316 {
317 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
318 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
319 }
320 }
321 }
322 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
323 {
324 const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
325 for (int k = 0; k < 3; ++k)
326 {
327 const btSoftBody::Node* node = face->m_n[k];
328 int index = node->index;
329 if (m_faceRigidConstraints[i][j].m_static)
330 {
331 if (m_projectionsDict.find(index) == NULL)
332 {
333 m_projectionsDict.insert(index, units);
334 }
335 else
336 {
337 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
338 for (int k = 0; k < 3; ++k)
339 {
340 projections.push_back(units[k]);
341 }
342 }
343 }
344 else
345 {
346 if (m_projectionsDict.find(index) == NULL)
347 {
348 btAlignedObjectArray<btVector3> projections;
349 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
350 m_projectionsDict.insert(index, projections);
351 }
352 else
353 {
354 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
355 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
356 }
357 }
358 }
359 }
360 for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
361 {
362 const btSoftBody::Face* face = m_deformableConstraints[i][j].m_face;
363 for (int k = 0; k < 3; ++k)
364 {
365 const btSoftBody::Node* node = face->m_n[k];
366 int index = node->index;
367 if (m_deformableConstraints[i][j].m_static)
368 {
369 if (m_projectionsDict.find(index) == NULL)
370 {
371 m_projectionsDict.insert(index, units);
372 }
373 else
374 {
375 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
376 for (int k = 0; k < 3; ++k)
377 {
378 projections.push_back(units[k]);
379 }
380 }
381 }
382 else
383 {
384 if (m_projectionsDict.find(index) == NULL)
385 {
386 btAlignedObjectArray<btVector3> projections;
387 projections.push_back(m_deformableConstraints[i][j].m_normal);
388 m_projectionsDict.insert(index, projections);
389 }
390 else
391 {
392 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
393 projections.push_back(m_deformableConstraints[i][j].m_normal);
394 }
395 }
396 }
397
398 const btSoftBody::Node* node = m_deformableConstraints[i][j].m_node;
399 int index = node->index;
400 if (m_deformableConstraints[i][j].m_static)
401 {
402 if (m_projectionsDict.find(index) == NULL)
403 {
404 m_projectionsDict.insert(index, units);
405 }
406 else
407 {
408 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
409 for (int k = 0; k < 3; ++k)
410 {
411 projections.push_back(units[k]);
412 }
413 }
414 }
415 else
416 {
417 if (m_projectionsDict.find(index) == NULL)
418 {
419 btAlignedObjectArray<btVector3> projections;
420 projections.push_back(m_deformableConstraints[i][j].m_normal);
421 m_projectionsDict.insert(index, projections);
422 }
423 else
424 {
425 btAlignedObjectArray<btVector3>& projections = *m_projectionsDict[index];
426 projections.push_back(m_deformableConstraints[i][j].m_normal);
427 }
428 }
429 }
430 }
431 }
432
433
applyDynamicFriction(TVStack & f)434 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
435 {
436 for (int i = 0; i < m_softBodies.size(); ++i)
437 {
438 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
439 {
440 const btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][j];
441 const btSoftBody::Node* node = constraint.m_node;
442 if (node->m_im != 0)
443 {
444 int index = node->index;
445 f[index] += constraint.getDv(node)* (1./node->m_im);
446 }
447 }
448 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
449 {
450 const btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][j];
451 const btSoftBody::Face* face = constraint.getContact()->m_face;
452 for (int k = 0; k < 3; ++k)
453 {
454 const btSoftBody::Node* node = face->m_n[k];
455 if (node->m_im != 0)
456 {
457 int index = node->index;
458 f[index] += constraint.getDv(node)* (1./node->m_im);
459 }
460 }
461 }
462 for (int j = 0; j < m_deformableConstraints[i].size(); ++j)
463 {
464 const btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[i][j];
465 const btSoftBody::Face* face = constraint.getContact()->m_face;
466 const btSoftBody::Node* node = constraint.getContact()->m_node;
467 if (node->m_im != 0)
468 {
469 int index = node->index;
470 f[index] += constraint.getDv(node)* (1./node->m_im);
471 }
472 for (int k = 0; k < 3; ++k)
473 {
474 const btSoftBody::Node* node = face->m_n[k];
475 if (node->m_im != 0)
476 {
477 int index = node->index;
478 f[index] += constraint.getDv(node)* (1./node->m_im);
479 }
480 }
481 }
482 }
483 }
484
reinitialize(bool nodeUpdated)485 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
486 {
487 int N = m_softBodies.size();
488 if (nodeUpdated)
489 {
490 m_staticConstraints.resize(N);
491 m_nodeAnchorConstraints.resize(N);
492 m_nodeRigidConstraints.resize(N);
493 m_faceRigidConstraints.resize(N);
494 m_deformableConstraints.resize(N);
495
496 }
497 for (int i = 0 ; i < N; ++i)
498 {
499 m_staticConstraints[i].clear();
500 m_nodeAnchorConstraints[i].clear();
501 m_nodeRigidConstraints[i].clear();
502 m_faceRigidConstraints[i].clear();
503 m_deformableConstraints[i].clear();
504 }
505 m_projectionsDict.clear();
506 }
507
508
509
510