public override void InitVelocityConstraints(b2SolverData data) { m_indexA = m_bodyA.IslandIndex; m_indexB = m_bodyB.IslandIndex; m_localCenterA = m_bodyA.Sweep.localCenter; m_localCenterB = m_bodyB.Sweep.localCenter; m_invMassA = m_bodyA.InvertedMass; m_invMassB = m_bodyB.InvertedMass; m_invIA = m_bodyA.InvertedI; m_invIB = m_bodyB.InvertedI; float aA = m_bodyA.InternalPosition.a; b2Vec2 vA = m_bodyA.InternalVelocity.v; float wA = m_bodyA.InternalVelocity.w; float aB = m_bodyB.InternalPosition.a; b2Vec2 vB = m_bodyB.InternalVelocity.v; float wB = m_bodyB.InternalVelocity.w; b2Rot qA = new b2Rot(aA); b2Rot qB = new b2Rot(aB); // Compute the effective mass matrix. m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA); m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); // 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 = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; b2Mat22 K = new b2Mat22(); K.exx = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y; K.exy = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y; K.eyx = K.ex.y; K.eyy = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x; m_linearMass = K.GetInverse(); m_angularMass = iA + iB; if (m_angularMass > 0.0f) { m_angularMass = 1.0f / m_angularMass; } if (data.step.warmStarting) { // Scale impulses to support a variable time step. m_linearImpulse *= data.step.dtRatio; m_angularImpulse *= data.step.dtRatio; b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y); vA -= mA * P; wA -= iA * (b2Math.b2Cross(m_rA, P) + m_angularImpulse); vB += mB * P; wB += iB * (b2Math.b2Cross(m_rB, P) + m_angularImpulse); } else { m_linearImpulse.SetZero(); m_angularImpulse = 0.0f; } m_bodyA.InternalVelocity.v = vA; m_bodyA.InternalVelocity.w = wA; m_bodyB.InternalVelocity.v = vB; m_bodyB.InternalVelocity.w = wB; }
public override void InitVelocityConstraints(b2SolverData data) { m_indexB = m_bodyB.IslandIndex; m_localCenterB = m_bodyB.Sweep.localCenter; m_invMassB = m_bodyB.InvertedMass; m_invIB = m_bodyB.InvertedI; b2Vec2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; b2Rot qB = new b2Rot(aB); float mass = m_bodyB.Mass; // Frequency float omega = 2.0f * b2Settings.b2_pi * m_frequencyHz; // Damping coefficient float d = 2.0f * mass * m_dampingRatio * omega; // Spring stiffness float k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. float h = data.step.dt; Debug.Assert(d + h * k > b2Settings.b2_epsilon); m_gamma = h * (d + h * k); if (m_gamma != 0.0f) { m_gamma = 1.0f / m_gamma; } m_beta = h * k * m_gamma; // Compute the effective mass matrix. m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB); // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] b2Mat22 K = new b2Mat22(); K.exx = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma; K.exy = -m_invIB * m_rB.x * m_rB.y; K.eyx = K.ex.y; K.eyy = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma; m_mass = K.GetInverse(); m_C = cB + m_rB - m_targetA; m_C *= m_beta; // Cheat with some damping wB *= 0.98f; if (data.step.warmStarting) { m_impulse *= data.step.dtRatio; vB += m_invMassB * m_impulse; wB += m_invIB * b2Math.b2Cross(m_rB, m_impulse); } else { m_impulse.SetZero(); } data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }