Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

allow auxilary link to be used for gear btMultiBodyGearConstraint.

Browse Source
This commit is contained in:
Erwin Coumans
2017-06-23 20:24:04 -07:00
parent 8e9f5ef3f3
commit 65e22ba3e9
11 changed files with 74 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
View File

@@ -22,7 +22,8 @@ subject to the following restrictions:
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
m_gearRatio(1)
m_gearRatio(1),
m_gearAuxLink(-1)
{
}
@@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar auxVel = 0;
if (m_gearAuxLink>=0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
//btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar desiredRelativeVelocity = 0;
btScalar desiredRelativeVelocity = auxVel;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);