/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso>
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
Vec2 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;
Vec2 Cdot1 = Pool.PopVec2();
Vec2 P = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
if (Frequency > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z);
m_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse1 = P;
Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
impulse1.NegateLocal();
m_impulse.X += impulse1.X;
m_impulse.Y += impulse1.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * Vec2.Cross(m_rA, P);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * Vec2.Cross(m_rB, P);
}
else
{
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mat33.MulToOutUnsafe(m_mass, Cdot, impulse);
impulse.NegateLocal();
m_impulse.AddLocal(impulse);
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z);
Pool.PushVec3(2);
}
data.Velocities[m_indexA].V.Set(vA);
data.Velocities[m_indexA].W = wA;
data.Velocities[m_indexB].V.Set(vB);
data.Velocities[m_indexB].W = wB;
Pool.PushVec2(3);
}
