Bullet Collision Detection & Physics Library
btDeformableContactProjection.cpp
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
18#include <algorithm>
19#include <cmath>
20btScalar 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 {
28 if (psb != deformableBodies[i])
29 {
30 continue;
31 }
32 for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
33 {
35 btScalar localResidualSquare = constraint.solveConstraint();
36 residualSquare = btMax(residualSquare, localResidualSquare);
37 }
38 for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
39 {
41 btScalar localResidualSquare = constraint.solveConstraint();
42 residualSquare = btMax(residualSquare, localResidualSquare);
43 }
44 for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
45 {
47 btScalar localResidualSquare = constraint.solveConstraint();
48 residualSquare = btMax(residualSquare, localResidualSquare);
49 }
50 for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
51 {
53 btScalar localResidualSquare = constraint.solveConstraint();
54 residualSquare = btMax(residualSquare, localResidualSquare);
55 }
56 }
57 }
58 return residualSquare;
59}
60
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 {
69 constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
70 }
71 // face constraints
72 for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
73 {
75 constraint.setPenetrationScale(infoGlobal.m_deformable_erp);
76 }
77 }
78}
79
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 {
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 {
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 {
103 btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal);
104 residualSquare = btMax(residualSquare, localResidualSquare);
105 }
106
107 }
108 return residualSquare;
109}
110
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 {
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 {
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 {
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 {
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
207{
208 const int dim = 3;
209 for (int index = 0; index < m_projectionsDict.size(); ++index)
210 {
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
245{
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 {
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 {
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 {
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 {
312 projections.push_back(m_nodeRigidConstraints[i][j].m_normal);
313 m_projectionsDict.insert(index, projections);
314 }
315 else
316 {
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 {
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 {
349 projections.push_back(m_faceRigidConstraints[i][j].m_normal);
350 m_projectionsDict.insert(index, projections);
351 }
352 else
353 {
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 {
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 {
387 projections.push_back(m_deformableConstraints[i][j].m_normal);
388 m_projectionsDict.insert(index, projections);
389 }
390 else
391 {
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 {
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 {
420 projections.push_back(m_deformableConstraints[i][j].m_normal);
421 m_projectionsDict.insert(index, projections);
422 }
423 else
424 {
426 projections.push_back(m_deformableConstraints[i][j].m_normal);
427 }
428 }
429 }
430 }
431}
432
433
435{
436 for (int i = 0; i < m_softBodies.size(); ++i)
437 {
438 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
439 {
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 {
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 {
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
486{
487 int N = m_softBodies.size();
488 if (nodeUpdated)
489 {
495
496 }
497 for (int i = 0 ; i < N; ++i)
498 {
504 }
506}
507
508
509
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:153
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void push_back(const T &_Val)
btCollisionObject can be used to manage collision detection objects.
btTransform & getWorldTransform()
virtual btScalar update(btCollisionObject **deformableBodies, int numDeformableBodies)
btAlignedObjectArray< btSoftBody * > & m_softBodies
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
virtual void reinitialize(bool nodeUpdated)
btAlignedObjectArray< btAlignedObjectArray< btDeformableFaceNodeContactConstraint > > m_deformableConstraints
btAlignedObjectArray< btAlignedObjectArray< btDeformableNodeRigidContactConstraint > > m_nodeRigidConstraints
btHashMap< btHashInt, btAlignedObjectArray< btVector3 > > m_projectionsDict
virtual void splitImpulseSetup(const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btAlignedObjectArray< btDeformableStaticConstraint > > m_staticConstraints
btAlignedObjectArray< btAlignedObjectArray< btDeformableFaceRigidContactConstraint > > m_faceRigidConstraints
btAlignedObjectArray< btAlignedObjectArray< btDeformableNodeAnchorConstraint > > m_nodeAnchorConstraints
virtual btVector3 getDv(const btSoftBody::Node *) const
const btSoftBody::DeformableFaceNodeContact * getContact() const
const btSoftBody::DeformableFaceRigidContact * getContact() const
virtual btVector3 getDv(const btSoftBody::Node *) const
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
virtual btVector3 getDv(const btSoftBody::Node *) const
virtual void setPenetrationScale(btScalar scale)
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
int getUid1() const
Definition: btHashMap.h:77
void insert(const Key &key, const Value &value)
Definition: btHashMap.h:264
Key getKeyAtIndex(int index)
Definition: btHashMap.h:400
void clear()
Definition: btHashMap.h:461
int size() const
Definition: btHashMap.h:373
const Value * getAtIndex(int index) const
Definition: btHashMap.h:378
const Value * find(const Key &key) const
Definition: btHashMap.h:424
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition: btSoftBody.h:73
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:789
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:790
btAlignedObjectArray< DeformableNodeRigidAnchor > m_deformableAnchors
Definition: btSoftBody.h:786
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:788
tNodeArray m_nodes
Definition: btSoftBody.h:777
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
Node * m_n[3]
Definition: btSoftBody.h:287
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btVector3 m_normal
Definition: btSoftBody.h:221