public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; // Cdot = v + cross(w, r) b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB); b2Vec2 impulse = b2Math.b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse)); b2Vec2 oldImpulse = m_impulse; m_impulse += impulse; float maxImpulse = data.step.dt * m_maxForce; if (m_impulse.LengthSquared() > maxImpulse * maxImpulse) { m_impulse *= maxImpulse / m_impulse.Length(); } impulse = m_impulse - oldImpulse; vB += m_invMassB * impulse; wB += m_invIB * b2Math.b2Cross(m_rB, impulse); data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
public override void SolveVelocityConstraints(b2SolverData data) { b2Vec2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; b2Vec2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; float h = data.step.dt; // Solve angular friction { float Cdot = wB - wA; float impulse = -m_angularMass * Cdot; float oldImpulse = m_angularImpulse; float maxImpulse = h * m_maxTorque; m_angularImpulse = b2Math.b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = m_angularImpulse - oldImpulse; wA -= iA * impulse; wB += iB * impulse; } // Solve linear friction { b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB) - vA - b2Math.b2Cross(wA, m_rA); b2Vec2 impulse = -b2Math.b2Mul(m_linearMass, Cdot); b2Vec2 oldImpulse = m_linearImpulse; m_linearImpulse += impulse; float maxImpulse = h * m_maxForce; if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse) { m_linearImpulse.Normalize(); m_linearImpulse *= maxImpulse; } impulse = m_linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * b2Math.b2Cross(m_rA, impulse); vB += mB * impulse; wB += iB * b2Math.b2Cross(m_rB, impulse); } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }