int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
{
btGeneric6DofConstraint * d6constraint = this;
int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
{
if(m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = btScalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_ERP = m_linearLimits.m_restitution;
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = btScalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
}
}
return row;
}
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
// int row = 0;
//solve linear limits
btRotationalLimitMotor limot;
for (int i=0;i<3 ;i++ )
{
if(m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = btScalar(0.f);
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = btScalar(0.f);
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
if(m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
int rotAllowed = 1; // rotations around orthos to current axis
if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
{
rotAllowed = 0;
}
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
}
else
{
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
}
}
}
int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
{
//solve linear limits
btRotationalLimitMotor2 limot;
for (int i=0;i<3 ;i++ )
{
if(m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
{ // re-use rotational motor code
limot.m_bounce = m_linearLimits.m_bounce[i];
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
limot.m_springDamping = m_linearLimits.m_springDamping[i];
limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
//rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
m_angularLimits[indx1].m_currentLimit == 2 ||
( m_angularLimits[indx1].m_currentLimit == 3 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx1].m_currentLimit == 4 && ( m_angularLimits[indx1].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx1].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
m_angularLimits[indx2].m_currentLimit == 2 ||
( m_angularLimits[indx2].m_currentLimit == 3 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitError > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) ) ||
( m_angularLimits[indx2].m_currentLimit == 4 && ( m_angularLimits[indx2].m_currentLimitError < -D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION || m_angularLimits[indx2].m_currentLimitErrorHi > D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION ) );
if( indx1Violated && indx2Violated )
{
rotAllowed = 0;
}
row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
}
}
int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
{
btGeneric6DofConstraint * d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i=0;i<3 ;i++ )
{
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
row += get_limit_motor_info2(
d6constraint->getRotationalLimitMotor(i),
&m_rbA,
&m_rbB,
info,row,axis,1);
}
}
return row;
}
