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; } }