Define the rigid bodies const rigidBodies id A mass 1 velocity x 0 y 0 position x 0 y 0 id B mass 1 velocity x 0 y 0 position x 1 y 0 id C mass 1 velocity x 0 y 0 position x 2 y 1 Define the contacts between objects const contacts bodies A B Contact between body A and B normal x 1 y 0 Normal direction from A to B penetrationDepth 0 1 Penetration depth between A and B bodies B C Contact between body B and C normal x 0 y 1 Normal direction from B to C penetrationDepth 0 05 Penetration depth between B and C Function to find a body by its ID function findBody id return rigidBodies find body body id id Function to build the M matrix based on the contacts returns full matrix function buildMMatrix contacts const size contacts length const M new Array size size fill 0 Create a size x size zero matrix for let i 0 i contacts length i const bodies contacts i const bodyA findBody bodies 0 const bodyB findBody bodies 1 Mass contribution to diagonal elements based on inverse masses const invMassA 1 bodyA mass const invMassB 1 bodyB mass Diagonal elements M i size i invMassA invMassB return M Function to build the q vector based on the contacts and rigid bodies function buildQVector contacts const q for let i 0 i contacts length i const bodies normal penetrationDepth contacts i const bodyA findBody bodies 0 const bodyB findBody bodies 1 Calculate the relative velocity in the direction of the normal const relativeVelocity x bodyB velocity x bodyA velocity x y bodyB velocity y bodyA velocity y Normal velocity dot product of relative velocity and contact normal const normalVelocity relativeVelocity x normal x relativeVelocity y normal y Bias due to penetration depth const bias penetrationDepth q_i relative normal velocity bias q push normalVelocity bias return q function lcpSolve M q numIterations 100 const n q length const x new Array n fill 0 Start with x 0 for let iter 0 iter numIterations iter for let i 0 i n i let sum 0 for let j 0 j n j if i j sum M i n j x j Solve for x i with the current guess x i Math max 0 q i sum M i n i return x Build M and q matrices based on the rigid body data and contacts const M buildMMatrix contacts const q buildQVector contacts Solve the LCP using the built M and q const solution lcpSolve M q 5 Output the solution console log M M console log q q console log Solution x solution M 2 1 1 2 q 0 1 0 05 Solution x 0 05 0 025 Apply the impulses to the velocities of the bodies function applyImpulses solution contacts for let i 0 i contacts length i const bodies normal contacts i const impulse solution i const bodyA findBody bodies 0 const bodyB findBody bodies 1 Update velocities based on the impulse and mass bodyA velocity x impulse normal x bodyA mass bodyA velocity y impulse normal y bodyA mass bodyB velocity x impulse normal x bodyB mass bodyB velocity y impulse normal y bodyB mass Apply the impulses calculated from the LCP solution applyImpulses solution contacts Output the updated velocities console log Updated velocities of bodies rigidBodies This is the output Solution x 0 05 0 025 Updated velocities of bodies id A mass 1 velocity x 0 05 y 0 position x 0 y 0 id B mass 1 velocity x 0 05 y 0 025 position x 1 y 0 id C mass 1 velocity x 0 y 0 025 position x 2 y 1
normal y Bias due to penetration depth const bias penetrationDepth q_i relative normal velocity bias q push normalVelocity bias return q function lcpSolve M q numIterations 100 const n q length const x new Array n fill 0 Start with x 0 for let iter 0 iter numIterations iter for let i 0 i n i let sum 0 for let j 0 j n j if i j sum M i n j x j Solve for x i with the current guess x i Math max 0 q i sum M i n i return x Build M and q matrices based on the rigid body data and contacts const M buildMMatrix contacts const q buildQVector contacts Solve the LCP using the built M and q const solution lcpSolve M q 5 Output the solution console log M M console log q q console log Solution x solution M 2 1 1 2 q 0 1 0 05 Solution x 0 05 0 025 Apply the impulses to the velocities of the bodies function applyImpulses solution contacts for let i 0 i contacts length i const bodies normal contacts i const impulse solution i const bodyA findBody bodies 0 const bodyB findBody bodies 1 Update velocities based on the impulse and mass bodyA velocity x impulse normal x bodyA mass bodyA velocity y impulse normal y bodyA mass bodyB velocity x impulse normal x bodyB mass bodyB velocity y impulse normal y bodyB mass Apply the impulses calculated from the LCP solution applyImpulses solution contacts Output the updated velocities console log Updated velocities of bodies rigidBodies This is the output Solution x 0 05 0 025 Updated velocities of bodies id A mass 1 velocity x 0 05 y 0 position x 0 y 0 id B mass 1 velocity x 0 05 y 0 025 position x 1 y 0 id C mass 1 velocity x 0 y 0 025 position x 2 y 1