Simple 2D Rigid Body Solver in JS

2d_physics.js

// https://ricefields.me/2024/10/11/intro-to-physics-simulation.html

const canvas = document.getElementById('render_canvas');
const ctx = canvas.getContext('2d');
function resizeCanvas() {
  canvas.width = window.innerWidth;
  canvas.height = window.innerHeight;
}
resizeCanvas();
window.addEventListener('resize', resizeCanvas);

class Broadphase {
  constructor(grid_size, cell_size) {
    this.grid_size = grid_size;
    this.cell_size = cell_size;

    this.max_cell = [
      Math.floor(grid_size[0] / cell_size[0]),
      Math.floor(grid_size[1] / cell_size[1]),
    ];

    // use hash map with string key instead
    this.cells = Array.from({ length: this.max_cell[0] + 1 }, () =>
      Array.from({ length: this.max_cell[1] + 1 }, () => new Set()),
    );

    // keep track of bodies that were moved
    this.move_set = new Set();
  }

  insert(body) {
    const [top_index, bottom_index] = this.toIndices(body.pos, [body.radius, body.radius]);

    body.broadphase = [top_index, bottom_index];

    // insert body into all cells from top_index to bottom_index
    for (let i = top_index[0]; i <= bottom_index[0]; ++i) {
      for (let j = top_index[1]; j <= bottom_index[1]; ++j) {
        if (this.cells[i][j]) {
          this.cells[i][j].add(body);
        } else {
          this.cells[i][j] = new Set([body]);
        }
      }
    }

    this.move_set.add(body);
  }

  remove(body) {
    if (body.broadphase.length) {
      const top_index = body.broadphase[0];
      const bottom_index = body.broadphase[1];

      // remove body from top_index to bottom_index
      for (let i = top_index[0]; i <= bottom_index[0]; ++i) {
        for (let j = top_index[1]; j <= bottom_index[1]; ++j) {
          this.cells[i][j].delete(body);
        }
      }

      body.broadphase = [];
    }
  }

  update(body) {
    this.remove(body);
    this.insert(body);
  }

  // generate a set of possibly colliding bodies
  computePairs() {
    const pairs = new Set();

    const bodies = Array.from(this.move_set);
    for (let a = 0; a < bodies.length; a++) {
      for (let b = a + 1; b < bodies.length; b++) {
        const body_a = bodies[a];
        const body_b = bodies[b];
        // cannot collide with self
        if (body_a.id === body_b.id) continue;

        // check if cells are intersecting
        const is_overlapping =
          body_a.broadphase[0][0] <= body_b.broadphase[1][0] && // A's min x <= B's max x
          body_a.broadphase[1][0] >= body_b.broadphase[0][0] && // A's max x >= B's min x
          body_a.broadphase[0][1] <= body_b.broadphase[1][1] && // A's min y <= B's max y
          body_a.broadphase[1][1] >= body_b.broadphase[0][1]; // A's max y >= B's min y
        if (is_overlapping) {
          const pair_key =
            body_a.id < body_b.id ? `${body_a.id},${body_b.id}` : `${body_b.id},${body_a.id}`;
          pairs.add(pair_key);
        }
      }
    }

    this.move_set.clear();
    return pairs;
  }

  queryPoint(point) {
    const bodies = new Set();
    const [top_index, bottom_index] = this.toIndices(point, this.cell_size);

    // insert body into all cells from top_index to bottom_index
    for (let i = top_index[0]; i <= bottom_index[0]; ++i) {
      for (let j = top_index[1]; j <= bottom_index[1]; ++j) {
        const cell = this.cells[i][j];
        if (cell) {
          cell.forEach((b) => bodies.add(b));
        }
      }
    }

    return bodies;
  }

  toIndices(center, half_size) {
    const cell_top = [
      Math.max(0, Math.floor((center[0] - half_size[0]) / this.cell_size[0])),
      Math.max(0, Math.floor((center[1] - half_size[1]) / this.cell_size[1])),
    ];

    const cell_bottom = [
      Math.min(this.max_cell[0], Math.floor((center[0] + half_size[0]) / this.cell_size[0])),
      Math.min(this.max_cell[1], Math.floor((center[1] + half_size[1]) / this.cell_size[1])),
    ];

    return [cell_top, cell_bottom];
  }

  draw() {
    const [grid_width, grid_height] = this.grid_size;
    const [cell_width, cell_height] = this.cell_size;

    ctx.strokeStyle = '#000';
    ctx.lineWidth = 1;

    for (let x = 0; x <= grid_width; x += cell_width) {
      ctx.beginPath();
      ctx.moveTo(x, 0);
      ctx.lineTo(x, grid_height);
      ctx.stroke();
    }

    for (let y = 0; y <= grid_height; y += cell_height) {
      ctx.beginPath();
      ctx.moveTo(0, y);
      ctx.lineTo(grid_width, y);
      ctx.stroke();
    }

    for (let i = 0; i < this.cells.length; i++) {
      for (let j = 0; j < this.cells[i].length; j++) {
        // draw the cell with body
        if (this.cells[i][j].size > 0) {
          ctx.fillStyle = 'rgba(240, 0, 0, 0.6)';
          ctx.fillRect(i * cell_width, j * cell_height, cell_width, cell_height);
        }
      }
    }
  }
}

class Simulation {
  constructor() {
    this.bodies = [];
    this.contacts = [];
    this.contact_constraints = [];
    this.broadphase = new Broadphase([1024, 1024], [16, 16]);
    this.ground_y = 200;
  }

  step(dt) {
    // possible collision pairs
    const pairs = this.broadphase.computePairs();
    this.contacts = [];
    for (const pair of pairs) {
      const pair_ids = pair.split(',').map((id) => +id);
      const body_a = this.bodies[pair_ids[0]];
      const body_b = this.bodies[pair_ids[1]];

      const contact = circleContact(body_a, body_b);
      if (contact) {
        this.contacts.push({ body_a, body_b, contact });
      }
    }

    // slove forces
    for (const body of this.bodies) {
      // v = u  + f/m * t;
      body.linear_velocity[0] += (body.force[0] / body.mass) * dt;
      body.linear_velocity[1] += (body.force[1] / body.mass) * dt;

      // w = wu + to/I * t;
      body.angular_velocity += (body.torque / body.mmI) * dt;

      // zero out forces
      body.force = [0, 0];
      body.torque = 0;
    }

    // init contact constraints
    this.contact_constraints = [];
    for (const { contact, body_a, body_b } of this.contacts) {
      const c1 = sub(contact.point, body_a.pos); // contact to body centroid
      const c2 = sub(contact.point, body_b.pos);
      const c1n = cross(c1, contact.normal);
      const c2n = cross(c2, contact.normal);
      const eff_mass =
        1.0 / body_a.mass + c1n * (1.0 / body_a.mmI) * c1n + 1.0 / body_b.mass + c2n * (1.0 / body_b.mmI) * c2n;
      const bias = (-0.2 * contact.penetration) / dt; // 0.3 for smoothing bias

      // friction constraint
      const t = [contact.normal[1], contact.normal[0]];
      const c1t = cross(c1, t);
      const c2t = cross(c2, t);
      const f_eff_mass =
        1.0 / body_a.mass + c1t * (1.0 / body_a.mmI) * c1t + 1.0 / body_b.mass + c2t * (1.0 / body_b.mmI) * c2t;
      const friction = Math.sqrt(body_a.friction * body_b.friction); // mix friction
      const constraint = {
        normal: {
          body_a,
          body_b,
          normal: contact.normal,
          inv_eff_mass: 1.0 / eff_mass,
          c1,
          c2,
          bias,
          total_l: 0, // total accumulated impulse
        },
        tangent: {
          f_inv_eff_mass: 1.0 / f_eff_mass,
          tangent: t,
          f_total_l: 0,
          friction: friction,
        },
      };
      this.contact_constraints.push(constraint);
    }

    // slove the constraint iteratively
    for (let i = 0; i < 10; ++i) {
      for (const constraint of this.contact_constraints) {
        const { c1, c2, body_a, body_b, bias, inv_eff_mass, normal, total_l } = constraint.normal;
        // solve contact
        {
          const V1 = add(body_a.linear_velocity, crossSV(c1, body_a.angular_velocity));
          const V2 = add(body_b.linear_velocity, crossSV(c2, body_b.angular_velocity));

          const rv = sub(V2, V1);
          const jv = dot(rv, normal);

          let l = -(jv + bias) * inv_eff_mass;

          // clamping impulse
          const old_l = total_l;
          constraint.normal.total_l = Math.max(0, old_l + l);
          l = constraint.normal.total_l - old_l;

          const impulse = scale(normal, l);

          // apply impulse
          body_a.linear_velocity = sub(body_a.linear_velocity, scale(impulse, 1.0 / body_a.mass));
          body_a.angular_velocity -= cross(c1, impulse) / body_a.mmI;

          body_b.linear_velocity = add(body_b.linear_velocity, scale(impulse, 1.0 / body_b.mass));
          body_b.angular_velocity += cross(c2, impulse) / body_b.mmI;
        }

        // solve friction
        {
          const { f_inv_eff_mass, tangent, f_total_l, friction } = constraint.tangent;
          const V1 = add(body_a.linear_velocity, crossSV(c1, body_a.angular_velocity));
          const V2 = add(body_b.linear_velocity, crossSV(c2, body_b.angular_velocity));

          const rv = sub(V2, V1);
          const jv = dot(rv, normal);

          let l = -jv * f_inv_eff_mass;

          // clamping impulse
          const max_l = friction * total_l;
          const old_l = f_total_l;
          constraint.tangent.f_total_l = old_l + l;
          constraint.tangent.f_total_l = clamp(constraint.tangent.f_total_l, -max_l, max_l);
          l = constraint.tangent.f_total_l - old_l;

          const impulse = scale(tangent, l);

          // apply impulse
          body_a.linear_velocity = sub(body_a.linear_velocity, scale(impulse, 1.0 / body_a.mass));
          body_a.angular_velocity -= cross(c1, impulse) / body_a.mmI;

          body_b.linear_velocity = add(body_b.linear_velocity, scale(impulse, 1.0 / body_b.mass));
          body_b.angular_velocity += cross(c2, impulse) / body_b.mmI;
        }
      }
    }

    // integrate position
    for (const body of this.bodies) {
      // s = s + v * t;
      body.pos[0] += body.linear_velocity[0] * dt;
      body.pos[1] += body.linear_velocity[1] * dt;

      // r = r + w * t;
      body.rot += body.angular_velocity * dt;
      // wrap angle around [0, 2*PI]
      if (body.rot < 0) body.rot += 2 * Math.PI;
      if (body.rot >= 2 * Math.PI) body.rot -= 2 * Math.PI;

      // enforce position constraint
      // this is a bit different form the article because the y coordinate is inverted in canvas2d
      if (body.pos[1] + body.radius >= this.ground_y) {
        const bias = body.pos[1] + body.radius - this.ground_y;
        body.pos[1] -= bias;
      }

      // update broadphase
      this.broadphase.update(body);
    }
  }

  newBody(pos, radius, density) {
    const body = createBody(this.bodies.length, pos, radius, density);
    this.bodies.push(body);
    this.broadphase.insert(body);
  }
}

function clamp(value, min, max) {
  return Math.min(Math.max(value, min), max);
}
function dot(a, b) {
  return a[0] * b[0] + a[1] * b[1];
}
function scale(a, s) {
  return [a[0] * s, a[1] * s];
}
function cross(a, b) {
  return a[0] * b[1] - a[1] * b[0];
}
function crossSV(a, s) {
  return [-a[1] * s, a[0] * s];
}
function add(a, b) {
  return [a[0] + b[0], a[1] + b[1]];
}
function sub(a, b) {
  return [a[0] - b[0], a[1] - b[1]];
}
function mul(a, b) {
  return [a[0] * b[0], a[1] * b[1]];
}

// compute contact between two cirls
function circleContact(c1, c2) {
  const sub = [c2.pos[0] - c1.pos[0], c2.pos[1] - c1.pos[1]];
  const distance = Math.sqrt(sub[0] ** 2 + sub[1] ** 2);
  if (distance > c1.radius + c2.radius) return null;

  const normal = [sub[0] / distance, sub[1] / distance];
  const point = [c1.pos[0] + c1.radius * normal[0], c1.pos[1] + c1.radius * normal[1]];
  const penetration = c1.radius + c2.radius - distance;

  return { normal, point, penetration };
}

// create a new physics body
function createBody(id, pos, radius, density, friction = 0.2) {
  const area = Math.PI * radius * radius;
  const mass = density * area;
  // mass moment of inertia
  const mmI = mass * radius * radius;
  return {
    id: id,
    pos: pos,
    rot: 0,
    mass: mass,
    mmI: mmI,
    radius: radius,
    force: [0, 0],
    torque: 0,
    linear_velocity: [0, 0],
    angular_velocity: 0,
    friction,
    broadphase: [], // broadphase indices
  };
}

// applic force at center of mass
function applyForce(body, acc) {
  // f = ma;
  body.force[0] += body.mass * acc[0];
  body.force[1] += body.mass * acc[1];
}

// apply force at arbitrary point, which will result in some angular motion
function applyForceAt(body, acc, point) {
  body.force[0] += body.mass * acc[0];
  body.force[1] += body.mass * acc[1];

  // point - centroid
  const p = sub(point, body.pos);
  // 2D cross product
  body.torque += cross(p, [acc[0] * body.mass, acc[1] * body.mass]);
}

const simulation = new Simulation();
function reset() {
  simulation.bodies = [];
  simulation.broadphase = new Broadphase([1024, 1024], [16, 16]);
  simulation.newBody([200, 100], 20, 10, 1.0);
  simulation.newBody([140, 100], 20, 10, 0.5);
}

// Animation loop
let last_time = 0;
function animate(time) {
  requestAnimationFrame(animate);
	
  // calculate frame time
	const diff = time - last_time;
  last_time = time;
  if(diff < Number.EPSILON || diff > 100) { // handle suspend
    return;
  }
  
	const dt = diff / 1000;
  simulation.step(dt);

  if (simulation.bodies.length) {
    // apply downward force
    applyForce(simulation.bodies[0], [0.0, 25.0]);
    
    // apply force to body 1
    const body1 = simulation.bodies[1];
    const local_point = [-20, -20]; // [-20, -20] units from the center in local space
    const world_point = [body1.pos[0] + local_point[0], body1.pos[1] + local_point[1]];
    applyForceAt(body1, [20, 25.0], world_point);

    ctx.clearRect(0, 0, canvas.width, canvas.height);
    ctx.lineWidth = 2;
    for (const body of simulation.bodies) {
      // circle
      ctx.beginPath();
      ctx.arc(body.pos[0], body.pos[1], body.radius, 0, 2 * Math.PI);
      ctx.stroke();
      // line from center
      const line_x = body.pos[0] + body.radius * Math.sin(body.rot);
      const line_y = body.pos[1] + body.radius * Math.cos(body.rot);
      ctx.beginPath();
      ctx.moveTo(body.pos[0], body.pos[1]);
      ctx.lineTo(line_x, line_y);
      ctx.stroke();

      // reset if body goes out of bounds
      if (
        body.pos[0] < 0 ||
        body.pos[1] < 0 ||
        body.pos[0] > canvas.width ||
        body.pos[1] > canvas.height
      ) {
        reset();
      }
    }

    // visulation
    simulation.broadphase.draw();
    for (const { contact } of simulation.contacts) {
      ctx.beginPath();
      ctx.strokeStyle = 'cyan';
      ctx.arc(contact.point[0], contact.point[1], 2, 0, 2 * Math.PI);
      ctx.stroke();
    }
    ctx.fillStyle = 'rgba(0, 200, 0, 0.5)';
    ctx.fillRect(0, simulation.ground_y, canvas.width, canvas.height - simulation.ground_y);
  }
}

// Start the animation
reset();
animate(0);

// add even listener
window.addEventListener('mousemove', (event) => {
  // Get the mouse coordinates relative to the element
  const x = event.clientX;
  const y = event.clientY;
  const query_result = simulation.broadphase.queryPoint([x, y]);
  if (query_result.size) {
    //console.log('queryResult: ', Array.from(queryResult));
  }
})

physics_2d.html

<!DOCTYPE html>
<html lang="en">
<head>
  <meta charset="UTF-8">
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  <title>Canvas Application</title>
  <style>
    /* Ensuring the canvas fills the entire window */
    body, html {
      margin: 0;
      padding: 0;
      overflow: hidden;
    }

    canvas {
      display: block;
      background-color: #f0f0f0;
    }
  </style>
</head>
<body>
  <canvas id="render_canvas"></canvas>

  <script src="physics_2d.js"></script>
</body>
</html>