internal void Solve(Manifold manifold)
{
VoltBody bodyA = manifold.ShapeA.Body;
VoltBody bodyB = manifold.ShapeB.Body;
Fix64 elasticity = bodyA.World.Elasticity;
// Calculate relative bias velocity
VoltVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft);
VoltVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft);
Fix64 vbn = VoltVector2.Dot((vb1 - vb2), this.normal);
// Calculate and clamp the bias impulse
Fix64 jbn = this.nMass * (vbn - this.bias);
jbn = VoltMath.Max(-this.jBias, jbn);
this.jBias += jbn;
// Apply the bias impulse
this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn);
// Calculate relative velocity
VoltVector2 vr = this.RelativeVelocity(bodyA, bodyB);
Fix64 vrn = VoltVector2.Dot(vr, this.normal);
// Calculate and clamp the normal impulse
Fix64 jn = nMass * (vrn + (this.restitution * elasticity));
jn = VoltMath.Max(-this.cachedNormalImpulse, jn);
this.cachedNormalImpulse += jn;
// Calculate the relative tangent velocity
Fix64 vrt = VoltVector2.Dot(vr, this.normal.Left());
// Calculate and clamp the friction impulse
Fix64 jtMax = manifold.Friction * this.cachedNormalImpulse;
Fix64 jt = vrt * tMass;
Fix64 result = VoltMath.Clamp(this.cachedTangentImpulse + jt, -jtMax, jtMax);
jt = result - this.cachedTangentImpulse;
this.cachedTangentImpulse = result;
// Apply the normal and tangent impulse
this.ApplyContactImpulse(bodyA, bodyB, jn, jt);
}
