internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Vector2 vA = bA._linearVelocity;
float wA = bA._angularVelocity;
Vector2 vB = bB._linearVelocity;
float wB = bB._angularVelocity;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA._linearVelocity = vA;
bA._angularVelocity = wA;
bB._linearVelocity = vB;
bB._angularVelocity = wB;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Mat22 K1 = new Mat22();
K1.col1.x = mA + mB; K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = mA + mB;
Mat22 K2 = new Mat22();
K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
Mat22 K3 = new Mat22();
K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.GetInverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}
