public float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref Vector3 pointInA,
RigidBody body2, ref Vector3 pointInB,
int limit_index,
ref Vector3 axis_normal_on_a,
ref Vector3 anchorPos)
{
///find relative velocity
// Vector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// Vector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
Vector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
Vector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
Vector3 vel1 = Vector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1);
Vector3 vel2 = Vector3.Zero;;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2);
Vector3 vel = vel1 - vel2;
float rel_vel = Vector3.Dot(axis_normal_on_a,vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -Vector3.Dot((pointInA - pointInB),axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = MathUtil.VectorComponent(ref m_lowerLimit,limit_index);
float maxLimit = MathUtil.VectorComponent(ref 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 = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index);
float sum = oldNormalImpulse + normalImpulse;
MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index,(sum > hi ? 0f: sum < lo ? 0f : sum));
normalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index) - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1,axis_normal_on_a);
Vector3 ftorqueAxis2 = Vector3.Cross(rel_pos2,axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a*body1.GetInvMass(), Vector3.TransformNormal(ftorqueAxis1,body1.GetInvInertiaTensorWorld()),normalImpulse);
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), Vector3.TransformNormal(ftorqueAxis2,body2.GetInvInertiaTensorWorld()), -normalImpulse);
return normalImpulse;
}
protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
//check magniture of applied impulse from SolverConstraint
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
float deltaVel1Dotn = Vector3.Dot(c.m_contactNormal, body1.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetDeltaAngularVelocity());
float deltaVel2Dotn = -Vector3.Dot(c.m_contactNormal, body2.InternalGetDeltaLinearVelocity()) + Vector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
Vector3 temp = c.m_contactNormal * body1.InternalGetInvMass();
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse);
temp = -c.m_contactNormal * body2.InternalGetInvMass();
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse);
}
//! apply the correction impulses for two bodies
public float SolveAngularLimits(float timeStep,ref Vector3 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
Vector3 angVelA = Vector3.Zero;
body0.InternalGetAngularVelocity(ref angVelA);
Vector3 angVelB = Vector3.Zero;
body1.InternalGetAngularVelocity(ref angVelB);
Vector3 vel_diff = angVelA-angVelB;
float rel_vel = Vector3.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;
Vector3 motorImp = clippedMotorImpulse * axis;
//body0.applyTorqueImpulse(motorImp);
//body1.applyTorqueImpulse(-motorImp);
body0.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body0.GetInvInertiaTensorWorld()),clippedMotorImpulse);
body1.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body1.GetInvInertiaTensorWorld()),-clippedMotorImpulse);
return clippedMotorImpulse;
}
protected void SetFrictionConstraintImpulse(ref SolverConstraint solverConstraint, RigidBody rb0, RigidBody rb1,
ManifoldPoint cp, ContactSolverInfo infoGlobal)
{
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_FRICTION_WARMSTARTING))
{
{
SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0 != null)
{
rb0.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb0.GetInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
}
if (rb1 != null)
{
rb1.InternalApplyImpulse(frictionConstraint1.m_contactNormal * rb1.GetInvMass(), -frictionConstraint1.m_angularComponentB, -frictionConstraint1.m_appliedImpulse);
}
}
else
{
frictionConstraint1.m_appliedImpulse = 0f;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;
}
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_WARMSTARTING))
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0 != null)
{
rb0.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb0.GetInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse);
}
if (rb1 != null)
{
rb1.InternalApplyImpulse(frictionConstraint2.m_contactNormal * rb1.GetInvMass(), -frictionConstraint2.m_angularComponentB, -frictionConstraint2.m_appliedImpulse);
}
}
else
{
frictionConstraint2.m_appliedImpulse = 0f;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
}
}
else
{
SolverConstraint frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
frictionConstraint1.m_appliedImpulse = 0f;
if (TestSolverMode(infoGlobal.m_solverMode, SolverMode.SOLVER_USE_2_FRICTION_DIRECTIONS))
{
SolverConstraint frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
frictionConstraint2.m_appliedImpulse = 0f;
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1] = frictionConstraint2;
}
m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex] = frictionConstraint1;
}
}