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

3
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -184,6 +184,9 @@ public:
virtual void debugDraw(class btIDebugDraw* drawer)=0;
virtual void setGearRatio(btScalar ratio) {}
virtual void setGearAuxLink(int gearAuxLink) {}
};
#endif //BT_MULTIBODY_CONSTRAINT_H

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);

7
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h
View File

@@ -31,10 +31,11 @@ protected:
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
int m_gearAuxLink;
public:
btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyGearConstraint();
@@ -97,6 +98,10 @@ public:
{
m_gearRatio = gearRatio;
}
virtual void setGearAuxLink(int gearAuxLink)
{
m_gearAuxLink = gearAuxLink;
}
};