Boingy2

boingy2.rb

RAD2DEG = 180.0 / Math::PI
TIME_STEP = 0.2 / 60 # delta time isn't required in DragonRuby but it really handy for tuning and debugging physics
COLLISION_BIAS = 0.05 # adds some energy into the collision to get objects to separate. tune this in steps of 0.01
COLLISION_SLOP = 0.1 # amount shapes are allowed to overlap without triggering correction. helps avoid position jitter
COLLISION_ITERATIONS = 10 # how many times to run the solver. a good range is between 5 and 15

def tick args
  args.gtk.reset if args.inputs.keyboard.r
  # Create some circles

  args.state.circles ||= 10.map_with_ys 2 do |x, y|
    radius = 32.0
    diameter = radius * 2.0
    # for static objects use mass = Float::INFINITY
    # make the bottom row static objects
    mass = y == 1 ? (Math::PI * radius * radius) : Float::INFINITY
    rotational_inertia = y == 1 ? (0.5 * radius * radius * mass) : Float::INFINITY
    offset_x = (1280.0 - ((diameter + radius) * 10.0)) / 2.0
    x = offset_x + x * (diameter + radius + 53.0) + (y == 1 ? -15 : 0)
    y = 100.0 + y * (200.0 + diameter + radius)
    {
      x: x,
      y: y,
      angle: 0.0,
      vx: 0.0, # velocity x
      vy: 0.0, # velocity y
      vw: 0.0, # angular velocity
      path: 'sprites/circle/blue.png',
      w: diameter,
      h: diameter,
      bounce: 0.8, # 0..1
      friction: 0.4,
      radius: radius,
      mass: mass,
      rotational_inertia: rotational_inertia
    }
  end

  # Create some borders
  args.state.lines ||= [
    { x: 0, y: 20, x2: 1280, y2: 0, vx: 0, vy: 0, vw: 0, bounce: 0.8, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY },
    { x: 0, y: 20, x2: 0, y2: 720,  vx: 0, vy: 0, vw: 0, bounce: 0.5, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY },
    { x: 1280, y: 0, x2: 1280, y2: 720, vx: 0, vy: 0, vw: 0, bounce: 0.5, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY },
    { x: 0, y: 720, x2: 1280, y2: 720, vx: 0, vy: 0, vw: 0, bounce: 0.5, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY },
    { x: 640.0, y: 360.0, x2: 700.0, y2: 250.0, vx: 0, vy: 0, vw: 0, bounce: 0.8, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY },
    { x: 200.0, y: 300.0, x2: 360.0, y2: 250.0,  vx: 0, vy: 0, vw: 0, bounce: 0.9, friction: 1, mass: Float::INFINITY, rotational_inertia: Float::INFINITY }
  ]

  circles = args.state.circles
  num_circles = circles.length
  args.outputs.sprites << circles

  lines = args.state.lines
  num_lines = lines.length
  args.outputs.lines << lines

  fn.each circles do |c|
    next if c.mass == Float::INFINITY

    c.vy -= 100.0 * TIME_STEP
  end


  i = 0
  while i < num_circles
    a = circles[i]

    j = i + 1
    while j < num_circles
      b = circles[j]
      j += 1
      next if a.mass == Float::INFINITY && b.mass == Float::INFINITY

      collision = find_circle_circle a, b
      calc_collision collision
    end

    k = 0
    while k < num_lines
      b = lines[k]
      k += 1
      next if a.mass == Float::INFINITY && b.mass == Float::INFINITY

      collision = find_circle_line a, b
      calc_collision collision
    end

    i += 1
  end

  fn.each circles do |c|
    next if c.mass == Float::INFINITY

    c.x += c.vx * TIME_STEP
    c.y += c.vy * TIME_STEP

    c.angle += c.vw * TIME_STEP * RAD2DEG
  end
end

def find_circle_circle a, b
  circle_ar = a.radius || [a.w, a.h].max * 0.5
  circle_br = b.radius || [b.w, b.h].max * 0.5
  circle_ax = a.x + circle_ar
  circle_ay = a.y + circle_ar
  circle_bx = b.x + circle_br
  circle_by = b.y + circle_br
  dx = circle_bx - circle_ax
  dy = circle_by - circle_ay
  distance = dx * dx + dy * dy
  min_distance = circle_ar + circle_br
  # distance should be less than the sum of radii
  # zero distance means the circles have the same centers, do nothing
  return if (distance > min_distance * min_distance) || distance.zero?

  distance = Math.sqrt distance
  dx /= distance
  dy /= distance

  contact = { r1x: circle_ax + circle_ar * dx,
              r1y: circle_ay + circle_ar * dy,
              r2x: circle_bx - circle_br * dx,
              r2y: circle_by - circle_br * dy,
              depth: distance - min_distance,
              jn: 0, jt: 0 }

  { a: a, ax: circle_ax, ay: circle_ay,
    b: b, bx: circle_bx, by: circle_by,
    normal_x: dx, normal_y: dy,
    contacts: [contact] }
end

def find_circle_line c, l
  circle_r = c.radius || [c.w, c.h].max * 0.5
  line_r = l.radius || 0.0

  circle_x = c.x + circle_r
  circle_y = c.y + circle_r
  line_x = l.x2 - l.x
  line_y = l.y2 - l.y
  t = ((line_x * (circle_x - l.x) + line_y * (circle_y - l.y)) /
    (line_x * line_x + line_y * line_y)).clamp(0.0, 1.0)

  closest_x = l.x + line_x * t
  closest_y = l.y + line_y * t

  dx = closest_x - circle_x
  dy = closest_y - circle_y
  distance = dx * dx + dy * dy
  min_distance = circle_r + line_r
  return if (distance > min_distance * min_distance) || distance.zero?

  distance = Math.sqrt distance
  dx /= distance
  dy /= distance

  contact = { r1x: circle_x + circle_r * dx,
              r1y: circle_y + circle_r * dy,
              r2x: closest_x - line_r * dx,
              r2y: closest_y - line_r * dy,
              depth: distance - min_distance,
              jn: 0, jt: 0 }

  { a: c, ax: circle_x, ay: circle_y,
    b: l, bx: line_x, by: line_y,
    normal_x: dx, normal_y: dy,
    contacts: [contact] }
end

def calc_collision collision
  return unless collision

  a = collision[:a]
  b = collision[:b]
  nx = collision[:normal_x]
  ny = collision[:normal_y]
  average_bounce = a.bounce * b.bounce
  average_friction = a.friction * b.friction

  inv_m_a = 1.0 / a.mass
  inv_m_b = 1.0 / b.mass
  inv_i_a = 1.0 / a.rotational_inertia
  inv_i_b = 1.0 / b.rotational_inertia

  inv_mass_sum = inv_m_a + inv_m_b

  fn.each collision.contacts do |contact|
    # contact point in local space
    r1x = contact[:r1x] - collision[:ax]
    r1y = contact[:r1y] - collision[:ay]
    r2x = contact[:r2x] - collision[:bx]
    r2y = contact[:r2y] - collision[:by]

    # contact point cross normal, tangent
    r1cn = r1x * ny - r1y * nx
    r2cn = r2x * ny - r2y * nx
    r1ct = r1x * nx + r1y * ny
    r2ct = r2x * nx + r2y * ny

    # sum of masses in normal and tangent directions
    mass_normal = 1.0 / (inv_mass_sum + inv_i_a * r1cn * r1cn + inv_i_b * r2cn * r2cn)
    mass_tangent = 1.0 / (inv_mass_sum + inv_i_a * r1ct * r1ct + inv_i_b * r2ct * r2ct)

    # penetration correction -- feed positional error into separation impulse (Baumgarte stabilization)
    bias = COLLISION_BIAS * [0.0, contact[:depth] + COLLISION_SLOP].min / TIME_STEP

    # relative velocity
    rvx = b.vx - r2y * b.vw - (a.vx - r1y * a.vw)
    rvy = b.vy + r2x * b.vw - (a.vy + r1x * a.vw)

    # relative velocity along normal * average bounce
    bounce = (rvx * nx + rvy * ny) * average_bounce

    COLLISION_ITERATIONS.times do
      # update the relative velocity
      vrx = b.vx - r2y * b.vw - (a.vx - r1y * a.vw)
      vry = b.vy + r2x * b.vw - (a.vy + r1x * a.vw)

      # relative velocity along normal and tangent
      rvn = vrx * nx + vry * ny
      rvt = vrx * -ny + vry * nx

      # impulse scalar (aka lambda, lagrange multiplier)
      jn = -(bounce + rvn + bias) * mass_normal
      previous_jn = contact[:jn]
      contact[:jn] = [previous_jn + jn, 0.0].max

      # tangent scalar, cannot exceed force along normal (Coulomb's law)
      max_jt = average_friction * contact[:jn]
      jt = -rvt * mass_tangent
      previous_jt = contact[:jt]
      contact[:jt] = (previous_jt + jt).clamp(-max_jt, max_jt)

      jn = contact[:jn] - previous_jn
      jt = contact[:jt] - previous_jt

      impulse_x = nx * jn - ny * jt
      impulse_y = nx * jt + ny * jn

      a[:vx] -= impulse_x * inv_m_a
      a[:vy] -= impulse_y * inv_m_a
      a[:vw] -= inv_i_a * (r1x * impulse_y - r1y * impulse_x)

      b[:vx] += impulse_x * inv_m_b
      b[:vy] += impulse_y * inv_m_b
      b[:vw] += inv_i_b * (r2x * impulse_y - r2y * impulse_x)
    end
  end
end