void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
void Generic6DofConstraintController::setConstraint(btGeneric6DofConstraint * ct)
{
this->constraint = ct;
// Local transform
btScalar eulerYaw,eulerRoll,eulerPitch;
this->constraint->getFrameOffsetA().getBasis().getEulerZYX(eulerYaw,eulerRoll,eulerPitch);
this->ui->leEulerRotX->setText(QString().setNum(eulerPitch));
this->ui->leEulerRotY->setText(QString().setNum(eulerRoll));
this->ui->leEulerRotZ->setText(QString().setNum(eulerYaw));
// Angular Limit Motors
btRotationalLimitMotor *motor;
for(int i=0;i<3;i++)
{
motor = this->constraint->getRotationalLimitMotor(i);
switch(i)
{
case 0 :
this->ui->cbEnable_m1->setChecked(motor->m_enableMotor);
this->ui->leTargetVelocity_m1->setText(QString().setNum(motor->m_targetVelocity));
this->ui->leMaxMotorForce_m1->setText(QString().setNum(motor->m_maxMotorForce));
this->ui->leMaxLimitForce_m1->setText(QString().setNum(motor->m_maxLimitForce));
this->ui->leBounce_m1->setText(QString().setNum(motor->m_bounce));
this->ui->leDamping_m1->setText(QString().setNum(motor->m_bounce));
this->ui->leAngularLowerLimit_x->setText(QString().setNum(btNormalizeAngle(motor->m_loLimit)));
this->ui->leAngularUpperLimit_x->setText(QString().setNum(btNormalizeAngle(motor->m_hiLimit)));
break;
case 1 :
this->ui->cbEnable_m2->setChecked(motor->m_enableMotor);
this->ui->leTargetVelocity_m2->setText(QString().setNum(motor->m_targetVelocity));
this->ui->leMaxMotorForce_m2->setText(QString().setNum(motor->m_maxMotorForce));
this->ui->leMaxLimitForce_m2->setText(QString().setNum(motor->m_maxLimitForce));
this->ui->leBounce_m2->setText(QString().setNum(motor->m_bounce));
this->ui->leDamping_m2->setText(QString().setNum(motor->m_bounce));
this->ui->leAngularLowerLimit_y->setText(QString().setNum(btNormalizeAngle(motor->m_loLimit)));
this->ui->leAngularUpperLimit_y->setText(QString().setNum(btNormalizeAngle(motor->m_hiLimit)));
break;
case 2 :
this->ui->cbEnable_m3->setChecked(motor->m_enableMotor);
this->ui->leTargetVelocity_m3->setText(QString().setNum(motor->m_targetVelocity));
this->ui->leMaxMotorForce_m3->setText(QString().setNum(motor->m_maxMotorForce));
this->ui->leMaxLimitForce_m3->setText(QString().setNum(motor->m_maxLimitForce));
this->ui->leBounce_m3->setText(QString().setNum(motor->m_bounce));
this->ui->leDamping_m3->setText(QString().setNum(motor->m_bounce));
this->ui->leAngularLowerLimit_z->setText(QString().setNum(btNormalizeAngle(motor->m_loLimit)));
this->ui->leAngularUpperLimit_z->setText(QString().setNum(btNormalizeAngle(motor->m_hiLimit)));
break;
}
}
}
void btAngularLimit::fit(btScalar& angle) const
{
if (m_halfRange > 0.0f)
{
btScalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
angle = getHigh();
}
else
{
angle = getLow();
}
}
}
}
void btAngularLimit::test(const btScalar angle)
{
m_correction = 0.0f;
m_sign = 0.0f;
m_solveLimit = false;
if (m_halfRange >= 0.0f)
{
btScalar deviation = btNormalizeAngle(angle - m_center);
if (deviation < -m_halfRange)
{
m_solveLimit = true;
m_correction = - (deviation + m_halfRange);
m_sign = +1.0f;
}
else if (deviation > m_halfRange)
{
m_solveLimit = true;
m_correction = m_halfRange - deviation;
m_sign = -1.0f;
}
}
}
btScalar btAngularLimit::getHigh() const
{
return btNormalizeAngle(m_center + m_halfRange);
}
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
btNormalizeAnglePositive(accAngle)));
return result;
}
