Blender  V3.3
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), btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
20 {
21 }
22 
24  : m_anchor(other.m_anchor), btDeformableContactConstraint(other)
25 {
26 }
27 
29 {
30  const btSoftBody::sCti& cti = m_anchor->m_cti;
31  btVector3 va(0, 0, 0);
32  if (cti.m_colObj->hasContactResponse())
33  {
34  btRigidBody* rigidCol = 0;
35  btMultiBodyLinkCollider* multibodyLinkCol = 0;
36 
37  // grab the velocity of the rigid body
38  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
39  {
40  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
41  va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0);
42  }
43  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
44  {
46  if (multibodyLinkCol)
47  {
48  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
50  const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
51  const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
52  const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
53  const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
54  // add in the normal component of the va
55  btScalar vel = 0.0;
56  for (int k = 0; k < ndof; ++k)
57  {
58  vel += (local_v[k] + local_dv[k]) * J_n[k];
59  }
60  va = cti.m_normal * vel;
61  // add in the tangential components of the va
62  vel = 0.0;
63  for (int k = 0; k < ndof; ++k)
64  {
65  vel += (local_v[k] + local_dv[k]) * J_t1[k];
66  }
67  va += m_anchor->t1 * vel;
68  vel = 0.0;
69  for (int k = 0; k < ndof; ++k)
70  {
71  vel += (local_v[k] + local_dv[k]) * J_t2[k];
72  }
73  va += m_anchor->t2 * vel;
74  }
75  }
76  }
77  return va;
78 }
79 
81 {
82  const btSoftBody::sCti& cti = m_anchor->m_cti;
83  btVector3 va = getVa();
84  btVector3 vb = getVb();
85  btVector3 vr = (vb - va);
86  // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
87  const btScalar dn = btDot(vr, vr);
88  // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
89  btScalar residualSquare = dn * dn;
90  btVector3 impulse = m_anchor->m_c0 * vr;
91  // apply impulse to deformable nodes involved and change their velocities
92  applyImpulse(impulse);
93 
94  // apply impulse to the rigid/multibodies involved and change their velocities
95  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
96  {
97  btRigidBody* rigidCol = 0;
98  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
99  if (rigidCol)
100  {
101  rigidCol->applyImpulse(impulse, m_anchor->m_c1);
102  }
103  }
104  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
105  {
106  btMultiBodyLinkCollider* multibodyLinkCol = 0;
108  if (multibodyLinkCol)
109  {
111  // apply normal component of the impulse
112  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
113  // apply tangential component of the impulse
115  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
117  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
118  }
119  }
120  return residualSquare;
121 }
122 
124 {
125  return m_anchor->m_node->m_v;
126 }
127 
129 {
130  btVector3 dv = impulse * m_anchor->m_c2;
131  m_anchor->m_node->m_v -= dv;
132 }
133 
134 /* ================ Deformable vs. Rigid =================== */
136  : m_contact(&c), btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
137 {
138  m_total_normal_dv.setZero();
139  m_total_tangent_dv.setZero();
140  // The magnitude of penetration is the depth of penetration.
141  m_penetration = c.m_cti.m_offset;
143  m_binding = false;
144 }
145 
147  : m_contact(other.m_contact), btDeformableContactConstraint(other), m_penetration(other.m_penetration), m_total_split_impulse(other.m_total_split_impulse), m_binding(other.m_binding)
148 {
151 }
152 
154 {
155  const btSoftBody::sCti& cti = m_contact->m_cti;
156  btVector3 va(0, 0, 0);
157  if (cti.m_colObj->hasContactResponse())
158  {
159  btRigidBody* rigidCol = 0;
160  btMultiBodyLinkCollider* multibodyLinkCol = 0;
161 
162  // grab the velocity of the rigid body
163  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
164  {
165  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
166  va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
167  }
168  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
169  {
171  if (multibodyLinkCol)
172  {
173  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
175  const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
176  const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
177  const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
178  const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
179  // add in the normal component of the va
180  btScalar vel = 0.0;
181  for (int k = 0; k < ndof; ++k)
182  {
183  vel += (local_v[k] + local_dv[k]) * J_n[k];
184  }
185  va = cti.m_normal * vel;
186  // add in the tangential components of the va
187  vel = 0.0;
188  for (int k = 0; k < ndof; ++k)
189  {
190  vel += (local_v[k] + local_dv[k]) * J_t1[k];
191  }
192  va += m_contact->t1 * vel;
193  vel = 0.0;
194  for (int k = 0; k < ndof; ++k)
195  {
196  vel += (local_v[k] + local_dv[k]) * J_t2[k];
197  }
198  va += m_contact->t2 * vel;
199  }
200  }
201  }
202  return va;
203 }
204 
206 {
207  const btSoftBody::sCti& cti = m_contact->m_cti;
208  btVector3 va(0, 0, 0);
209  if (cti.m_colObj->hasContactResponse())
210  {
211  btRigidBody* rigidCol = 0;
212  btMultiBodyLinkCollider* multibodyLinkCol = 0;
213 
214  // grab the velocity of the rigid body
215  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
216  {
217  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
218  va = rigidCol ? (rigidCol->getPushVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
219  }
220  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
221  {
223  if (multibodyLinkCol)
224  {
225  const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
227  const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
228  const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
229  const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
230  // add in the normal component of the va
231  btScalar vel = 0.0;
232  for (int k = 0; k < ndof; ++k)
233  {
234  vel += local_split_v[k] * J_n[k];
235  }
236  va = cti.m_normal * vel;
237  // add in the tangential components of the va
238  vel = 0.0;
239  for (int k = 0; k < ndof; ++k)
240  {
241  vel += local_split_v[k] * J_t1[k];
242  }
243  va += m_contact->t1 * vel;
244  vel = 0.0;
245  for (int k = 0; k < ndof; ++k)
246  {
247  vel += local_split_v[k] * J_t2[k];
248  }
249  va += m_contact->t2 * vel;
250  }
251  }
252  }
253  return va;
254 }
255 
257 {
258  const btSoftBody::sCti& cti = m_contact->m_cti;
259  btVector3 va = getVa();
260  btVector3 vb = getVb();
261  btVector3 vr = vb - va;
263  if (m_penetration > 0)
264  {
266  }
268  {
270  }
271  // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
274  {
276  }
277  btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
278  btVector3 impulse_tangent = impulse - impulse_normal;
279  if (dn > 0)
280  {
281  return 0;
282  }
283  m_binding = true;
284  btScalar residualSquare = dn * dn;
285  btVector3 old_total_tangent_dv = m_total_tangent_dv;
286  // m_c5 is the inverse mass of the deformable node/face
287  m_total_normal_dv -= m_contact->m_c5 * impulse_normal;
288  m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent;
289 
290  if (m_total_normal_dv.dot(cti.m_normal) < 0)
291  {
292  // separating in the normal direction
293  m_binding = false;
294  m_static = false;
295  impulse_tangent.setZero();
296  }
297  else
298  {
299  if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
300  {
301  // dynamic friction
302  // 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.
303  m_static = false;
304  if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
305  {
306  m_total_tangent_dv = btVector3(0, 0, 0);
307  }
308  else
309  {
311  }
312  // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
313  impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv);
314  }
315  else
316  {
317  // static friction
318  m_static = true;
319  }
320  }
321  impulse = impulse_normal + impulse_tangent;
322  // apply impulse to deformable nodes involved and change their velocities
323  applyImpulse(impulse);
324  // apply impulse to the rigid/multibodies involved and change their velocities
325  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
326  {
327  btRigidBody* rigidCol = 0;
328  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
329  if (rigidCol)
330  {
331  rigidCol->applyImpulse(impulse, m_contact->m_c1);
332  }
333  }
334  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
335  {
336  btMultiBodyLinkCollider* multibodyLinkCol = 0;
338  if (multibodyLinkCol)
339  {
341  // apply normal component of the impulse
342  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
343  if (impulse_tangent.norm() > SIMD_EPSILON)
344  {
345  // apply tangential component of the impulse
347  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
349  multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
350  }
351  }
352  }
353  return residualSquare;
354 }
355 
357 {
358  btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction;
359  const btSoftBody::sCti& cti = m_contact->m_cti;
360  btVector3 vb = getSplitVb();
361  btVector3 va = getSplitVa();
363  if (p > 0)
364  {
365  return 0;
366  }
367  btVector3 vr = vb - va;
369  if (dn > 0)
370  {
371  return 0;
372  }
373  if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION)
374  {
375  dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse;
376  }
377  if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION)
378  {
379  dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse;
380  }
381  m_total_split_impulse += dn;
382 
383  btScalar residualSquare = dn * dn;
384  const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
385  applySplitImpulse(impulse);
386 
387  // apply split impulse to the rigid/multibodies involved and change their velocities
388  if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
389  {
390  btRigidBody* rigidCol = 0;
391  rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
392  if (rigidCol)
393  {
394  rigidCol->applyPushImpulse(impulse, m_contact->m_c1);
395  }
396  }
397  else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
398  {
399  btMultiBodyLinkCollider* multibodyLinkCol = 0;
401  if (multibodyLinkCol)
402  {
404  // apply normal component of the impulse
405  multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
406  }
407  }
408  return residualSquare;
409 }
410 /* ================ Node vs. Rigid =================== */
413 {
414 }
415 
418 {
419 }
420 
422 {
423  return m_node->m_v;
424 }
425 
427 {
428  return m_node->m_splitv;
429 }
430 
432 {
434 }
435 
437 {
439  btVector3 dv = contact->m_c5 * impulse;
440  contact->m_node->m_v -= dv;
441 }
442 
444 {
446  btVector3 dv = contact->m_c5 * impulse;
447  contact->m_node->m_splitv -= dv;
448 }
449 
450 /* ================ Face vs. Rigid =================== */
452  : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btDeformableRigidContactConstraint(contact, infoGlobal)
453 {
454 }
455 
457  : m_face(other.m_face), m_useStrainLimiting(other.m_useStrainLimiting), btDeformableRigidContactConstraint(other)
458 {
459 }
460 
462 {
464  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];
465  return vb;
466 }
467 
469 {
472  if (m_face->m_n[0] == node)
473  {
474  return face_dv * contact->m_weights[0];
475  }
476  if (m_face->m_n[1] == node)
477  {
478  return face_dv * contact->m_weights[1];
479  }
480  btAssert(node == m_face->m_n[2]);
481  return face_dv * contact->m_weights[2];
482 }
483 
485 {
487  btVector3 dv = impulse * contact->m_c2;
488  btSoftBody::Face* face = contact->m_face;
489 
490  btVector3& v0 = face->m_n[0]->m_v;
491  btVector3& v1 = face->m_n[1]->m_v;
492  btVector3& v2 = face->m_n[2]->m_v;
493  const btScalar& im0 = face->m_n[0]->m_im;
494  const btScalar& im1 = face->m_n[1]->m_im;
495  const btScalar& im2 = face->m_n[2]->m_im;
496  if (im0 > 0)
497  v0 -= dv * contact->m_weights[0];
498  if (im1 > 0)
499  v1 -= dv * contact->m_weights[1];
500  if (im2 > 0)
501  v2 -= dv * contact->m_weights[2];
503  {
504  btScalar relaxation = 1. / btScalar(m_infoGlobal->m_numIterations);
505  btScalar m01 = (relaxation / (im0 + im1));
506  btScalar m02 = (relaxation / (im0 + im2));
507  btScalar m12 = (relaxation / (im1 + im2));
508 #ifdef USE_STRAIN_RATE_LIMITING
509  // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
510  btScalar p = 0.01;
511  btVector3& x0 = face->m_n[0]->m_x;
512  btVector3& x1 = face->m_n[1]->m_x;
513  btVector3& x2 = face->m_n[2]->m_x;
514  const btVector3 x_diff[3] = {x1 - x0, x2 - x0, x2 - x1};
515  const btVector3 v_diff[3] = {v1 - v0, v2 - v0, v2 - v1};
516  btVector3 u[3];
517  btScalar x_diff_dot_u, dn[3];
519  for (int i = 0; i < 3; ++i)
520  {
521  btScalar x_diff_norm = x_diff[i].safeNorm();
522  btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
523  btScalar strainRate = x_diff_norm_new / x_diff_norm;
524  u[i] = v_diff[i];
525  u[i].safeNormalize();
526  if (x_diff_norm == 0 || (1 - p <= strainRate && strainRate <= 1 + p))
527  {
528  dn[i] = 0;
529  continue;
530  }
531  x_diff_dot_u = btDot(x_diff[i], u[i]);
532  btScalar s;
533  if (1 - p > strainRate)
534  {
535  s = 1 / dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p - 2 * p) * x_diff_norm * x_diff_norm));
536  }
537  else
538  {
539  s = 1 / dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u * x_diff_dot_u + (p * p + 2 * p) * x_diff_norm * x_diff_norm));
540  }
541  // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
542  // strainRate = x_diff_norm_new/x_diff_norm;
543  dn[i] = s - v_diff[i].safeNorm();
544  }
545  btVector3 dv0 = im0 * (m01 * u[0] * (-dn[0]) + m02 * u[1] * -(dn[1]));
546  btVector3 dv1 = im1 * (m01 * u[0] * (dn[0]) + m12 * u[2] * (-dn[2]));
547  btVector3 dv2 = im2 * (m12 * u[2] * (dn[2]) + m02 * u[1] * (dn[1]));
548 #else
549  // apply strain limiting to prevent undamped modes
550  btVector3 dv0 = im0 * (m01 * (v1 - v0) + m02 * (v2 - v0));
551  btVector3 dv1 = im1 * (m01 * (v0 - v1) + m12 * (v2 - v1));
552  btVector3 dv2 = im2 * (m12 * (v1 - v2) + m02 * (v0 - v2));
553 #endif
554  v0 += dv0;
555  v1 += dv1;
556  v2 += dv2;
557  }
558 }
559 
561 {
563  btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
564  return vb;
565 }
566 
568 {
570  btVector3 dv = impulse * contact->m_c2;
571  btSoftBody::Face* face = contact->m_face;
572  btVector3& v0 = face->m_n[0]->m_splitv;
573  btVector3& v1 = face->m_n[1]->m_splitv;
574  btVector3& v2 = face->m_n[2]->m_splitv;
575  const btScalar& im0 = face->m_n[0]->m_im;
576  const btScalar& im1 = face->m_n[1]->m_im;
577  const btScalar& im2 = face->m_n[2]->m_im;
578  if (im0 > 0)
579  {
580  v0 -= dv * contact->m_weights[0];
581  }
582  if (im1 > 0)
583  {
584  v1 -= dv * contact->m_weights[1];
585  }
586  if (im2 > 0)
587  {
588  v2 -= dv * contact->m_weights[2];
589  }
590 }
591 
592 /* ================ Face vs. Node =================== */
594  : m_node(contact.m_node), m_face(contact.m_face), m_contact(&contact), btDeformableContactConstraint(contact.m_normal, infoGlobal)
595 {
596  m_total_normal_dv.setZero();
597  m_total_tangent_dv.setZero();
598 }
599 
601 {
602  return m_node->m_v;
603 }
604 
606 {
608  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];
609  return vb;
610 }
611 
613 {
615  if (n == m_node)
616  return dv;
618  if (m_face->m_n[0] == n)
619  {
620  return dv * contact->m_weights[0];
621  }
622  if (m_face->m_n[1] == n)
623  {
624  return dv * contact->m_weights[1];
625  }
626  btAssert(n == m_face->m_n[2]);
627  return dv * contact->m_weights[2];
628 }
629 
631 {
632  btVector3 va = getVa();
633  btVector3 vb = getVb();
634  btVector3 vr = vb - va;
635  const btScalar dn = btDot(vr, m_contact->m_normal);
636  // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
637  btScalar residualSquare = dn * dn;
638  btVector3 impulse = m_contact->m_c0 * vr;
639  const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
640  btVector3 impulse_tangent = impulse - impulse_normal;
641 
642  btVector3 old_total_tangent_dv = m_total_tangent_dv;
643  // m_c2 is the inverse mass of the deformable node/face
644  if (m_node->m_im > 0)
645  {
646  m_total_normal_dv -= impulse_normal * m_node->m_im;
647  m_total_tangent_dv -= impulse_tangent * m_node->m_im;
648  }
649  else
650  {
651  m_total_normal_dv -= impulse_normal * m_contact->m_imf;
652  m_total_tangent_dv -= impulse_tangent * m_contact->m_imf;
653  }
654 
655  if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
656  {
657  // separating in the normal direction
658  m_static = false;
659  m_total_tangent_dv = btVector3(0, 0, 0);
660  impulse_tangent.setZero();
661  }
662  else
663  {
665  {
666  // dynamic friction
667  // 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.
668  m_static = false;
669  if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
670  {
671  m_total_tangent_dv = btVector3(0, 0, 0);
672  }
673  else
674  {
676  }
677  impulse_tangent = -btScalar(1) / m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
678  }
679  else
680  {
681  // static friction
682  m_static = true;
683  }
684  }
685  impulse = impulse_normal + impulse_tangent;
686  // apply impulse to deformable nodes involved and change their velocities
687  applyImpulse(impulse);
688  return residualSquare;
689 }
690 
692 {
694  btVector3 dva = impulse * contact->m_node->m_im;
695  btVector3 dvb = impulse * contact->m_imf;
696  if (contact->m_node->m_im > 0)
697  {
698  contact->m_node->m_v += dva;
699  }
700 
701  btSoftBody::Face* face = contact->m_face;
702  btVector3& v0 = face->m_n[0]->m_v;
703  btVector3& v1 = face->m_n[1]->m_v;
704  btVector3& v2 = face->m_n[2]->m_v;
705  const btScalar& im0 = face->m_n[0]->m_im;
706  const btScalar& im1 = face->m_n[1]->m_im;
707  const btScalar& im2 = face->m_n[2]->m_im;
708  if (im0 > 0)
709  {
710  v0 -= dvb * contact->m_weights[0];
711  }
712  if (im1 > 0)
713  {
714  v1 -= dvb * contact->m_weights[1];
715  }
716  if (im2 > 0)
717  {
718  v2 -= dvb * contact->m_weights[2];
719  }
720 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble x2
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble v1
ATTR_WARN_UNUSED_RESULT const BMVert * v2
@ CO_FEATHERSTONE_LINK
@ CO_RIGID_BODY
struct btDbvtNode * m_node
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
SIMD_FORCE_INLINE btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
SIMD_FORCE_INLINE btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
const btContactSolverInfo * m_infoGlobal
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
virtual btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
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 btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
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 solveConstraint(const btContactSolverInfo &infoGlobal)
virtual btVector3 getSplitVb() const =0
btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
virtual void applySplitImpulse(const btVector3 &impulse)=0
const btSoftBody::DeformableRigidContact * m_contact
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
static const btRigidBody * upcast(const btCollisionObject *colObj)
Definition: btRigidBody.h:189
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:335
btVector3 getPushVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:469
void applyPushImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:347
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:361
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:360
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:362
OperationNode * node
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_jacobians
Node * m_n[3]
Definition: btSoftBody.h:296
btVector3 m_x
Definition: btSoftBody.h:263
btVector3 m_splitv
Definition: btSoftBody.h:275
btVector3 m_v
Definition: btSoftBody.h:265
const btCollisionObject * m_colObj
Definition: btSoftBody.h:226
btVector3 m_normal
Definition: btSoftBody.h:227