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);
}
예제 #6
0
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
{
	btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
	btNormalizeAnglePositive(accAngle)));
	return result;
}