summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp86
1 files changed, 38 insertions, 48 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
index 09ddd65cd8..bf6b811d26 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp
@@ -21,20 +21,18 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
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_gearAuxLink(-1),
- m_erp(0),
- m_relativePositionTarget(0)
+ : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, 1, false),
+ m_gearRatio(1),
+ m_gearAuxLink(-1),
+ m_erp(0),
+ m_relativePositionTarget(0)
{
-
}
void btMultiBodyGearConstraint::finalizeMultiDof()
{
-
allocateJacobiansMultiDof();
-
+
m_numDofsFinalized = m_jacSizeBoth;
}
@@ -42,7 +40,6 @@ btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
{
}
-
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
@@ -81,27 +78,25 @@ int btMultiBodyGearConstraint::getIslandIdB() const
return -1;
}
-
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
- btMultiBodyJacobianData& data,
- const btContactSolverInfo& infoGlobal)
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal)
{
- // only positions need to be updated -- data.m_jacobians and force
- // directions were set in the ctor and never change.
-
+ // only positions need to be updated -- data.m_jacobians and force
+ // directions were set in the ctor and never change.
+
if (m_numDofsFinalized != m_jacSizeBoth)
{
- finalizeMultiDof();
+ finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
-
- if (m_maxAppliedImpulse==0.f)
+ if (m_maxAppliedImpulse == 0.f)
return;
-
+
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
@@ -114,67 +109,66 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
-
+
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
- for (int row=0;row<numRows;row++)
+ for (int row = 0; row < numRows; row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-
- int dof = 0;
- btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
- btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+ 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)
+
+ if (m_gearAuxLink >= 0)
{
auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
}
currentVelocity += auxVel;
- if (m_erp!=0)
+ if (m_erp != 0)
{
btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
if (m_gearAuxLink >= 0)
{
currentPositionA -= m_bodyA->getJointPosMultiDof(m_gearAuxLink)[dof];
}
- btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof];
- btScalar diff = currentPositionB+currentPositionA;
+ btScalar currentPositionB = m_gearRatio * m_bodyA->getJointPosMultiDof(m_linkB)[dof];
+ btScalar diff = currentPositionB + currentPositionA;
btScalar desiredPositionDiff = this->m_relativePositionTarget;
- posError = -m_erp*(desiredPositionDiff - diff);
+ posError = -m_erp * (desiredPositionDiff - diff);
}
-
- btScalar desiredRelativeVelocity = auxVel;
-
- fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
+
+ btScalar desiredRelativeVelocity = auxVel;
+
+ fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, desiredRelativeVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
- btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
+ btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
- btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
- constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
- constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
-
+ btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
+ constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
+ constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
+
break;
}
case btMultibodyLink::ePrismatic:
{
- btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
- constraintRow.m_contactNormal1=prismaticAxisInWorld;
- constraintRow.m_contactNormal2=-prismaticAxisInWorld;
+ btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
+ constraintRow.m_contactNormal1 = prismaticAxisInWorld;
+ constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
- constraintRow.m_relpos2CrossNormal.setZero();
+ constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
@@ -182,10 +176,6 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
btAssert(0);
}
};
-
}
-
}
-
}
-
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-11 23:54:16 +0000