//! apply the correction impulses for two bodies
public float SolveAngularLimits(float timeStep, ref IndexedVector3 axis, float jacDiagABInv, RigidBody body0, RigidBody body1)
{
if (NeedApplyTorques() == false)
{
return 0.0f;
}
float target_velocity = m_targetVelocity;
float 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
IndexedVector3 angVelA = IndexedVector3.Zero;
body0.InternalGetAngularVelocity(ref angVelA);
IndexedVector3 angVelB = IndexedVector3.Zero;
body1.InternalGetAngularVelocity(ref angVelB);
IndexedVector3 vel_diff = angVelA - angVelB;
float rel_vel = IndexedVector3.Dot(axis, vel_diff);
// correction velocity
float motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
if (motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON)
{
return 0.0f;//no need for applying force
}
// correction impulse
float unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
// clip correction impulse
float 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
float lo = float.MinValue;
float hi = float.MaxValue;
float oldaccumImpulse = m_accumulatedImpulse;
float sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
IndexedVector3 motorImp = clippedMotorImpulse * axis;
//body0.applyTorqueImpulse(motorImp);
//body1.applyTorqueImpulse(-motorImp);
body0.InternalApplyImpulse(IndexedVector3.Zero, body0.GetInvInertiaTensorWorld() * axis, clippedMotorImpulse, "Generic6DoF body0");
body1.InternalApplyImpulse(IndexedVector3.Zero, body1.GetInvInertiaTensorWorld() * axis, -clippedMotorImpulse, "Generic6DoF body1");
return clippedMotorImpulse;
}
public float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref IndexedVector3 pointInA,
RigidBody body2, ref IndexedVector3 pointInB,
int limit_index,
ref IndexedVector3 axis_normal_on_a,
ref IndexedVector3 anchorPos)
{
///find relative velocity
// IndexedVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// IndexedVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
IndexedVector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
IndexedVector3 vel1 = IndexedVector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1, ref vel1);
IndexedVector3 vel2 = IndexedVector3.Zero; ;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2, ref vel2);
IndexedVector3 vel = vel1 - vel2;
float rel_vel = IndexedVector3.Dot(axis_normal_on_a, vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -IndexedVector3.Dot((pointInA - pointInB), axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = m_lowerLimit[limit_index];
float maxLimit = m_upperLimit[limit_index];
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = 0f;
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = 0f;
}
else
{
return 0.0f;
}
}
}
}
float normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
float oldNormalImpulse = m_accumulatedImpulse[limit_index];
float sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = (sum > hi ? 0f : sum < lo ? 0f : sum);
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
IndexedVector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, axis_normal_on_a);
IndexedVector3 ftorqueAxis2 = IndexedVector3.Cross(rel_pos2, axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a * body1.GetInvMass(), body1.GetInvInertiaTensorWorld() * ftorqueAxis1, normalImpulse, "Generic6DoF body1");
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), body2.GetInvInertiaTensorWorld() * ftorqueAxis2, -normalImpulse, "Generic6DoF body2");
return normalImpulse;
}
