38 nonContactResidual = 0;
39 for (
int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
41 int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
46 nonContactResidual =
btMax(nonContactResidual, residual * residual);
48 if (constraint.m_multiBodyA)
49 constraint.m_multiBodyA->setPosUpdated(
false);
50 if (constraint.m_multiBodyB)
51 constraint.m_multiBodyB->setPosUpdated(
false);
54 leastSquaredResidual =
btMax(leastSquaredResidual, nonContactResidual);
69 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
71 if (constraint.m_multiBodyA)
72 constraint.m_multiBodyA->setPosUpdated(
false);
73 if (constraint.m_multiBodyB)
74 constraint.m_multiBodyB->setPosUpdated(
false);
91 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
92 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
94 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
96 if (frictionConstraint.m_multiBodyA)
97 frictionConstraint.m_multiBodyA->setPosUpdated(
false);
98 if (frictionConstraint.m_multiBodyB)
99 frictionConstraint.m_multiBodyB->setPosUpdated(
false);
116 if (totalImpulse >
btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
118 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
119 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
120 frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
121 frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
124 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
126 if (frictionConstraint.m_multiBodyA)
127 frictionConstraint.m_multiBodyA->setPosUpdated(
false);
128 if (frictionConstraint.m_multiBodyB)
129 frictionConstraint.m_multiBodyB->setPosUpdated(
false);
131 if (frictionConstraintB.m_multiBodyA)
132 frictionConstraintB.m_multiBodyA->setPosUpdated(
false);
133 if (frictionConstraintB.m_multiBodyB)
134 frictionConstraintB.m_multiBodyB->setPosUpdated(
false);
150 btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
152 if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
154 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
155 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
156 frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
157 frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
159 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
161 if (frictionConstraintB.m_multiBodyA)
162 frictionConstraintB.m_multiBodyA->setPosUpdated(
false);
163 if (frictionConstraintB.m_multiBodyB)
164 frictionConstraintB.m_multiBodyB->setPosUpdated(
false);
166 if (frictionConstraint.m_multiBodyA)
167 frictionConstraint.m_multiBodyA->setPosUpdated(
false);
168 if (frictionConstraint.m_multiBodyB)
169 frictionConstraint.m_multiBodyB->setPosUpdated(
false);
187 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
188 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
190 leastSquaredResidual =
btMax(leastSquaredResidual, residual * residual);
192 if (frictionConstraint.m_multiBodyA)
193 frictionConstraint.m_multiBodyA->setPosUpdated(
false);
194 if (frictionConstraint.m_multiBodyB)
195 frictionConstraint.m_multiBodyB->setPosUpdated(
false);
200 return leastSquaredResidual;
205 m_multiBodyNonContactConstraints.resize(0);
231 for (
int i = 0; i < ndof; ++i)
232 m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
247 ndofA =
c.m_multiBodyA->getNumDofs() + 6;
248 for (
int i = 0; i < ndofA; ++i)
249 deltaVelADotn +=
m_data.m_jacobians[
c.m_jacAindex + i] *
m_data.m_deltaVelocities[
c.m_deltaVelAindex + i];
251 else if (
c.m_solverBodyIdA >= 0)
253 bodyA = &m_tmpSolverBodyPool[
c.m_solverBodyIdA];
254 deltaVelADotn +=
c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) +
c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
259 ndofB =
c.m_multiBodyB->getNumDofs() + 6;
260 for (
int i = 0; i < ndofB; ++i)
261 deltaVelBDotn +=
m_data.m_jacobians[
c.m_jacBindex + i] *
m_data.m_deltaVelocities[
c.m_deltaVelBindex + i];
263 else if (
c.m_solverBodyIdB >= 0)
265 bodyB = &m_tmpSolverBodyPool[
c.m_solverBodyIdB];
266 deltaVelBDotn +=
c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) +
c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
269 deltaImpulse -= deltaVelADotn *
c.m_jacDiagABInv;
270 deltaImpulse -= deltaVelBDotn *
c.m_jacDiagABInv;
273 if (
sum <
c.m_lowerLimit)
275 deltaImpulse =
c.m_lowerLimit -
c.m_appliedImpulse;
276 c.m_appliedImpulse =
c.m_lowerLimit;
278 else if (
sum >
c.m_upperLimit)
280 deltaImpulse =
c.m_upperLimit -
c.m_appliedImpulse;
281 c.m_appliedImpulse =
c.m_upperLimit;
285 c.m_appliedImpulse =
sum;
290 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacAindex], deltaImpulse,
c.m_deltaVelAindex, ndofA);
291 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
294 c.m_multiBodyA->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacAindex], deltaImpulse);
297 else if (
c.m_solverBodyIdA >= 0)
299 bodyA->internalApplyImpulse(
c.m_contactNormal1 * bodyA->internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
303 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacBindex], deltaImpulse,
c.m_deltaVelBindex, ndofB);
304 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
307 c.m_multiBodyB->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacBindex], deltaImpulse);
310 else if (
c.m_solverBodyIdB >= 0)
312 bodyB->internalApplyImpulse(
c.m_contactNormal2 * bodyB->internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
314 btScalar deltaVel = deltaImpulse /
c.m_jacDiagABInv;
327 deltaImpulseB = cB.m_rhs -
btScalar(cB.m_appliedImpulse) * cB.m_cfm;
332 ndofA = cB.m_multiBodyA->getNumDofs() + 6;
333 for (
int i = 0; i < ndofA; ++i)
334 deltaVelADotn +=
m_data.m_jacobians[cB.m_jacAindex + i] *
m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
336 else if (cB.m_solverBodyIdA >= 0)
338 bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
339 deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
344 ndofB = cB.m_multiBodyB->getNumDofs() + 6;
345 for (
int i = 0; i < ndofB; ++i)
346 deltaVelBDotn +=
m_data.m_jacobians[cB.m_jacBindex + i] *
m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
348 else if (cB.m_solverBodyIdB >= 0)
350 bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
351 deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
354 deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv;
355 deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
356 sumB =
btScalar(cB.m_appliedImpulse) + deltaImpulseB;
364 deltaImpulseA = cA.m_rhs -
btScalar(cA.m_appliedImpulse) * cA.m_cfm;
369 ndofA = cA.m_multiBodyA->getNumDofs() + 6;
370 for (
int i = 0; i < ndofA; ++i)
371 deltaVelADotn +=
m_data.m_jacobians[cA.m_jacAindex + i] *
m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
373 else if (cA.m_solverBodyIdA >= 0)
375 bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
376 deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
381 ndofB = cA.m_multiBodyB->getNumDofs() + 6;
382 for (
int i = 0; i < ndofB; ++i)
383 deltaVelBDotn +=
m_data.m_jacobians[cA.m_jacBindex + i] *
m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
385 else if (cA.m_solverBodyIdB >= 0)
387 bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
388 deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
391 deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv;
392 deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
393 sumA =
btScalar(cA.m_appliedImpulse) + deltaImpulseA;
397 if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
403 if (sumA < -sumAclipped)
405 deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
406 cA.m_appliedImpulse = -sumAclipped;
408 else if (sumA > sumAclipped)
410 deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
411 cA.m_appliedImpulse = sumAclipped;
415 cA.m_appliedImpulse = sumA;
418 if (sumB < -sumBclipped)
420 deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
421 cB.m_appliedImpulse = -sumBclipped;
423 else if (sumB > sumBclipped)
425 deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
426 cB.m_appliedImpulse = sumBclipped;
430 cB.m_appliedImpulse = sumB;
439 cA.m_appliedImpulse = sumA;
440 cB.m_appliedImpulse = sumB;
445 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
446 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
449 cA.m_multiBodyA->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
452 else if (cA.m_solverBodyIdA >= 0)
454 bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
458 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
459 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
462 cA.m_multiBodyB->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
465 else if (cA.m_solverBodyIdB >= 0)
467 bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
472 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
473 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
476 cB.m_multiBodyA->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
479 else if (cB.m_solverBodyIdA >= 0)
481 bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
485 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
486 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
489 cB.m_multiBodyB->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
492 else if (cB.m_solverBodyIdB >= 0)
494 bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
497 btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
503 BT_PROFILE(
"setupMultiBodyContactConstraint");
507 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
508 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
513 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
514 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
516 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
517 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
520 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
522 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
569 if (solverConstraint.m_linkA < 0)
571 rel_pos1 = pos1 - multiBodyA->getBasePos();
575 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
577 const int ndofA = multiBodyA->getNumDofs() + 6;
579 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
581 if (solverConstraint.m_deltaVelAindex < 0)
583 solverConstraint.m_deltaVelAindex =
m_data.m_deltaVelocities.
size();
584 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
589 btAssert(
m_data.m_deltaVelocities.
size() >= solverConstraint.m_deltaVelAindex + ndofA);
592 solverConstraint.m_jacAindex =
m_data.m_jacobians.
size();
597 btScalar* jac1 = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
599 btScalar* delta = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
600 multiBodyA->calcAccelerationDeltasMultiDof(&
m_data.m_jacobians[solverConstraint.m_jacAindex], delta,
m_data.scratch_r,
m_data.scratch_v);
602 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
603 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
604 solverConstraint.m_contactNormal1 = contactNormal;
608 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
609 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
610 solverConstraint.m_contactNormal1 = contactNormal;
616 if (solverConstraint.m_linkB < 0)
618 rel_pos2 = pos2 - multiBodyB->getBasePos();
622 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
625 const int ndofB = multiBodyB->getNumDofs() + 6;
627 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
628 if (solverConstraint.m_deltaVelBindex < 0)
630 solverConstraint.m_deltaVelBindex =
m_data.m_deltaVelocities.
size();
631 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
635 solverConstraint.m_jacBindex =
m_data.m_jacobians.
size();
641 multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.
getPositionWorldOnB(), -contactNormal, &
m_data.m_jacobians[solverConstraint.m_jacBindex],
m_data.scratch_r,
m_data.scratch_v,
m_data.scratch_m);
642 multiBodyB->calcAccelerationDeltasMultiDof(&
m_data.m_jacobians[solverConstraint.m_jacBindex], &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],
m_data.scratch_r,
m_data.scratch_v);
644 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
645 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
646 solverConstraint.m_contactNormal2 = -contactNormal;
650 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
651 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
652 solverConstraint.m_contactNormal2 = -contactNormal;
668 ndofA = multiBodyA->getNumDofs() + 6;
669 jacA = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
670 lambdaA = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
671 for (
int i = 0; i < ndofA; ++i)
682 vec = (solverConstraint.m_angularComponentA).
cross(rel_pos1);
683 denom0 = rb0->
getInvMass() + contactNormal.dot(vec);
688 const int ndofB = multiBodyB->getNumDofs() + 6;
689 jacB = &
m_data.m_jacobians[solverConstraint.m_jacBindex];
690 lambdaB = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
691 for (
int i = 0; i < ndofB; ++i)
702 vec = (-solverConstraint.m_angularComponentB).
cross(rel_pos2);
703 denom1 = rb1->
getInvMass() + contactNormal.dot(vec);
710 solverConstraint.m_jacDiagABInv = relaxation / (d);
715 solverConstraint.m_jacDiagABInv = 0.f;
742 ndofA = multiBodyA->getNumDofs() + 6;
743 btScalar* jacA = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
744 for (
int i = 0; i < ndofA; ++i)
745 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
754 .
dot(solverConstraint.m_contactNormal1);
759 ndofB = multiBodyB->getNumDofs() + 6;
760 btScalar* jacB = &
m_data.m_jacobians[solverConstraint.m_jacBindex];
761 for (
int i = 0; i < ndofB; ++i)
762 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
771 .
dot(solverConstraint.m_contactNormal2);
789 btScalar velocityError = restitution - rel_vel;
807 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
808 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
815 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
816 solverConstraint.m_rhsPenetration = 0.f;
825 solverConstraint.m_lowerLimit = 0;
826 solverConstraint.m_upperLimit = 1e10f;
830 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
831 solverConstraint.m_rhsPenetration = 0.f;
832 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
833 solverConstraint.m_upperLimit = solverConstraint.m_friction;
836 solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
844 if (solverConstraint.m_appliedImpulse < 0)
845 solverConstraint.m_appliedImpulse = 0;
849 solverConstraint.m_appliedImpulse = 0.f;
852 if (solverConstraint.m_appliedImpulse)
856 btScalar impulse = solverConstraint.m_appliedImpulse;
857 btScalar* deltaV = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
858 multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
860 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
865 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->
getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
869 btScalar impulse = solverConstraint.m_appliedImpulse;
870 btScalar* deltaV = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
871 multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
872 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
877 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->
getLinearFactor(), -solverConstraint.m_angularComponentB, -(
btScalar)solverConstraint.m_appliedImpulse);
883 solverConstraint.m_appliedImpulse = 0.f;
884 solverConstraint.m_appliedPushImpulse = 0.f;
896 BT_PROFILE(
"setupMultiBodyRollingFrictionConstraint");
900 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
901 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
906 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
907 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
909 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
910 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
913 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
915 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
923 if (solverConstraint.m_linkA < 0)
925 rel_pos1 = pos1 - multiBodyA->getBasePos();
929 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
931 const int ndofA = multiBodyA->getNumDofs() + 6;
933 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
935 if (solverConstraint.m_deltaVelAindex < 0)
937 solverConstraint.m_deltaVelAindex =
m_data.m_deltaVelocities.
size();
938 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
943 btAssert(
m_data.m_deltaVelocities.
size() >= solverConstraint.m_deltaVelAindex + ndofA);
946 solverConstraint.m_jacAindex =
m_data.m_jacobians.
size();
951 btScalar* jac1 = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
953 btScalar* delta = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
954 multiBodyA->calcAccelerationDeltasMultiDof(&
m_data.m_jacobians[solverConstraint.m_jacAindex], delta,
m_data.scratch_r,
m_data.scratch_v);
956 btVector3 torqueAxis0 = constraintNormal;
957 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
958 solverConstraint.m_contactNormal1 =
btVector3(0, 0, 0);
962 btVector3 torqueAxis0 = constraintNormal;
963 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
964 solverConstraint.m_contactNormal1 =
btVector3(0, 0, 0);
970 if (solverConstraint.m_linkB < 0)
972 rel_pos2 = pos2 - multiBodyB->getBasePos();
976 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
979 const int ndofB = multiBodyB->getNumDofs() + 6;
981 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
982 if (solverConstraint.m_deltaVelBindex < 0)
984 solverConstraint.m_deltaVelBindex =
m_data.m_deltaVelocities.
size();
985 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
989 solverConstraint.m_jacBindex =
m_data.m_jacobians.
size();
996 multiBodyB->calcAccelerationDeltasMultiDof(&
m_data.m_jacobians[solverConstraint.m_jacBindex], &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],
m_data.scratch_r,
m_data.scratch_v);
998 btVector3 torqueAxis1 = -constraintNormal;
999 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1000 solverConstraint.m_contactNormal2 = -
btVector3(0, 0, 0);
1004 btVector3 torqueAxis1 = -constraintNormal;
1005 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1006 solverConstraint.m_contactNormal2 = -
btVector3(0, 0, 0);
1021 ndofA = multiBodyA->getNumDofs() + 6;
1022 jacA = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
1023 lambdaA = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1024 for (
int i = 0; i < ndofA; ++i)
1036 denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1041 const int ndofB = multiBodyB->getNumDofs() + 6;
1042 jacB = &
m_data.m_jacobians[solverConstraint.m_jacBindex];
1043 lambdaB = &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1044 for (
int i = 0; i < ndofB; ++i)
1056 denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1063 solverConstraint.m_jacDiagABInv = relaxation / (d);
1068 solverConstraint.m_jacDiagABInv = 0.f;
1084 ndofA = multiBodyA->getNumDofs() + 6;
1085 btScalar* jacA = &
m_data.m_jacobians[solverConstraint.m_jacAindex];
1086 for (
int i = 0; i < ndofA; ++i)
1087 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1093 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1094 rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity :
btVector3(0, 0, 0));
1099 ndofB = multiBodyB->getNumDofs() + 6;
1100 btScalar* jacB = &
m_data.m_jacobians[solverConstraint.m_jacBindex];
1101 for (
int i = 0; i < ndofB; ++i)
1102 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1108 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1109 rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse :
btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity :
btVector3(0, 0, 0));
1113 solverConstraint.m_friction = combinedTorsionalFriction;
1125 solverConstraint.m_appliedImpulse = 0.f;
1126 solverConstraint.m_appliedPushImpulse = 0.f;
1129 btScalar velocityError = 0 - rel_vel;
1131 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1133 solverConstraint.m_rhs = velocityImpulse;
1134 solverConstraint.m_rhsPenetration = 0.f;
1135 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1136 solverConstraint.m_upperLimit = solverConstraint.m_friction;
1142 btMultiBodySolverConstraint&
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(
const btVector3& normalAxis,
const btScalar& appliedImpulse,
btPersistentManifold* manifold,
int frictionIndex,
btManifoldPoint& cp,
btCollisionObject* colObj0,
btCollisionObject* colObj1,
btScalar relaxation,
const btContactSolverInfo&
infoGlobal,
btScalar desiredVelocity,
btScalar cfmSlip)
1144 BT_PROFILE(
"addMultiBodyFrictionConstraint");
1146 solverConstraint.m_orgConstraint = 0;
1147 solverConstraint.m_orgDofIndex = -1;
1149 solverConstraint.m_frictionIndex = frictionIndex;
1150 bool isFriction =
true;
1161 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1162 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1163 solverConstraint.m_multiBodyA = mbA;
1165 solverConstraint.m_linkA = fcA->
m_link;
1167 solverConstraint.m_multiBodyB = mbB;
1169 solverConstraint.m_linkB = fcB->
m_link;
1171 solverConstraint.m_originalContactPoint = &cp;
1174 return solverConstraint;
1178 btScalar combinedTorsionalFriction,
1181 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1186 solverConstraint.m_orgConstraint = 0;
1187 solverConstraint.m_orgDofIndex = -1;
1189 solverConstraint.m_frictionIndex = frictionIndex;
1190 bool isFriction =
true;
1201 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1202 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1203 solverConstraint.m_multiBodyA = mbA;
1205 solverConstraint.m_linkA = fcA->
m_link;
1207 solverConstraint.m_multiBodyB = mbB;
1209 solverConstraint.m_linkB = fcB->
m_link;
1211 solverConstraint.m_originalContactPoint = &cp;
1214 return solverConstraint;
1218 btScalar combinedTorsionalFriction,
1221 BT_PROFILE(
"addMultiBodyRollingFrictionConstraint");
1224 solverConstraint.m_orgConstraint = 0;
1225 solverConstraint.m_orgDofIndex = -1;
1227 solverConstraint.m_frictionIndex = frictionIndex;
1228 bool isFriction =
true;
1239 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1240 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1241 solverConstraint.m_multiBodyA = mbA;
1243 solverConstraint.m_linkA = fcA->
m_link;
1245 solverConstraint.m_multiBodyB = mbB;
1247 solverConstraint.m_linkB = fcB->
m_link;
1249 solverConstraint.m_originalContactPoint = &cp;
1252 return solverConstraint;
1278 int rollingFriction = 4;
1280 for (
int j = 0; j < manifold->getNumContacts(); j++)
1284 if (cp.
getDistance() <= manifold->getContactProcessingThreshold())
1294 solverConstraint.m_orgConstraint = 0;
1295 solverConstraint.m_orgDofIndex = -1;
1296 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1297 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1298 solverConstraint.m_multiBodyA = mbA;
1300 solverConstraint.m_linkA = fcA->
m_link;
1302 solverConstraint.m_multiBodyB = mbB;
1304 solverConstraint.m_linkB = fcB->
m_link;
1306 solverConstraint.m_originalContactPoint = &cp;
1308 bool isFriction =
false;
1315 #define ENABLE_FRICTION
1316 #ifdef ENABLE_FRICTION
1339 if (rollingFriction > 0)
1400 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir1, cp.
m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation,
infoGlobal, cp.
m_contactMotion1, cp.
m_frictionCFM);
1403 addMultiBodyFrictionConstraint(cp.
m_lateralFrictionDir2, cp.
m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation,
infoGlobal, cp.
m_contactMotion2, cp.
m_frictionCFM);
1404 solverConstraint.m_appliedImpulse = 0.f;
1405 solverConstraint.m_appliedPushImpulse = 0.f;
1442 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
1451 for (
int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1454 m_multiBodyNonContactConstraints[i];
1455 solverConstraint.m_appliedImpulse =
1456 solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
1459 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
1460 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
1461 if (solverConstraint.m_appliedImpulse)
1465 int ndofA = multiBodyA->getNumDofs() + 6;
1467 &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1468 btScalar impulse = solverConstraint.m_appliedImpulse;
1469 multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
1470 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
1474 int ndofB = multiBodyB->getNumDofs() + 6;
1476 &
m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1477 btScalar impulse = solverConstraint.m_appliedImpulse;
1478 multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
1479 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
1486 for (
int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1489 solverConstraint.m_appliedImpulse = 0;
1503 if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1507 int numDofsPlusBase = 6+mb->getNumDofs();
1508 forceVector.
resize(numDofsPlusBase);
1509 for (
int i=0;i<numDofsPlusBase;i++)
1511 forceVector[i] =
data.m_jacobians[jacIndex+i]*appliedImpulse;
1514 output.resize(numDofsPlusBase);
1515 bool applyJointFeedback =
true;
1516 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&
output[0],
data.scratch_r,
data.scratch_v,applyJointFeedback);
1528 if (
c.m_orgConstraint)
1530 c.m_orgConstraint->internalSetAppliedImpulse(
c.m_orgDofIndex,
c.m_appliedImpulse);
1535 c.m_multiBodyA->setCompanionId(-1);
1536 btVector3 force =
c.m_contactNormal1 * (
c.m_appliedImpulse / deltaTime);
1537 btVector3 torque =
c.m_relpos1CrossNormal * (
c.m_appliedImpulse / deltaTime);
1540 c.m_multiBodyA->addBaseConstraintForce(force);
1541 c.m_multiBodyA->addBaseConstraintTorque(torque);
1545 c.m_multiBodyA->addLinkConstraintForce(
c.m_linkA, force);
1547 c.m_multiBodyA->addLinkConstraintTorque(
c.m_linkA, torque);
1554 c.m_multiBodyB->setCompanionId(-1);
1555 btVector3 force =
c.m_contactNormal2 * (
c.m_appliedImpulse / deltaTime);
1556 btVector3 torque =
c.m_relpos2CrossNormal * (
c.m_appliedImpulse / deltaTime);
1559 c.m_multiBodyB->addBaseConstraintForce(force);
1560 c.m_multiBodyB->addBaseConstraintTorque(torque);
1565 c.m_multiBodyB->addLinkConstraintForce(
c.m_linkB, force);
1567 c.m_multiBodyB->addLinkConstraintTorque(
c.m_linkB, torque);
1574 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1578 c.m_multiBodyA->applyDeltaVeeMultiDof(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacAindex],
c.m_appliedImpulse);
1583 c.m_multiBodyB->applyDeltaVeeMultiDof(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacBindex],
c.m_appliedImpulse);
1590 BT_PROFILE(
"btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1595 for (
int i = 0; i < numPoolConstraints; i++)
1608 for (
int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1617 for (
int j = 0; j < numPoolConstraints; j++)
1641 for (
int j=0;j<numPoolConstraints;j++)
1648 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1652 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1694 for (
int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1697 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1701 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1708 numPoolConstraints = m_multiBodyNonContactConstraints.size();
1712 for (
int i=0;i<numPoolConstraints;i++)
1720 fb->m_appliedForceBodyA +=
c.m_contactNormal1*
c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/
infoGlobal.
m_timeStep;
1721 fb->m_appliedForceBodyB +=
c.m_contactNormal2*
c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/
infoGlobal.
m_timeStep;
1722 fb->m_appliedTorqueBodyA +=
c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*
c.m_appliedImpulse/
infoGlobal.
m_timeStep;
1723 fb->m_appliedTorqueBodyB +=
c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*
c.m_appliedImpulse/
infoGlobal.
m_timeStep;
1727 constr->internalSetAppliedImpulse(
c.m_appliedImpulse);
1728 if (
btFabs(
c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1730 constr->setEnabled(
false);
1740 void btMultiBodyConstraintSolver::solveMultiBodyGroup(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifold,
int numManifolds,
btTypedConstraint**
constraints,
int numConstraints,
btMultiBodyConstraint** multiBodyConstraints,
int numMultiBodyConstraints,
const btContactSolverInfo& info,
btIDebugDraw* debugDrawer,
btDispatcher* dispatcher)
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
@ CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
int m_tmpNumMultiBodyConstraints
btMultiBodySolverConstraint & addMultiBodySpinningFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, const btScalar &appliedImpulse, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, const btScalar &appliedImpulse, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
btMultiBodySolverConstraint & addMultiBodyTorsionalFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
SIMD_FORCE_INLINE btScalar btCos(btScalar x)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btSin(btScalar x)
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
static T sum(const btAlignedObjectArray< T > &items)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
const btVector3 & getPositionWorldOnB() const
btScalar m_combinedRestitution
btVector3 m_lateralFrictionDir2
btScalar m_combinedContactDamping1
btScalar m_appliedImpulseLateral2
btScalar m_appliedImpulse
btScalar m_appliedImpulseLateral1
const btVector3 & getPositionWorldOnA() const
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
btMultiBody * m_multiBody
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
const btVector3 & getLinearFactor() const
btScalar getInvMass() const
const btVector3 & getTotalTorque() const
const btVector3 & getTotalForce() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
const btVector3 & getAngularFactor() const
BLI_INLINE float fb(float length, float L)
ccl_global KernelShaderEvalInput ccl_global float * output
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T distance(const T &a, const T &b)