Ejemplo n.º 1
0
dgFloat32 dgHingeConstraint::CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const
{
	dgFloat32 alpha;
	dgFloat32 omega;
	dgFloat32 penetrationErr;

	alpha = dgFloat32 (0.0f);
	if (m_angle > angle) {
		omega = GetJointOmega ();
		if (omega < dgFloat32 (0.0f)) {
			omega = dgFloat32 (0.0f);
		}
		penetrationErr = (angle - m_angle); 
		alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep;

	} else if (m_angle < angle) {
		omega = GetJointOmega ();
		if (omega > dgFloat32 (0.0f)) {
			omega = dgFloat32 (0.0f);
		}

		penetrationErr = MIN_JOINT_PIN_LENGTH * (angle - m_angle); 
		alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep;
	} 
	return alpha;
}
void dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
	if (m_actuatorFlag) {
		dFloat jointangle = GetActuatorAngle();
		dFloat relAngle = jointangle - m_angle;
		dFloat currentSpeed = GetJointOmega();
		dFloat step = dFloat(2.0f) * m_angularRate * timestep;
		dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
		dFloat accel = (desiredSpeed - currentSpeed) / timestep;
		NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
		NewtonUserJointSetRowAcceleration(m_joint, accel);
        NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
        NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxForce);
		NewtonUserJointSetRowStiffness (m_joint, 1.0f);
	} else {
		dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
	}
}