Bullet Collision Detection & Physics Library
btDeformableContactConstraint.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
17/* ================ Deformable Node Anchor =================== */
19: m_anchor(&a)
20, btDeformableContactConstraint(a.m_cti.m_normal)
21{
22}
23
25: m_anchor(other.m_anchor)
27{
28}
29
31{
32 const btSoftBody::sCti& cti = m_anchor->m_cti;
33 btVector3 va(0, 0, 0);
35 {
36 btRigidBody* rigidCol = 0;
37 btMultiBodyLinkCollider* multibodyLinkCol = 0;
38
39 // grab the velocity of the rigid body
41 {
43 va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
44 }
46 {
48 if (multibodyLinkCol)
49 {
50 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
54 const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
55 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
56 // add in the normal component of the va
57 btScalar vel = 0.0;
58 for (int k = 0; k < ndof; ++k)
59 {
60 vel += (local_v[k]+local_dv[k]) * J_n[k];
61 }
62 va = cti.m_normal * vel;
63 // add in the tangential components of the va
64 vel = 0.0;
65 for (int k = 0; k < ndof; ++k)
66 {
67 vel += (local_v[k]+local_dv[k]) * J_t1[k];
68 }
69 va += m_anchor->t1 * vel;
70 vel = 0.0;
71 for (int k = 0; k < ndof; ++k)
72 {
73 vel += (local_v[k]+local_dv[k]) * J_t2[k];
74 }
75 va += m_anchor->t2 * vel;
76 }
77 }
78 }
79 return va;
80}
81
83{
84 const btSoftBody::sCti& cti = m_anchor->m_cti;
85 btVector3 va = getVa();
86 btVector3 vb = getVb();
87 btVector3 vr = (vb - va);
88 // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
89 const btScalar dn = btDot(vr, cti.m_normal);
90 // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
91 btScalar residualSquare = dn*dn;
92 btVector3 impulse = m_anchor->m_c0 * vr;
93 // apply impulse to deformable nodes involved and change their velocities
94 applyImpulse(impulse);
95
96 // apply impulse to the rigid/multibodies involved and change their velocities
98 {
99 btRigidBody* rigidCol = 0;
100 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
101 if (rigidCol)
102 {
103 rigidCol->applyImpulse(impulse, m_anchor->m_c1);
104 }
105 }
107 {
108 btMultiBodyLinkCollider* multibodyLinkCol = 0;
110 if (multibodyLinkCol)
111 {
113 // apply normal component of the impulse
114 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
115 // apply tangential component of the impulse
117 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
119 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
120 }
121 }
122 return residualSquare;
123}
124
126{
127 return m_anchor->m_node->m_v;
128}
129
131{
132 btVector3 dv = impulse * m_anchor->m_c2;
133 m_anchor->m_node->m_v -= dv;
134}
135
136/* ================ Deformable vs. Rigid =================== */
138: m_contact(&c)
139, btDeformableContactConstraint(c.m_cti.m_normal)
140{
143 // penetration is non-positive. The magnitude of penetration is the depth of penetration.
145}
146
148: m_contact(other.m_contact)
150, m_penetration(other.m_penetration)
151{
154}
155
156
158{
159 const btSoftBody::sCti& cti = m_contact->m_cti;
160 btVector3 va(0, 0, 0);
161 if (cti.m_colObj->hasContactResponse())
162 {
163 btRigidBody* rigidCol = 0;
164 btMultiBodyLinkCollider* multibodyLinkCol = 0;
165
166 // grab the velocity of the rigid body
168 {
169 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
170 va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
171 }
173 {
175 if (multibodyLinkCol)
176 {
177 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
181 const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
182 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
183 // add in the normal component of the va
184 btScalar vel = 0.0;
185 for (int k = 0; k < ndof; ++k)
186 {
187 vel += (local_v[k]+local_dv[k]) * J_n[k];
188 }
189 va = cti.m_normal * vel;
190 // add in the tangential components of the va
191 vel = 0.0;
192 for (int k = 0; k < ndof; ++k)
193 {
194 vel += (local_v[k]+local_dv[k]) * J_t1[k];
195 }
196 va += m_contact->t1 * vel;
197 vel = 0.0;
198 for (int k = 0; k < ndof; ++k)
199 {
200 vel += (local_v[k]+local_dv[k]) * J_t2[k];
201 }
202 va += m_contact->t2 * vel;
203 }
204 }
205 }
206 return va;
207}
208
210{
211 const btSoftBody::sCti& cti = m_contact->m_cti;
212 btVector3 va = getVa();
213 btVector3 vb = getVb();
214 btVector3 vr = vb - va;
215 const btScalar dn = btDot(vr, cti.m_normal);
216 // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
217 btScalar residualSquare = dn*dn;
218 btVector3 impulse = m_contact->m_c0 * vr;
219 const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
220 btVector3 impulse_tangent = impulse - impulse_normal;
221 btVector3 old_total_tangent_dv = m_total_tangent_dv;
222 // m_c2 is the inverse mass of the deformable node/face
223 m_total_normal_dv -= impulse_normal * m_contact->m_c2;
224 m_total_tangent_dv -= impulse_tangent * m_contact->m_c2;
225
226 if (m_total_normal_dv.dot(cti.m_normal) < 0)
227 {
228 // separating in the normal direction
229 m_static = false;
231 impulse_tangent.setZero();
232 }
233 else
234 {
236 {
237 // dynamic friction
238 // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
239 m_static = false;
241 {
243 }
244 else
245 {
247 }
248 impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
249 }
250 else
251 {
252 // static friction
253 m_static = true;
254 }
255 }
256 impulse = impulse_normal + impulse_tangent;
257 // apply impulse to deformable nodes involved and change their velocities
258 applyImpulse(impulse);
259 // apply impulse to the rigid/multibodies involved and change their velocities
261 {
262 btRigidBody* rigidCol = 0;
263 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
264 if (rigidCol)
265 {
266 rigidCol->applyImpulse(impulse, m_contact->m_c1);
267 }
268 }
270 {
271 btMultiBodyLinkCollider* multibodyLinkCol = 0;
273 if (multibodyLinkCol)
274 {
276 // apply normal component of the impulse
277 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
278 if (impulse_tangent.norm() > SIMD_EPSILON)
279 {
280 // apply tangential component of the impulse
282 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
284 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
285 }
286 }
287 }
288 return residualSquare;
289}
290
292{
293 const btSoftBody::sCti& cti = m_contact->m_cti;
294 const btScalar dn = m_penetration;
295 if (dn != 0)
296 {
297 const btVector3 impulse = (m_contact->m_c0 * (cti.m_normal * dn / infoGlobal.m_timeStep));
298 // one iteration of the position impulse corrects all the position error at this timestep
299 m_penetration -= dn;
300 // apply impulse to deformable nodes involved and change their position
301 applySplitImpulse(impulse);
302 // apply impulse to the rigid/multibodies involved and change their position
304 {
305 btRigidBody* rigidCol = 0;
306 rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
307 if (rigidCol)
308 {
309 rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
310 }
311 }
313 {
314 // todo xuchenhan@
315 }
316 return (m_penetration/infoGlobal.m_timeStep) * (m_penetration/infoGlobal.m_timeStep);
317 }
318 return 0;
319}
320
321/* ================ Node vs. Rigid =================== */
323 : m_node(contact.m_node)
325 {
326 }
327
329: m_node(other.m_node)
331{
332}
333
335{
336 return m_node->m_v;
337}
338
339
341{
343}
344
346{
348 btVector3 dv = impulse * contact->m_c2;
349 contact->m_node->m_v -= dv;
350}
351
353{
355 btVector3 dv = impulse * contact->m_c2;
356 contact->m_node->m_vsplit -= dv;
357};
358
359/* ================ Face vs. Rigid =================== */
361: m_face(contact.m_face)
363{
364}
365
367: m_face(other.m_face)
369{
370}
371
373{
375 btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
376 return vb;
377}
378
379
381{
384 if (m_face->m_n[0] == node)
385 {
386 return face_dv * contact->m_weights[0];
387 }
388 if (m_face->m_n[1] == node)
389 {
390 return face_dv * contact->m_weights[1];
391 }
392 btAssert(node == m_face->m_n[2]);
393 return face_dv * contact->m_weights[2];
394}
395
397{
399 btVector3 dv = impulse * contact->m_c2;
400 btSoftBody::Face* face = contact->m_face;
401
402 btVector3& v0 = face->m_n[0]->m_v;
403 btVector3& v1 = face->m_n[1]->m_v;
404 btVector3& v2 = face->m_n[2]->m_v;
405 const btScalar& im0 = face->m_n[0]->m_im;
406 const btScalar& im1 = face->m_n[1]->m_im;
407 const btScalar& im2 = face->m_n[2]->m_im;
408 if (im0 > 0)
409 v0 -= dv * contact->m_weights[0];
410 if (im1 > 0)
411 v1 -= dv * contact->m_weights[1];
412 if (im2 > 0)
413 v2 -= dv * contact->m_weights[2];
414
415 // apply strain limiting to prevent undamped modes
416 btScalar m01 = (btScalar(1)/(im0 + im1));
417 btScalar m02 = (btScalar(1)/(im0 + im2));
418 btScalar m12 = (btScalar(1)/(im1 + im2));
419
420 btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
421 btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
422 btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
423
424 v0 += dv0;
425 v1 += dv1;
426 v2 += dv2;
427}
428
430{
432 btVector3 dv = impulse * contact->m_c2;
433 btSoftBody::Face* face = contact->m_face;
434
435 btVector3& v0 = face->m_n[0]->m_vsplit;
436 btVector3& v1 = face->m_n[1]->m_vsplit;
437 btVector3& v2 = face->m_n[2]->m_vsplit;
438 const btScalar& im0 = face->m_n[0]->m_im;
439 const btScalar& im1 = face->m_n[1]->m_im;
440 const btScalar& im2 = face->m_n[2]->m_im;
441 if (im0 > 0)
442 v0 -= dv * contact->m_weights[0];
443 if (im1 > 0)
444 v1 -= dv * contact->m_weights[1];
445 if (im2 > 0)
446 v2 -= dv * contact->m_weights[2];
447}
448
449/* ================ Face vs. Node =================== */
451: m_node(contact.m_node)
452, m_face(contact.m_face)
453, m_contact(&contact)
454, btDeformableContactConstraint(contact.m_normal)
455{
458}
459
461{
462 return m_node->m_v;
463}
464
466{
468 btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
469 return vb;
470}
471
473{
475 if (n == m_node)
476 return dv;
478 if (m_face->m_n[0] == n)
479 {
480 return dv * contact->m_weights[0];
481 }
482 if (m_face->m_n[1] == n)
483 {
484 return dv * contact->m_weights[1];
485 }
486 btAssert(n == m_face->m_n[2]);
487 return dv * contact->m_weights[2];
488}
489
491{
492 btVector3 va = getVa();
493 btVector3 vb = getVb();
494 btVector3 vr = vb - va;
495 const btScalar dn = btDot(vr, m_contact->m_normal);
496 // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
497 btScalar residualSquare = dn*dn;
498 btVector3 impulse = m_contact->m_c0 * vr;
499 const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
500 btVector3 impulse_tangent = impulse - impulse_normal;
501
502 btVector3 old_total_tangent_dv = m_total_tangent_dv;
503 // m_c2 is the inverse mass of the deformable node/face
504 if (m_node->m_im > 0)
505 {
506 m_total_normal_dv -= impulse_normal * m_node->m_im;
507 m_total_tangent_dv -= impulse_tangent * m_node->m_im;
508 }
509 else
510 {
511 m_total_normal_dv -= impulse_normal * m_contact->m_imf;
512 m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
513 }
514
516 {
517 // separating in the normal direction
518 m_static = false;
520 impulse_tangent.setZero();
521 }
522 else
523 {
525 {
526 // dynamic friction
527 // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
528 m_static = false;
530 {
532 }
533 else
534 {
536 }
537 impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
538 }
539 else
540 {
541 // static friction
542 m_static = true;
543 }
544 }
545 impulse = impulse_normal + impulse_tangent;
546 // apply impulse to deformable nodes involved and change their velocities
547 applyImpulse(impulse);
548 return residualSquare;
549}
550
552{
554 btVector3 dva = impulse * contact->m_node->m_im;
555 btVector3 dvb = impulse * contact->m_imf;
556 if (contact->m_node->m_im > 0)
557 {
558 contact->m_node->m_v += dva;
559 }
560
561 btSoftBody::Face* face = contact->m_face;
562 btVector3& v0 = face->m_n[0]->m_v;
563 btVector3& v1 = face->m_n[1]->m_v;
564 btVector3& v2 = face->m_n[2]->m_v;
565 const btScalar& im0 = face->m_n[0]->m_im;
566 const btScalar& im1 = face->m_n[1]->m_im;
567 const btScalar& im2 = face->m_n[2]->m_im;
568 if (im0 > 0)
569 {
570 v0 -= dvb * contact->m_weights[0];
571 }
572 if (im1 > 0)
573 {
574 v1 -= dvb * contact->m_weights[1];
575 }
576 if (im2 > 0)
577 {
578 v2 -= dvb * contact->m_weights[2];
579 }
580 // todo: Face node constraints needs more work
581// btScalar m01 = (btScalar(1)/(im0 + im1));
582// btScalar m02 = (btScalar(1)/(im0 + im2));
583// btScalar m12 = (btScalar(1)/(im1 + im2));
584//
585// btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
586// btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
587// btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
588// v0 += dv0;
589// v1 += dv1;
590// v2 += dv2;
591}
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
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
int getInternalType() const
reserved for Bullet internal usage
bool hasContactResponse() const
virtual void applySplitImpulse(const btVector3 &impulse)=0
virtual btVector3 getVb() const =0
virtual void applyImpulse(const btVector3 &impulse)=0
virtual void applyImpulse(const btVector3 &impulse)
const btSoftBody::DeformableFaceNodeContact * m_contact
virtual btVector3 getDv(const btSoftBody::Node *) const
const btSoftBody::DeformableFaceNodeContact * getContact() const
virtual void applySplitImpulse(const btVector3 &impulse)
const btSoftBody::DeformableFaceRigidContact * getContact() const
virtual btVector3 getDv(const btSoftBody::Node *) const
virtual void applyImpulse(const btVector3 &impulse)
virtual void applyImpulse(const btVector3 &impulse)
const btSoftBody::DeformableNodeRigidAnchor * m_anchor
virtual void applySplitImpulse(const btVector3 &impulse)
virtual void applyImpulse(const btVector3 &impulse)
virtual btVector3 getDv(const btSoftBody::Node *) const
const btSoftBody::DeformableNodeRigidContact * getContact() const
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
const btSoftBody::DeformableRigidContact * m_contact
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
const btScalar * getDeltaVelocityVector() const
Definition: btMultiBody.h:277
int getNumDofs() const
Definition: btMultiBody.h:167
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:326
void applyPushImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:338
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don't store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:348
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:347
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:349
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
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
void setZero()
Definition: btVector3.h:671
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
Node * m_n[3]
Definition: btSoftBody.h:287
btVector3 m_vsplit
Definition: btSoftBody.h:260
btVector3 m_v
Definition: btSoftBody.h:259
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btScalar m_offset
Definition: btSoftBody.h:222
btVector3 m_normal
Definition: btSoftBody.h:221