28 m_relativePositionTarget(0)
51 return col->getIslandTag();
55 if (m_bodyA->getLink(
m_linkA).m_collider)
56 return m_bodyA->getLink(
m_linkA).m_collider->getIslandTag();
70 return col->getIslandTag();
103 unsigned int offsetA = 6 + (m_bodyA->getLink(
m_linkA).m_dofOffset + linkDoF);
104 unsigned int offsetB = 6 + (
m_bodyB->getLink(
m_linkB).m_dofOffset + linkDoF);
117 for (
int row = 0; row < numRows; row++)
130 currentVelocity += auxVel;
133 btScalar currentPositionA = m_bodyA->getJointPosMultiDof(
m_linkA)[dof];
136 currentPositionA -= m_bodyA->getJointPosMultiDof(
m_gearAuxLink)[dof];
141 posError = -
m_erp * (desiredPositionDiff -
diff);
144 btScalar desiredRelativeVelocity = auxVel;
146 fillMultiBodyConstraint(constraintRow,
data,
jacobianA(row),
jacobianB(row), dummy, dummy, dummy, dummy, posError,
infoGlobal, -
m_maxAppliedImpulse,
m_maxAppliedImpulse,
false, 1,
false, desiredRelativeVelocity);
148 constraintRow.m_orgConstraint =
this;
149 constraintRow.m_orgDofIndex = row;
153 switch (m_bodyA->getLink(
m_linkA).m_jointType)
157 constraintRow.m_contactNormal1.setZero();
158 constraintRow.m_contactNormal2.setZero();
160 constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
161 constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
168 constraintRow.m_contactNormal1 = prismaticAxisInWorld;
169 constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
170 constraintRow.m_relpos1CrossNormal.setZero();
171 constraintRow.m_relpos2CrossNormal.setZero();
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
@ MULTIBODY_CONSTRAINT_GEAR
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...
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 ~btMultiBodyGearConstraint()
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
btScalar m_relativePositionTarget
btMultiBodyGearConstraint(btMultiBody *bodyA, int linkA, btMultiBody *bodyB, int linkB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB)
This file was written by Erwin Coumans.
virtual void finalizeMultiDof()
virtual int getIslandIdA() const
virtual int getIslandIdB() const
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)