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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btReducedDeformableContactConstraint.cpp
Go to the documentation of this file.
1#include "btReducedDeformableContactConstraint.h"
2#include <iostream>
3
4// ================= static constraints ===================
5 btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
6 btReducedDeformableBody* rsb,
7 btSoftBody::Node* node,
8 const btVector3& ri,
9 const btVector3& x0,
10 const btVector3& dir,
11 const btContactSolverInfo& infoGlobal,
12 btScalar dt)
13 : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
14{
15 m_erp = 0.2;
16 m_appliedImpulse = 0;
17
18 // get impulse factor
19 m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
20 m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
21
22 btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
23 btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
24
25 m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
26}
27
28 btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
29{
30 // target velocity of fixed constraint is 0
31 btVector3 deltaVa = getDeltaVa();
32 btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
33 btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
34 m_appliedImpulse = m_appliedImpulse + deltaImpulse;
35
36 btVector3 impulse = deltaImpulse * m_impulseDirection;
37 applyImpulse(impulse);
38
39 // calculate residual
40 btScalar residualSquare = m_impulseFactor * deltaImpulse;
41 residualSquare *= residualSquare;
42
43 return residualSquare;
44}
45
46// this calls reduced deformable body's internalApplyFullSpaceImpulse
47 void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
48{
49 // apply full space impulse
50 m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
51}
52
53 btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
54{
55 return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
56}
57
58// ================= base contact constraints ===================
59 btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
60 btReducedDeformableBody* rsb,
61 const btSoftBody::DeformableRigidContact& c,
62 const btContactSolverInfo& infoGlobal,
63 btScalar dt)
64 : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
65{
66 m_nodeQueryIndex = 0;
67 m_appliedNormalImpulse = 0;
68 m_appliedTangentImpulse = 0;
69 m_rhs = 0;
70 m_rhs_tangent = 0;
71 m_cfm = infoGlobal.m_deformable_cfm;
72 m_cfm_friction = 0;
73 m_erp = infoGlobal.m_deformable_erp;
74 m_erp_friction = infoGlobal.m_deformable_erp;
75 m_friction = infoGlobal.m_friction;
76
77 m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
78 m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
79}
80
81 void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
82{
83 if (!m_collideMultibody)
84 {
85 m_solverBodyId = bodyId;
86 m_solverBody = &solver_body;
87 m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
88 btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA);
89 m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
90
91 m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
92 btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
93 m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
94 }
95}
96
97 btVector3 btReducedDeformableRigidContactConstraint::getVa() const
98{
99 btVector3 Va(0, 0, 0);
100 if (!m_collideStatic)
101 {
102 Va = btDeformableRigidContactConstraint::getVa();
103 }
104 return Va;
105}
106
107 btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
108{
109 // btVector3 Va = getVa();
110 // btVector3 deltaVa = Va - m_bufferVelocityA;
111 // if (!m_collideStatic)
112 // {
113 // std::cout << "moving collision!!!\n";
114 // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
115 // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
116 // << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
117 // << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
118 // }
119 btVector3 deltaVa = getDeltaVa();
120 btVector3 deltaVb = getDeltaVb();
121
122 // if (!m_collideStatic)
123 // {
124 // std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
125 // std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
126 // }
127
128 // get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
129 btVector3 deltaV_rel = deltaVa - deltaVb;
130 btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
131
132 // if (!m_collideStatic)
133 // {
134 // std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
135 // std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
136 // std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
137 // }
138
139 // get the normal impulse to be applied
140 btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
141 // if (!m_collideStatic)
142 // {
143 // std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n";
144 // std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
145 // }
146
147 {
148 // cumulative impulse that has been applied
149 btScalar sum = m_appliedNormalImpulse + deltaImpulse;
150 // if the cumulative impulse is pushing the object into the rigid body, set it zero
151 if (sum < 0)
152 {
153 deltaImpulse = -m_appliedNormalImpulse;
154 m_appliedNormalImpulse = 0;
155 }
156 else
157 {
158 m_appliedNormalImpulse = sum;
159 }
160 }
161
162 // if (!m_collideStatic)
163 // {
164 // std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
165 // std::cout << "deltaImpulse: " << deltaImpulse << '\n';
166 // }
167
168 // residual is the nodal normal velocity change in current iteration
169 btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual
170 residualSquare *= residualSquare;
171
172
173 // apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
174 btScalar deltaImpulse_tangent = 0;
175 btScalar deltaImpulse_tangent2 = 0;
176 {
177 // calculate how much impulse is needed
178 // btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
179 // btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
180 // deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
181
182 // btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
183 btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
184 btScalar upper_limit = m_appliedNormalImpulse * m_friction;
185 // if (sum > upper_limit)
186 // {
187 // deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
188 // m_appliedTangentImpulse = upper_limit;
189 // }
190 // else if (sum < lower_limit)
191 // {
192 // deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
193 // m_appliedTangentImpulse = lower_limit;
194 // }
195 // else
196 // {
197 // m_appliedTangentImpulse = sum;
198 // }
199 //
200 calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
201 m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
202
203 if (m_collideMultibody)
204 {
205 calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
206 m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
207 }
208
209
210 if (!m_collideStatic)
211 {
212 // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n";
213 // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n';
214 // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
215 // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
216 }
217 }
218
219 // get the total impulse vector
220 btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
221 btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
222 btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
223 btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
224
225 applyImpulse(impulse);
226
227 // apply impulse to the rigid/multibodies involved and change their velocities
228 if (!m_collideStatic)
229 {
230 // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
231 // << m_linearComponentNormal[1] << '\t'
232 // << m_linearComponentNormal[2] << '\n';
233 // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
234 // << m_angularComponentNormal[1] << '\t'
235 // << m_angularComponentNormal[2] << '\n';
236
237 if (!m_collideMultibody) // collision with rigid body
238 {
239 // std::cout << "rigid impulse applied!!\n";
240 // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
241 // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
242 // << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
243 // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
244 // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
245 // << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
246
247 m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
248 m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
249
250 // std::cout << "after\n";
251 // std::cout << "rigid impulse applied!!\n";
252 // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
253 // << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
254 // << m_solverBody->getDeltaLinearVelocity()[2] << '\n';
255 // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
256 // << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
257 // << m_solverBody->getDeltaAngularVelocity()[2] << '\n';
258 }
259 else // collision with multibody
260 {
261 btMultiBodyLinkCollider* multibodyLinkCol = 0;
262 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
263 if (multibodyLinkCol)
264 {
265 const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
266 // apply normal component of the impulse
267 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
268
269 // const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
270 // std::cout << "deltaV_normal: ";
271 // for (int i = 0; i < ndof; ++i)
272 // {
273 // std::cout << i << "\t" << deltaV_normal[i] << '\n';
274 // }
275
276 if (impulse_tangent.norm() > SIMD_EPSILON)
277 {
278 // apply tangential component of the impulse
279 const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
280 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent);
281 const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
282 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
283 }
284 }
285 }
286 }
287 return residualSquare;
288}
289
290 void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent,
291 btScalar& appliedImpulse,
292 const btScalar rhs_tangent,
293 const btScalar tangentImpulseFactorInv,
294 const btVector3& tangent,
295 const btScalar lower_limit,
296 const btScalar upper_limit,
297 const btVector3& deltaV_rel)
298{
299 btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
300 btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
301 deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
302
303 btScalar sum = appliedImpulse + deltaImpulse_tangent;
304 if (sum > upper_limit)
305 {
306 deltaImpulse_tangent = upper_limit - appliedImpulse;
307 appliedImpulse = upper_limit;
308 }
309 else if (sum < lower_limit)
310 {
311 deltaImpulse_tangent = lower_limit - appliedImpulse;
312 appliedImpulse = lower_limit;
313 }
314 else
315 {
316 appliedImpulse = sum;
317 }
318}
319
320// ================= node vs rigid constraints ===================
321 btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
322 btReducedDeformableBody* rsb,
323 const btSoftBody::DeformableNodeRigidContact& contact,
324 const btContactSolverInfo& infoGlobal,
325 btScalar dt)
326 : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
327{
328 m_contactNormalA = contact.m_cti.m_normal;
329 m_contactNormalB = -contact.m_cti.m_normal;
330
331 if (contact.m_node->index < rsb->m_nodes.size())
332 {
333 m_nodeQueryIndex = contact.m_node->index;
334 }
335 else
336 {
337 m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
338 }
339
340 if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
341 {
342 m_relPosA = contact.m_c1;
343 }
344 else
345 {
346 m_relPosA = btVector3(0,0,0);
347 }
348 m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
349
350 if (m_collideStatic) // colliding with static object, only consider reduced deformable body's impulse factor
351 {
352 m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
353 }
354 else // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
355 {
356 m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
357 }
358
359 m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
360 m_tangentImpulseFactor = 0;
361
362 warmStarting();
363}
364
365 void btReducedDeformableNodeRigidContactConstraint::warmStarting()
366{
367 btVector3 va = getVa();
368 btVector3 vb = getVb();
369 m_bufferVelocityA = va;
370 m_bufferVelocityB = vb;
371
372 // we define the (+) direction of errors to be the outward surface normal of the rigid object
373 btVector3 v_rel = vb - va;
374 // get tangent direction of the relative velocity
375 btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
376 if (v_tangent.norm() < SIMD_EPSILON)
377 {
378 m_contactTangent = btVector3(0, 0, 0);
379 // tangent impulse factor
380 m_tangentImpulseFactor = 0;
381 m_tangentImpulseFactorInv = 0;
382 }
383 else
384 {
385 if (!m_collideMultibody)
386 {
387 m_contactTangent = v_tangent.normalized();
388 m_contactTangent2.setZero();
389 // tangent impulse factor 1
390 m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
391 m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
392 // tangent impulse factor 2
393 m_tangentImpulseFactor2 = 0;
394 m_tangentImpulseFactorInv2 = 0;
395 }
396 else // multibody requires 2 contact directions
397 {
398 m_contactTangent = m_contact->t1;
399 m_contactTangent2 = m_contact->t2;
400
401 // tangent impulse factor 1
402 m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
403 m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
404 // tangent impulse factor 2
405 m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
406 m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
407 }
408 }
409
410
411 // initial guess for normal impulse
412 {
413 btScalar velocity_error = btDot(v_rel, m_contactNormalA); // magnitude of relative velocity
414 btScalar position_error = 0;
415 if (m_penetration > 0)
416 {
417 velocity_error += m_penetration / m_dt;
418 }
419 else
420 {
421 // add penetration correction vel
422 position_error = m_penetration * m_erp / m_dt;
423 }
424 // get the initial estimate of impulse magnitude to be applied
425 m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
426 }
427
428 // initial guess for tangential impulse
429 {
430 btScalar velocity_error = btDot(v_rel, m_contactTangent);
431 m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
432
433 if (m_collideMultibody)
434 {
435 btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
436 m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
437 }
438 }
439
440 // warm starting
441 // applyImpulse(m_rhs * m_contactNormalA);
442 // if (!m_collideStatic)
443 // {
444 // const btSoftBody::sCti& cti = m_contact->m_cti;
445 // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
446 // {
447 // m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
448 // }
449 // }
450}
451
452 btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
453{
454 return m_node->m_v;
455}
456
457 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
458{
459 btVector3 deltaVa(0, 0, 0);
460 if (!m_collideStatic)
461 {
462 if (!m_collideMultibody) // for rigid body
463 {
464 deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
465 }
466 else // for multibody
467 {
468 btMultiBodyLinkCollider* multibodyLinkCol = 0;
469 multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
470 if (multibodyLinkCol)
471 {
472 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
473 const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
474 const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
475 const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
476 const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
477 // add in the normal component of the va
478 btScalar vel = 0;
479 for (int k = 0; k < ndof; ++k)
480 {
481 vel += local_dv[k] * J_n[k];
482 }
483 deltaVa = m_contact->m_cti.m_normal * vel;
484
485 // add in the tangential components of the va
486 vel = 0;
487 for (int k = 0; k < ndof; ++k)
488 {
489 vel += local_dv[k] * J_t1[k];
490 }
491 deltaVa += m_contact->t1 * vel;
492
493 vel = 0;
494 for (int k = 0; k < ndof; ++k)
495 {
496 vel += local_dv[k] * J_t2[k];
497 }
498 deltaVa += m_contact->t2 * vel;
499 }
500 }
501 }
502 return deltaVa;
503}
504
505 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
506{
507 // std::cout << "node: " << m_node->index << '\n';
508 return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
509}
510
511 btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
512{
513 return m_node->m_splitv;
514}
515
516 btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
517{
518 return m_total_normal_dv + m_total_tangent_dv;
519}
520
521 void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
522{
523 m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
524 // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
525 // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
526 // if (!m_collideStatic)
527 // {
528 // // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
529 // // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
530 // btVector3 v_after = getDeltaVb() + m_node->m_v;
531 // // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
532 // }
533 // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';
534}
535
536// ================= face vs rigid constraints ===================
537 btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
538 btReducedDeformableBody* rsb,
539 const btSoftBody::DeformableFaceRigidContact& contact,
540 const btContactSolverInfo& infoGlobal,
541 btScalar dt,
542 bool useStrainLimiting)
543 : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
544{}
545
546 btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const
547{
548 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
549 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];
550 return vb;
551}
552
553 btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
554{
555 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
556 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];
557 return vb;
558}
559
560 btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
561{
562 btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
563 const btSoftBody::DeformableFaceRigidContact* contact = getContact();
564 if (m_face->m_n[0] == node)
565 {
566 return face_dv * contact->m_weights[0];
567 }
568 if (m_face->m_n[1] == node)
569 {
570 return face_dv * contact->m_weights[1];
571 }
572 btAssert(node == m_face->m_n[2]);
573 return face_dv * contact->m_weights[2];
574}
575
576 void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
577{
578 //
579}
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition btQuaternion.h:888
btReducedDeformableContactConstraint.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:543
btAssert
#define btAssert(x)
Definition btScalar.h:153
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition btSoftBodyHelpers.cpp:95
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition btVector3.h:890
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition btAlignedObjectArray.h:142
btCollisionObject::CO_FEATHERSTONE_LINK
@ CO_FEATHERSTONE_LINK
Definition btCollisionObject.h:155
btCollisionObject::CO_RIGID_BODY
@ CO_RIGID_BODY
Definition btCollisionObject.h:148
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition btCollisionObject.h:374
btCollisionObject::isStaticObject
bool isStaticObject() const
Definition btCollisionObject.h:197
btCollisionObject::getInterpolationWorldTransform
const btTransform & getInterpolationWorldTransform() const
Definition btCollisionObject.h:410
btDeformableRigidContactConstraint
Definition btDeformableContactConstraint.h:142
btDeformableRigidContactConstraint::m_total_normal_dv
btVector3 m_total_normal_dv
Definition btDeformableContactConstraint.h:144
btDeformableRigidContactConstraint::getVa
virtual btVector3 getVa() const
Definition btDeformableContactConstraint.cpp:153
btDeformableRigidContactConstraint::m_penetration
btScalar m_penetration
Definition btDeformableContactConstraint.h:146
btDeformableRigidContactConstraint::m_contact
const btSoftBody::DeformableRigidContact * m_contact
Definition btDeformableContactConstraint.h:149
btDeformableRigidContactConstraint::m_total_tangent_dv
btVector3 m_total_tangent_dv
Definition btDeformableContactConstraint.h:145
btDeformableStaticConstraint
Definition btDeformableContactConstraint.h:72
btDeformableStaticConstraint::m_node
btSoftBody::Node * m_node
Definition btDeformableContactConstraint.h:74
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition btMultiBodyLinkCollider.h:61
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition btMultiBodyLinkCollider.h:36
btMultiBody::getDeltaVelocityVector
const btScalar * getDeltaVelocityVector() const
Definition btMultiBody.h:307
btMultiBody::getNumDofs
int getNumDofs() const
Definition btMultiBody.h:167
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition btMultiBody.h:428
btReducedDeformableBody
Definition btReducedDeformableBody.h:12
btReducedDeformableBody::getImpulseFactor
virtual btMatrix3x3 getImpulseFactor(int n_node)
Definition btReducedDeformableBody.cpp:633
btReducedDeformableBody::internalComputeNodeDeltaVelocity
const btVector3 internalComputeNodeDeltaVelocity(const btTransform &ref_trans, int n_node) const
Definition btReducedDeformableBody.cpp:372
btReducedDeformableBody::internalApplyFullSpaceImpulse
void internalApplyFullSpaceImpulse(const btVector3 &impulse, const btVector3 &rel_pos, int n_node, btScalar dt)
Definition btReducedDeformableBody.cpp:693
btReducedDeformableBody::getRigidTransform
btTransform & getRigidTransform()
Definition btReducedDeformableBody.cpp:769
btReducedDeformableFaceRigidContactConstraint::getDv
virtual btVector3 getDv(const btSoftBody::Node *) const
Definition btReducedDeformableContactConstraint.cpp:560
btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint
btReducedDeformableFaceRigidContactConstraint()
Definition btReducedDeformableContactConstraint.h:174
btReducedDeformableFaceRigidContactConstraint::getSplitVb
virtual btVector3 getSplitVb() const
Definition btReducedDeformableContactConstraint.cpp:553
btReducedDeformableFaceRigidContactConstraint::getContact
const btSoftBody::DeformableFaceRigidContact * getContact() const
Definition btReducedDeformableContactConstraint.h:187
btReducedDeformableFaceRigidContactConstraint::getVb
virtual btVector3 getVb() const
Definition btReducedDeformableContactConstraint.cpp:546
btReducedDeformableFaceRigidContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition btReducedDeformableContactConstraint.cpp:576
btReducedDeformableFaceRigidContactConstraint::m_face
btSoftBody::Face * m_face
Definition btReducedDeformableContactConstraint.h:165
btReducedDeformableNodeRigidContactConstraint::getSplitVb
virtual btVector3 getSplitVb() const
Definition btReducedDeformableContactConstraint.cpp:511
btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint
btReducedDeformableNodeRigidContactConstraint()
Definition btReducedDeformableContactConstraint.h:131
btReducedDeformableNodeRigidContactConstraint::getDeltaVa
virtual btVector3 getDeltaVa() const
Definition btReducedDeformableContactConstraint.cpp:457
btReducedDeformableNodeRigidContactConstraint::getVb
virtual btVector3 getVb() const
Definition btReducedDeformableContactConstraint.cpp:452
btReducedDeformableNodeRigidContactConstraint::m_node
btSoftBody::Node * m_node
Definition btReducedDeformableContactConstraint.h:124
btReducedDeformableNodeRigidContactConstraint::getDv
virtual btVector3 getDv(const btSoftBody::Node *) const
Definition btReducedDeformableContactConstraint.cpp:516
btReducedDeformableNodeRigidContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition btReducedDeformableContactConstraint.cpp:521
btReducedDeformableNodeRigidContactConstraint::warmStarting
virtual void warmStarting()
Definition btReducedDeformableContactConstraint.cpp:365
btReducedDeformableNodeRigidContactConstraint::getDeltaVb
virtual btVector3 getDeltaVb() const
Definition btReducedDeformableContactConstraint.cpp:505
btReducedDeformableRigidContactConstraint
Definition btReducedDeformableContactConstraint.h:42
btReducedDeformableRigidContactConstraint::solveConstraint
virtual btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
Definition btReducedDeformableContactConstraint.cpp:107
btReducedDeformableRigidContactConstraint::m_collideMultibody
bool m_collideMultibody
Definition btReducedDeformableContactConstraint.h:45
btReducedDeformableRigidContactConstraint::m_tangentImpulseFactorInv
btScalar m_tangentImpulseFactorInv
Definition btReducedDeformableContactConstraint.h:60
btReducedDeformableRigidContactConstraint::m_collideStatic
bool m_collideStatic
Definition btReducedDeformableContactConstraint.h:44
btReducedDeformableRigidContactConstraint::m_angularComponentNormal
btVector3 m_angularComponentNormal
Definition btReducedDeformableContactConstraint.h:83
btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint
btReducedDeformableRigidContactConstraint()
Definition btReducedDeformableContactConstraint.h:93
btReducedDeformableRigidContactConstraint::m_solverBodyId
int m_solverBodyId
Definition btReducedDeformableContactConstraint.h:48
btReducedDeformableRigidContactConstraint::setSolverBody
void setSolverBody(const int bodyId, btSolverBody &solver_body)
Definition btReducedDeformableContactConstraint.cpp:81
btReducedDeformableRigidContactConstraint::calculateTangentialImpulse
void calculateTangentialImpulse(btScalar &deltaImpulse_tangent, btScalar &appliedImpulse, const btScalar rhs_tangent, const btScalar tangentImpulseFactorInv, const btVector3 &tangent, const btScalar lower_limit, const btScalar upper_limit, const btVector3 &deltaV_rel)
Definition btReducedDeformableContactConstraint.cpp:290
btReducedDeformableRigidContactConstraint::m_bufferVelocityA
btVector3 m_bufferVelocityA
Definition btReducedDeformableContactConstraint.h:80
btReducedDeformableRigidContactConstraint::m_tangentImpulseFactor
btScalar m_tangentImpulseFactor
Definition btReducedDeformableContactConstraint.h:58
btReducedDeformableRigidContactConstraint::m_dt
btScalar m_dt
Definition btReducedDeformableContactConstraint.h:52
btReducedDeformableRigidContactConstraint::m_angularComponentTangent
btVector3 m_angularComponentTangent
Definition btReducedDeformableContactConstraint.h:86
btReducedDeformableRigidContactConstraint::m_rsb
btReducedDeformableBody * m_rsb
Definition btReducedDeformableContactConstraint.h:50
btReducedDeformableRigidContactConstraint::m_linearComponentNormal
btVector3 m_linearComponentNormal
Definition btReducedDeformableContactConstraint.h:82
btReducedDeformableRigidContactConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition btReducedDeformableContactConstraint.h:111
btReducedDeformableRigidContactConstraint::getDeltaVb
virtual btVector3 getDeltaVb() const =0
btReducedDeformableRigidContactConstraint::m_friction
btScalar m_friction
Definition btReducedDeformableContactConstraint.h:70
btReducedDeformableRigidContactConstraint::m_contactNormalB
btVector3 m_contactNormalB
Definition btReducedDeformableContactConstraint.h:73
btReducedDeformableRigidContactConstraint::m_cfm_friction
btScalar m_cfm_friction
Definition btReducedDeformableContactConstraint.h:67
btReducedDeformableRigidContactConstraint::m_rhs_tangent
btScalar m_rhs_tangent
Definition btReducedDeformableContactConstraint.h:63
btReducedDeformableRigidContactConstraint::m_contactNormalA
btVector3 m_contactNormalA
Definition btReducedDeformableContactConstraint.h:72
btReducedDeformableRigidContactConstraint::m_tangentImpulseFactor2
btScalar m_tangentImpulseFactor2
Definition btReducedDeformableContactConstraint.h:59
btReducedDeformableRigidContactConstraint::m_bufferVelocityB
btVector3 m_bufferVelocityB
Definition btReducedDeformableContactConstraint.h:81
btReducedDeformableRigidContactConstraint::m_tangentImpulseFactorInv2
btScalar m_tangentImpulseFactorInv2
Definition btReducedDeformableContactConstraint.h:61
btReducedDeformableRigidContactConstraint::m_contactTangent2
btVector3 m_contactTangent2
Definition btReducedDeformableContactConstraint.h:75
btReducedDeformableRigidContactConstraint::m_linearComponentTangent
btVector3 m_linearComponentTangent
Definition btReducedDeformableContactConstraint.h:85
btReducedDeformableRigidContactConstraint::m_normalImpulseFactor
btScalar m_normalImpulseFactor
Definition btReducedDeformableContactConstraint.h:57
btReducedDeformableRigidContactConstraint::m_erp_friction
btScalar m_erp_friction
Definition btReducedDeformableContactConstraint.h:69
btReducedDeformableRigidContactConstraint::m_nodeQueryIndex
int m_nodeQueryIndex
Definition btReducedDeformableContactConstraint.h:47
btReducedDeformableRigidContactConstraint::m_appliedTangentImpulse
btScalar m_appliedTangentImpulse
Definition btReducedDeformableContactConstraint.h:55
btReducedDeformableRigidContactConstraint::m_rhs
btScalar m_rhs
Definition btReducedDeformableContactConstraint.h:62
btReducedDeformableRigidContactConstraint::m_impulseFactor
btMatrix3x3 m_impulseFactor
Definition btReducedDeformableContactConstraint.h:78
btReducedDeformableRigidContactConstraint::m_contactTangent
btVector3 m_contactTangent
Definition btReducedDeformableContactConstraint.h:74
btReducedDeformableRigidContactConstraint::m_relPosB
btVector3 m_relPosB
Definition btReducedDeformableContactConstraint.h:77
btReducedDeformableRigidContactConstraint::m_cfm
btScalar m_cfm
Definition btReducedDeformableContactConstraint.h:66
btReducedDeformableRigidContactConstraint::m_solverBody
btSolverBody * m_solverBody
Definition btReducedDeformableContactConstraint.h:51
btReducedDeformableRigidContactConstraint::m_rhs_tangent2
btScalar m_rhs_tangent2
Definition btReducedDeformableContactConstraint.h:64
btReducedDeformableRigidContactConstraint::m_erp
btScalar m_erp
Definition btReducedDeformableContactConstraint.h:68
btReducedDeformableRigidContactConstraint::getDeltaVa
virtual btVector3 getDeltaVa() const =0
btReducedDeformableRigidContactConstraint::m_relPosA
btVector3 m_relPosA
Definition btReducedDeformableContactConstraint.h:76
btReducedDeformableRigidContactConstraint::m_appliedNormalImpulse
btScalar m_appliedNormalImpulse
Definition btReducedDeformableContactConstraint.h:54
btReducedDeformableRigidContactConstraint::m_appliedTangentImpulse2
btScalar m_appliedTangentImpulse2
Definition btReducedDeformableContactConstraint.h:56
btReducedDeformableRigidContactConstraint::getVa
virtual btVector3 getVa() const
Definition btReducedDeformableContactConstraint.cpp:97
btReducedDeformableStaticConstraint::m_dt
btScalar m_dt
Definition btReducedDeformableContactConstraint.h:9
btReducedDeformableStaticConstraint::m_impulseFactor
btScalar m_impulseFactor
Definition btReducedDeformableContactConstraint.h:14
btReducedDeformableStaticConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition btReducedDeformableContactConstraint.h:16
btReducedDeformableStaticConstraint::m_ri
btVector3 m_ri
Definition btReducedDeformableContactConstraint.h:10
btReducedDeformableStaticConstraint::m_rsb
btReducedDeformableBody * m_rsb
Definition btReducedDeformableContactConstraint.h:8
btReducedDeformableStaticConstraint::m_impulseDirection
btVector3 m_impulseDirection
Definition btReducedDeformableContactConstraint.h:12
btReducedDeformableStaticConstraint::m_erp
btScalar m_erp
Definition btReducedDeformableContactConstraint.h:17
btReducedDeformableStaticConstraint::m_targetPos
btVector3 m_targetPos
Definition btReducedDeformableContactConstraint.h:11
btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint
btReducedDeformableStaticConstraint()
Definition btReducedDeformableContactConstraint.h:27
btReducedDeformableStaticConstraint::m_rhs
btScalar m_rhs
Definition btReducedDeformableContactConstraint.h:15
btReducedDeformableStaticConstraint::applyImpulse
virtual void applyImpulse(const btVector3 &impulse)
Definition btReducedDeformableContactConstraint.cpp:47
btReducedDeformableStaticConstraint::getDeltaVa
btVector3 getDeltaVa() const
Definition btReducedDeformableContactConstraint.cpp:53
btReducedDeformableStaticConstraint::solveConstraint
virtual btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
Definition btReducedDeformableContactConstraint.cpp:28
btReducedDeformableStaticConstraint::m_impulseFactorMatrix
btMatrix3x3 m_impulseFactorMatrix
Definition btReducedDeformableContactConstraint.h:13
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition btRigidBody.h:265
btSoftBody::DeformableFaceRigidContact
Definition btSoftBody.h:391
btSoftBody::DeformableFaceRigidContact::m_bary
btVector3 m_bary
Definition btSoftBody.h:395
btSoftBody::DeformableFaceRigidContact::m_weights
btVector3 m_weights
Definition btSoftBody.h:396
btSoftBody::DeformableNodeRigidContact
Definition btSoftBody.h:379
btSoftBody::DeformableNodeRigidContact::m_node
Node * m_node
Definition btSoftBody.h:381
btSoftBody::DeformableRigidContact
Definition btSoftBody.h:360
btSoftBody::DeformableRigidContact::t1
btVector3 t1
Definition btSoftBody.h:374
btSoftBody::DeformableRigidContact::m_cti
sCti m_cti
Definition btSoftBody.h:362
btSoftBody::DeformableRigidContact::m_c0
btMatrix3x3 m_c0
Definition btSoftBody.h:363
btSoftBody::DeformableRigidContact::t2
btVector3 t2
Definition btSoftBody.h:375
btSoftBody::DeformableRigidContact::jacobianData_t1
btMultiBodyJacobianData jacobianData_t1
Definition btSoftBody.h:372
btSoftBody::DeformableRigidContact::jacobianData_normal
btMultiBodyJacobianData jacobianData_normal
Definition btSoftBody.h:371
btSoftBody::DeformableRigidContact::jacobianData_t2
btMultiBodyJacobianData jacobianData_t2
Definition btSoftBody.h:373
btSoftBody::m_nodes
tNodeArray m_nodes
Definition btSoftBody.h:814
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition btTransform.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
btVector3::norm
btScalar norm() const
Return the norm (length) of the vector.
Definition btVector3.h:263
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition btVector3.h:949
btVector3::setZero
void setZero()
Definition btVector3.h:671
btContactSolverInfoData::m_friction
btScalar m_friction
Definition btContactSolverInfo.h:41
btContactSolverInfoData::m_deformable_erp
btScalar m_deformable_erp
Definition btContactSolverInfo.h:49
btContactSolverInfoData::m_deformable_cfm
btScalar m_deformable_cfm
Definition btContactSolverInfo.h:50
btContactSolverInfo
Definition btContactSolverInfo.h:76
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition btMultiBodyConstraint.h:46
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition btMultiBodyConstraint.h:45
btSoftBody::Face::m_n
Node * m_n[3]
Definition btSoftBody.h:307
btSoftBody::Node
Definition btSoftBody.h:268
btSoftBody::Node::m_x
btVector3 m_x
Definition btSoftBody.h:269
btSoftBody::Node::m_splitv
btVector3 m_splitv
Definition btSoftBody.h:281
btSoftBody::Node::m_v
btVector3 m_v
Definition btSoftBody.h:271
btSoftBody::Node::index
int index
Definition btSoftBody.h:280
btSoftBody::sCti::m_colObj
const btCollisionObject * m_colObj
Definition btSoftBody.h:226
btSoftBody::sCti::m_normal
btVector3 m_normal
Definition btSoftBody.h:227
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition btSolverBody.h:105
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition btSolverBody.h:202
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition btSolverBody.h:120
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition btSolverBody.h:197
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition btSolverBody.h:243
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition btSolverBody.h:212