void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
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.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
}
}
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
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.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
if (m_bodyA->isMultiDof())
{
//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));
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;
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;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}
void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
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.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
// row 0: the lower bound
setPosition(0, m_bodyA->getJointPos(m_linkA) - m_lowerBound); //multidof: this is joint-type dependent
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btScalar penetration = getPosition(row);
//todo: consider adding some safety threshold here
if (penetration>0)
{
continue;
}
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
const btScalar posError = 0; //why assume it's zero?
const btVector3 dummy(0, 0, 0);
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
{
//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));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = direction*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 = direction* 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();
break;
}
default:
{
btAssert(0);
}
};
}
{
btScalar positionalError = 0.f;
btScalar velocityError = - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
erp = infoGlobal.m_erp;
}
if (penetration>0)
{
positionalError = 0;
velocityError = -penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
btScalar penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
//combine position and velocity into rhs
constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
constraintRow.m_rhsPenetration = 0.f;
} else
{
//split position and velocity into rhs and m_rhsPenetration
constraintRow.m_rhs = velocityImpulse;
constraintRow.m_rhsPenetration = penetrationImpulse;
}
}
}
}
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
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.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
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;
unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
// row 0: the lower bound
jacobianA(0)[offsetA] = 1;
jacobianB(0)[offsetB] = m_gearRatio;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar erp = infoGlobal.m_erp;
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
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];
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 = 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));
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;
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;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}
