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