38 unsigned int offset = 6 + m_bodyA->getLink(
m_linkA).m_dofOffset;
61 return col->getIslandTag();
65 if (m_bodyA->getLink(
m_linkA).m_collider)
66 return m_bodyA->getLink(
m_linkA).m_collider->getIslandTag();
80 return col->getIslandTag();
121 constraintRow.m_orgConstraint =
this;
122 constraintRow.m_orgDofIndex = row;
124 constraintRow.m_multiBodyA = m_bodyA;
125 constraintRow.m_multiBodyB =
m_bodyB;
129 btScalar rel_vel =
fillMultiBodyConstraint(constraintRow,
data,
jacobianA(row),
jacobianB(row), dummy, dummy, dummy, dummy, posError,
infoGlobal, 0,
m_maxAppliedImpulse);
134 switch (m_bodyA->getLink(
m_linkA).m_jointType)
138 constraintRow.m_contactNormal1.setZero();
139 constraintRow.m_contactNormal2.setZero();
140 btVector3 revoluteAxisInWorld = direction *
quatRotate(m_bodyA->getLink(
m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(
m_linkA).m_axes[0].m_topVec);
141 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
142 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
148 btVector3 prismaticAxisInWorld = direction *
quatRotate(m_bodyA->getLink(
m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(
m_linkA).m_axes[0].m_bottomVec);
149 constraintRow.m_contactNormal1 = prismaticAxisInWorld;
150 constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
151 constraintRow.m_relpos1CrossNormal.setZero();
152 constraintRow.m_relpos2CrossNormal.setZero();
181 btScalar penetrationImpulse = positionalError * constraintRow.m_jacDiagABInv;
182 btScalar velocityImpulse = velocityError * constraintRow.m_jacDiagABInv;
186 constraintRow.m_rhs = penetrationImpulse + velocityImpulse;
187 constraintRow.m_rhsPenetration = 0.f;
192 constraintRow.m_rhs = velocityImpulse;
193 constraintRow.m_rhsPenetration = penetrationImpulse;
@ MULTIBODY_CONSTRAINT_LIMIT
btScalar getPosition(int row) const
void setPosition(int row, btScalar pos)
btScalar * jacobianB(int row)
void allocateJacobiansMultiDof()
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar m_maxAppliedImpulse
btScalar * jacobianA(int row)
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btMultibodyLink & getLink(int index) const
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
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 T & expandNonInitializing()
virtual int getIslandIdB() const
virtual void finalizeMultiDof()
virtual int getIslandIdA() const
virtual ~btMultiBodyJointLimitConstraint()
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btMultiBodyJointLimitConstraint(btMultiBody *body, int link, btScalar lower, btScalar upper)
This file was written by Erwin Coumans.
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset