Bullet Collision Detection & Physics Library: btDeformableContactConstraint.cpp Source File

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
16 #include "btDeformableContactConstraint.h"
17 /* ================ Deformable Node Anchor =================== */
18 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
19 : m_anchor(&a)
20 , btDeformableContactConstraint(a.m_cti.m_normal)
21 {
22 }
23
24 btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
25 : m_anchor(other.m_anchor)
26 , btDeformableContactConstraint(other)
27 {
28 }
29
30 btVector3 btDeformableNodeAnchorConstraint::getVa() const
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
40 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_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 }
45 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
46 {
47 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
48 if (multibodyLinkCol)
49 {
50 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
51 const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
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
82 btScalar btDeformableNodeAnchorConstraint::solveConstraint()
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
97 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
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 }
106 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
107 {
108 btMultiBodyLinkCollider* multibodyLinkCol = 0;
109 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
110 if (multibodyLinkCol)
111 {
112 const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
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
116 const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
117 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
118 const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
119 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
120 }
121 }
122 return residualSquare;
123 }
124
125 btVector3 btDeformableNodeAnchorConstraint::getVb() const
126 {
127 return m_anchor->m_node->m_v;
128 }
129
130 void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
131 {
132 btVector3 dv = impulse * m_anchor->m_c2;
133 m_anchor->m_node->m_v -= dv;
134 }
135
136 /* ================ Deformable vs. Rigid =================== */
137 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
138 : m_contact(&c)
139 , btDeformableContactConstraint(c.m_cti.m_normal)
140 {
141 m_total_normal_dv.setZero();
142 m_total_tangent_dv.setZero();
143 // penetration is non-positive. The magnitude of penetration is the depth of penetration.
144 m_penetration = btMin(btScalar(0), c.m_cti.m_offset);
145 }
146
147 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
148 : m_contact(other.m_contact)
149 , btDeformableContactConstraint(other)
150 , m_penetration(other.m_penetration)
151 {
152 m_total_normal_dv = other.m_total_normal_dv;
153 m_total_tangent_dv = other.m_total_tangent_dv;
154 }
155
156
157 btVector3 btDeformableRigidContactConstraint::getVa() const
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
167 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_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 }
172 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
173 {
174 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
175 if (multibodyLinkCol)
176 {
177 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
178 const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
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
209 btScalar btDeformableRigidContactConstraint::solveConstraint()
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 {
235 if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
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;
240 if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
241 {
242 m_total_tangent_dv = btVector3(0,0,0);
243 }
244 else
245 {
246 m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3;
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
260 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
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 }
269 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
270 {
271 btMultiBodyLinkCollider* multibodyLinkCol = 0;
272 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
273 if (multibodyLinkCol)
274 {
275 const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
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
281 const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
282 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
283 const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
284 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
285 }
286 }
287 }
288 return residualSquare;
289 }
290
291 btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal)
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
303 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
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 }
312 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
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 =================== */
322 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
323 : m_node(contact.m_node)
324 , btDeformableRigidContactConstraint(contact)
325 {
326 }
327
328 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
329 : m_node(other.m_node)
330 , btDeformableRigidContactConstraint(other)
331 {
332 }
333
334 btVector3 btDeformableNodeRigidContactConstraint::getVb() const
335 {
336 return m_node->m_v;
337 }
338
339
340 btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
341 {
342 return m_total_normal_dv + m_total_tangent_dv;
343 }
344
345 void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
346 {
347 const btSoftBody::DeformableNodeRigidContact* contact = getContact();
348 btVector3 dv = impulse * contact->m_c2;
349 contact->m_node->m_v -= dv;
350 }
351
352 void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
353 {
354 const btSoftBody::DeformableNodeRigidContact* contact = getContact();
355 btVector3 dv = impulse * contact->m_c2;
356 contact->m_node->m_vsplit -= dv;
357 };
358
359 /* ================ Face vs. Rigid =================== */
360 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
361 : m_face(contact.m_face)
362 , btDeformableRigidContactConstraint(contact)
363 {
364 }
365
366 btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
367 : m_face(other.m_face)
368 , btDeformableRigidContactConstraint(other)
369 {
370 }
371
372 btVector3 btDeformableFaceRigidContactConstraint::getVb() const
373 {
374 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
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
380 btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
381 {
382 btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
383 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
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
396 void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
397 {
398 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
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
429 void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3& impulse)
430 {
431 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
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 =================== */
450 btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
451 : m_node(contact.m_node)
452 , m_face(contact.m_face)
453 , m_contact(&contact)
454 , btDeformableContactConstraint(contact.m_normal)
455 {
456 m_total_normal_dv.setZero();
457 m_total_tangent_dv.setZero();
458 }
459
460 btVector3 btDeformableFaceNodeContactConstraint::getVa() const
461 {
462 return m_node->m_v;
463 }
464
465 btVector3 btDeformableFaceNodeContactConstraint::getVb() const
466 {
467 const btSoftBody::DeformableFaceNodeContact* contact = getContact();
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
472 btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
473 {
474 btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
475 if (n == m_node)
476 return dv;
477 const btSoftBody::DeformableFaceNodeContact* contact = getContact();
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
490 btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
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
515 if (m_total_normal_dv.dot(m_contact->m_normal) > 0)
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 {
524 if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
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;
529 if (m_total_tangent_dv.safeNorm() < SIMD_EPSILON)
530 {
531 m_total_tangent_dv = btVector3(0,0,0);
532 }
533 else
534 {
535 m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_friction;
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
551 void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
552 {
553 const btSoftBody::DeformableFaceNodeContact* contact = getContact();
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 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btSoftBody::DeformableRigidContact::jacobianData_t2
btMultiBodyJacobianData jacobianData_t2
Definition: btSoftBody.h:349
btDeformableFaceNodeContactConstraint::getVa
virtual btVector3 getVa() const
Definition: btDeformableContactConstraint.cpp:460
btDeformableFaceNodeContactConstraint::m_node
btSoftBody::Node * m_node
Definition: btDeformableContactConstraint.h:260
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btSoftBody::DeformableFaceNodeContact::m_node
Node * m_node
Definition: btSoftBody.h:377
btDeformableFaceRigidContactConstraint
Definition: btDeformableContactConstraint.h:228
btDeformableRigidContactConstraint::getVa
virtual btVector3 getVa() const
Definition: btDeformableContactConstraint.cpp:157
btDeformableContactConstraint
Definition: btDeformableContactConstraint.h:22
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btDeformableNodeRigidContactConstraint::m_node
const btSoftBody::Node * m_node
Definition: btDeformableContactConstraint.h:199
btDeformableNodeRigidContactConstraint::getVb
virtual btVector3 getVb() const
Definition: btDeformableContactConstraint.cpp:334
btContactSolverInfo
Definition: btContactSolverInfo.h:73
btDeformableFaceRigidContactConstraint::getContact
const btSoftBody::DeformableFaceRigidContact * getContact() const
Definition: btDeformableContactConstraint.h:246
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSoftBody::DeformableFaceNodeContact::m_c0
btScalar m_c0
Definition: btSoftBody.h:385
btDeformableFaceRigidContactConstraint::m_face
const btSoftBody::Face * m_face
Definition: btDeformableContactConstraint.h:230
btDeformableRigidContactConstraint::btDeformableRigidContactConstraint
btDeformableRigidContactConstraint()
Definition: btDeformableContactConstraint.h:173
btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint
btDeformableFaceRigidContactConstraint()
Definition: btDeformableContactConstraint.h:231
btSoftBody::DeformableNodeRigidContact
Definition: btSoftBody.h:355
btDeformableNodeAnchorConstraint
Definition: btDeformableContactConstraint.h:130
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btDeformableNodeRigidContactConstraint
Definition: btDeformableContactConstraint.h:196
btDeformableRigidContactConstraint::m_penetration
btScalar m_penetration
Definition: btDeformableContactConstraint.h:170
btSoftBody::DeformableRigidContact::m_c3
btScalar m_c3
Definition: btSoftBody.h:343
btVector3::safeNorm
btScalar safeNorm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btDeformableFaceNodeContactConstraint::getContact
const btSoftBody::DeformableFaceNodeContact * getContact() const
Definition: btDeformableContactConstraint.h:290
btVector3::setZero
void setZero()
Definition: btVector3.h:671
btSoftBody::Node::m_vsplit
btVector3 m_vsplit
Definition: btSoftBody.h:260
btMultiBody::getDeltaVelocityVector
const btScalar * getDeltaVelocityVector() const
Definition: btMultiBody.h:277
btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint
btDeformableNodeAnchorConstraint()
Definition: btDeformableContactConstraint.h:134
btDeformableNodeRigidContactConstraint::getDv
virtual btVector3 getDv(const btSoftBody::Node *) const
Definition: btDeformableContactConstraint.cpp:340
btDeformableFaceNodeContactConstraint::getDv
virtual btVector3 getDv(const btSoftBody::Node *) const
Definition: btDeformableContactConstraint.cpp:472
btMin
const T & btMin(const T &a, const T &b)
Definition: btMinMax.h:21
btSoftBody::Node
Definition: btSoftBody.h:256
btDeformableNodeAnchorConstraint::getVa
virtual btVector3 getVa() const
Definition: btDeformableContactConstraint.cpp:30
btSoftBody::sCti
Definition: btSoftBody.h:219
btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint
btDeformableNodeRigidContactConstraint()
Definition: btDeformableContactConstraint.h:201
btSoftBody::DeformableFaceRigidContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:371
btDeformableContactConstraint::getVb
virtual btVector3 getVb() const =0
btSoftBody::DeformableFaceNodeContact::m_normal
btVector3 m_normal
Definition: btSoftBody.h:381
btSoftBody::DeformableNodeRigidAnchor
Definition: btSoftBody.h:361
btSoftBody::DeformableFaceRigidContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:372
btCollisionObject::CO_RIGID_BODY
@ CO_RIGID_BODY
Definition: btCollisionObject.h:146
btSoftBody::DeformableFaceNodeContact::m_imf
btScalar m_imf
Definition: btSoftBody.h:384
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSoftBody::sCti::m_colObj
const btCollisionObject * m_colObj
Definition: btSoftBody.h:220
btDeformableNodeAnchorConstraint::solveConstraint
virtual btScalar solveConstraint()
Definition: btDeformableContactConstraint.cpp:82
btSoftBody::DeformableRigidContact::jacobianData_normal
btMultiBodyJacobianData jacobianData_normal
Definition: btSoftBody.h:347
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btSoftBody::Face
Definition: btSoftBody.h:286
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btDeformableFaceNodeContactConstraint::solveConstraint
virtual btScalar solveConstraint()
Definition: btDeformableContactConstraint.cpp:490
btDeformableRigidContactConstraint::solveSplitImpulse
virtual btScalar solveSplitImpulse(const btContactSolverInfo &infoGlobal)
Definition: btDeformableContactConstraint.cpp:291
btDeformableContactConstraint.h
btSoftBody::DeformableNodeRigidContact::m_node
Node * m_node
Definition: btSoftBody.h:357
btDeformableFaceNodeContactConstraint::getVb
virtual btVector3 getVb() const
Definition: btDeformableContactConstraint.cpp:465
btDeformableNodeAnchorConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:130
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btDeformableNodeRigidContactConstraint::getContact
const btSoftBody::DeformableNodeRigidContact * getContact() const
Definition: btDeformableContactConstraint.h:216
btDeformableContactConstraint::m_static
bool m_static
Definition: btDeformableContactConstraint.h:26
btSoftBody::sCti::m_normal
btVector3 m_normal
Definition: btSoftBody.h:221
btDeformableFaceRigidContactConstraint::getDv
virtual btVector3 getDv(const btSoftBody::Node *) const
Definition: btDeformableContactConstraint.cpp:380
btSoftBody::sCti::m_offset
btScalar m_offset
Definition: btSoftBody.h:222
btSoftBody::DeformableFaceNodeContact::m_bary
btVector3 m_bary
Definition: btSoftBody.h:379
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btCollisionObject::hasContactResponse
bool hasContactResponse() const
Definition: btCollisionObject.h:210
btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint
btDeformableFaceNodeContactConstraint()
Definition: btDeformableContactConstraint.h:266
btSoftBody::DeformableRigidContact::m_c0
btMatrix3x3 m_c0
Definition: btSoftBody.h:340
btSoftBody::DeformableFaceRigidContact
Definition: btSoftBody.h:367
btDeformableRigidContactConstraint
Definition: btDeformableContactConstraint.h:166
btCollisionObject::CO_FEATHERSTONE_LINK
@ CO_FEATHERSTONE_LINK
Definition: btCollisionObject.h:153
btRigidBody::applyPushImpulse
void applyPushImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:338
btSoftBody::DeformableRigidContact::m_c1
btVector3 m_c1
Definition: btSoftBody.h:341
btDeformableNodeAnchorConstraint::m_anchor
const btSoftBody::DeformableNodeRigidAnchor * m_anchor
Definition: btDeformableContactConstraint.h:132
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btDeformableFaceNodeContactConstraint::m_total_normal_dv
btVector3 m_total_normal_dv
Definition: btDeformableContactConstraint.h:263
btSoftBody::DeformableFaceNodeContact::m_friction
btScalar m_friction
Definition: btSoftBody.h:383
btDeformableRigidContactConstraint::m_contact
const btSoftBody::DeformableRigidContact * m_contact
Definition: btDeformableContactConstraint.h:171
btDeformableNodeRigidContactConstraint::applySplitImpulse
virtual void applySplitImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:352
btDeformableContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)=0
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:362
btDeformableFaceRigidContactConstraint::getVb
virtual btVector3 getVb() const
Definition: btDeformableContactConstraint.cpp:372
btSoftBody::DeformableFaceNodeContact::m_weights
btVector3 m_weights
Definition: btSoftBody.h:380
btSoftBody::DeformableFaceNodeContact
Definition: btSoftBody.h:376
btSoftBody::DeformableRigidContact::m_cti
sCti m_cti
Definition: btSoftBody.h:339
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btDeformableFaceNodeContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:551
btDeformableNodeAnchorConstraint::getVb
virtual btVector3 getVb() const
Definition: btDeformableContactConstraint.cpp:125
btDeformableRigidContactConstraint::m_total_normal_dv
btVector3 m_total_normal_dv
Definition: btDeformableContactConstraint.h:168
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:36
btSoftBody::DeformableFaceNodeContact::m_face
Face * m_face
Definition: btSoftBody.h:378
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btDeformableFaceNodeContactConstraint::m_face
btSoftBody::Face * m_face
Definition: btDeformableContactConstraint.h:261
btDeformableFaceRigidContactConstraint::applySplitImpulse
virtual void applySplitImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:429
btSoftBody::DeformableRigidContact::jacobianData_t1
btMultiBodyJacobianData jacobianData_t1
Definition: btSoftBody.h:348
btSoftBody::Face::m_n
Node * m_n[3]
Definition: btSoftBody.h:287
btDeformableContactConstraint::applySplitImpulse
virtual void applySplitImpulse(const btVector3 &impulse)=0
btDeformableFaceRigidContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:396
btSoftBody::DeformableRigidContact
Definition: btSoftBody.h:337
btSoftBody::DeformableRigidContact::t2
btVector3 t2
Definition: btSoftBody.h:351
btRigidBody::applyImpulse
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:326
btDeformableFaceNodeContactConstraint::m_contact
const btSoftBody::DeformableFaceNodeContact * m_contact
Definition: btDeformableContactConstraint.h:262
btDeformableNodeRigidContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition: btDeformableContactConstraint.cpp:345
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:264
btSoftBody::Node::m_v
btVector3 m_v
Definition: btSoftBody.h:259
btDeformableFaceNodeContactConstraint::m_total_tangent_dv
btVector3 m_total_tangent_dv
Definition: btDeformableContactConstraint.h:264
btDeformableRigidContactConstraint::m_total_tangent_dv
btVector3 m_total_tangent_dv
Definition: btDeformableContactConstraint.h:169
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btSoftBody::DeformableRigidContact::t1
btVector3 t1
Definition: btSoftBody.h:350
btRigidBody::upcast
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
btDeformableRigidContactConstraint::solveConstraint
virtual btScalar solveConstraint()
Definition: btDeformableContactConstraint.cpp:209
btSoftBody::DeformableRigidContact::m_c2
btScalar m_c2
Definition: btSoftBody.h:342
btSoftBody::DeformableFaceRigidContact::m_face
Face * m_face
Definition: btSoftBody.h:369