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);
34  if (cti.m_colObj->hasContactResponse())
35  {
36  btRigidBody* rigidCol = 0;
37  btMultiBodyLinkCollider* multibodyLinkCol = 0;
38 
39  // grab the velocity of the rigid body
41  {
42  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
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;
52  const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
53  const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
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;
179  const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
180  const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
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;
230  m_total_tangent_dv = btVector3(0,0,0);
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  {
242  m_total_tangent_dv = btVector3(0,0,0);
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;
519  m_total_tangent_dv = btVector3(0,0,0);
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  {
531  m_total_tangent_dv = btVector3(0,0,0);
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
const btSoftBody::DeformableFaceNodeContact * getContact() const
virtual void applyImpulse(const btVector3 &impulse)
const btSoftBody::DeformableFaceNodeContact * m_contact
virtual btVector3 getDv(const btSoftBody::Node *) const
const btSoftBody::DeformableFaceRigidContact * getContact() const
virtual void applySplitImpulse(const btVector3 &impulse)
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
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
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
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
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
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