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