void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
{
// Compute limit information
m_hingeAngle = getHingeAngle(transA,transB);
m_correction = btScalar(0.);
m_limitSign = btScalar(0.);
m_solveLimit = false;
if (m_lowerLimit <= m_upperLimit)
{
m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
if (m_hingeAngle <= m_lowerLimit)
{
m_correction = (m_lowerLimit - m_hingeAngle);
m_limitSign = 1.0f;
m_solveLimit = true;
}
else if (m_hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - m_hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
}
return;
}
void btSliderConstraint::testAngLimits(void)
{
m_angDepth = btScalar(0.);
m_solveAngLim = false;
if(m_lowerAngLimit <= m_upperAngLimit)
{
const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
// btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
m_angPos = rot;
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
m_solveAngLim = true;
}
else if(rot > m_upperAngLimit)
{
m_angDepth = rot - m_upperAngLimit;
m_solveAngLim = true;
}
}
}
void btGeneric6DofSpring2Constraint::testAngularLimitMotor(int axis_index)
{
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
m_angularLimits[axis_index].m_currentPosition = angle;
m_angularLimits[axis_index].testLimitValue(angle);
}
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
{
btScalar angle = m_calculatedAxisAngleDiff[axis_index];
angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
m_angularLimits[axis_index].m_currentPosition = angle;
//test limits
m_angularLimits[axis_index].testLimitValue(angle);
return m_angularLimits[axis_index].needApplyTorques();
}
