btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btRigidBody * body1 )
{
if (needApplyTorques()==false) return 0.0f;
btScalar target_velocity = m_targetVelocity;
btScalar maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA;
body0->internalGetAngularVelocity(angVelA);
btVector3 angVelB;
body1->internalGetAngularVelocity(angVelB);
btVector3 vel_diff;
vel_diff = angVelA-angVelB;
btScalar rel_vel = axis.dot(vel_diff);
// correction velocity
btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
btScalar clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse>0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
}
// sort with accumulated impulses
btScalar lo = btScalar(-BT_LARGE_FLOAT);
btScalar hi = btScalar(BT_LARGE_FLOAT);
btScalar oldaccumImpulse = m_accumulatedImpulse;
btScalar sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp = clippedMotorImpulse * axis;
//body0->applyTorqueImpulse(motorImp);
//body1->applyTorqueImpulse(-motorImp);
body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
return clippedMotorImpulse;
}
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
real_t timeStep,Vector3& axis,real_t jacDiagABInv,
BodySW * body0, BodySW * body1)
{
if (needApplyTorques()==false) return 0.0f;
real_t target_velocity = m_targetVelocity;
real_t maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
Vector3 vel_diff = body0->get_angular_velocity();
if (body1)
{
vel_diff -= body1->get_angular_velocity();
}
real_t rel_vel = axis.dot(vel_diff);
// correction velocity
real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
real_t clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse>0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
}
// sort with accumulated impulses
real_t lo = real_t(-1e30);
real_t hi = real_t(1e30);
real_t oldaccumImpulse = m_accumulatedImpulse;
real_t sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
Vector3 motorImp = clippedMotorImpulse * axis;
body0->apply_torque_impulse(motorImp);
if (body1) body1->apply_torque_impulse(-motorImp);
return clippedMotorImpulse;
}
