xenobrain · October 17, 2024 14:18
diff --git a/gjk.rb b/gjk.rb
 DEG2RAD = Math::PI / 180.0

 def tick args
  b = args.state.box_a ||= { x: 540, y: 360, w: 200, h: 200, angle: 45, path: 'sprites/square/blue.png' }
  a = args.state.box_b ||= { x: 340, y: 360, w: 100, h: 100, angle: 45, path: 'sprites/square/green.png' }
  $a = a

  a.y += 1 if args.inputs.up
  a.y -= 1 if args.inputs.down
  a.x -= 1 if args.inputs.left
  a.x += 1 if args.inputs.right
  a.angle -= 1 if args.inputs.keyboard.e
  a.angle += 1 if args.inputs.keyboard.q

  args.outputs.background_color = [32, 32, 32]
  args.outputs.sprites << b << a

  start = Time.now
  100.times do
    p = closest_points a, b

    #p = find_collision_polygon_polygon a, b
  end
  stop = (Time.now - start) * 1000

  #if p
  #  args.outputs.sprites << p.contacts.map do |c|
  #    args.outputs.sprites << { x: c.r1x - 4, y: c.r1y - 4, w: 8, h: 8, path: 'sprites/circle/yellow.png' }
  #    args.outputs.sprites << { x: c.r2x - 4, y: c.r2y - 4, w: 8, h: 8, path: 'sprites/circle/orange.png' }
  #  end
  #end
  #a.intersect_rect? b

  #args.outputs.labels << { x: 10, y: 500, text: "#{stop}", r: 255, g: 255, b: 255 }
  #puts stop
 end

 def furthest p, dx, dy
  p_len = p.length

  index = 0
  x, y = p[0]
  max = x * dx + y * dy

  i = 1
  while i < p_len
    x, y = p[i]
    d = x * dx + y * dy
    if d > max
      max = d
      index = i
    end
    i += 1
  end

  index
 end

 def closest_points a, b
  dx = a.x - b.x
  dy = a.y - b.y
  a = create_vertices a
  b = create_vertices b

  ## Initialize the Simplex ##

  # Point A
  index_a = furthest a, -dx, -dy
  index_b = furthest b, dx, dy
  a_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
  ax, ay = a[index_a]
  bx, by = b[index_b]
  simplex_a = [bx - ax, by - ay, index_a, index_b, a_id]

  # Point B
  index_a = furthest a, dx, dy
  index_b = furthest b, -dx, -dy
  b_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
  ax, ay = a[index_a]
  bx, by = b[index_b]
  simplex_b = [bx - ax, by - ay, index_a, index_b, b_id]

  ## Iterations ##
  while true
    simplex_ax, simplex_ay, a_index_a, a_index_b, a_id = simplex_a
    simplex_bx, simplex_by, b_index_a, b_index_b, b_id = simplex_b

    # Preserve winding order by flipping the points
    if (simplex_by - simplex_ay) * (simplex_ax + simplex_bx) < (simplex_bx - simplex_ax) * (simplex_ay + simplex_by)
      t = simplex_a
      simplex_a = simplex_b
      simplex_b = t
      next
    end

    # Find closest point on the line to the origin
    line_x = simplex_bx - simplex_ax
    line_y = simplex_by - simplex_ay
    closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
                  (line_x * line_x + line_y * line_y)).clamp(-1, 1)

    # If the origin projection is in Voronoi region AB, the search direction is perpendicular to the simplex
    if closest_t > -1 && closest_t < 1
      dy = (simplex_bx - simplex_ax)
      dx = -(simplex_by - simplex_ay)
    else
      # The point is in Voronoi region A or B, lerp along the simplex towards the origin
      closest_t *= 0.5
      dx = -(simplex_ax * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t))
      dy = -(simplex_ay * (0.5 - closest_t) + simplex_by * (0.5 + closest_t))
    end

    # Point C
    index_a = furthest a, -dx, -dy
    index_b = furthest b, dx, dy
    id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
    break if id == a_id || id == b_id || index_a == a_index_a || index_a == b_index_a || index_b == a_index_b || index_b == b_index_b

    ax, ay = a[index_a]
    bx, by = b[index_b]
    simplex_c = [bx - ax, by - ay, index_a, index_b, id]
    simplex_cx, simplex_cy = simplex_c

    # If CA and BC are to the right of the origin, the simplex contains the origin
    if ((simplex_ay - simplex_cy) * (simplex_cx + simplex_ax) > (simplex_ax - simplex_cx) * (simplex_cy + simplex_ay)) &&
    ((simplex_cy - simplex_by) * (simplex_bx + simplex_cx) > (simplex_cx - simplex_bx) * (simplex_by + simplex_cy))
      $gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
      break
    end

    # Simplex A and B are closer to the origin than Simplex C
    na = simplex_ax * dx + simplex_ay * dy
    nb = simplex_bx * dx + simplex_by * dy
    break if (simplex_cx * dx + simplex_cy * dy) <= (na > nb ? na : nb)


    ## Drop the furthest point from the origin ##

    # distance from origin A, C
    line_x = simplex_cx - simplex_ax
    line_y = simplex_cy - simplex_ay
    closest_t = -((line_x * (simplex_ax + simplex_cx) + line_y * (simplex_ay + simplex_cy)) /
                  (line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
    x = simplex_ax * (0.5 - closest_t) + simplex_cx * (0.5 + closest_t)
    y = simplex_ay * (0.5 - closest_t) + simplex_cy * (0.5 + closest_t)
    ac = x * x + y * y

    # distance from origin C, B
    line_x = simplex_bx - simplex_cx
    line_y = simplex_by - simplex_cy
    closest_t = -((line_x * (simplex_cx + simplex_bx) + line_y * (simplex_cy + simplex_by)) /
                  (line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
    x = simplex_cx * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t)
    y = simplex_cy * (0.5 - closest_t) + simplex_by * (0.5 + closest_t)
    cb = x * x + y * y

    ac < cb ? simplex_b = simplex_c : simplex_a = simplex_c
  end

  a_ax, a_ay, a_index_a, a_index_b, id = simplex_a
  b_ax, b_ay, b_index_a, b_index_b, id = simplex_b

  # closest point on the simplex
  line_x = simplex_bx - simplex_ax
  line_y = simplex_by - simplex_ay
  closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
    (line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5

  #closest_sx = a_ax * (0.5 - closest_t) + a_bx * (0.5 + closest_t)
  #closest_sy = a_ay * (0.5 - closest_t) + a_by * (0.5 + closest_t)

  # interpolate original points to get the closest points
  a_ax, a_ay = a[a_index_a]
  b_ax, b_ay = a[b_index_a]
  closest_ax = a_ax * (0.5 - closest_t) + b_ax * (0.5 + closest_t)
  closest_ay = a_ay * (0.5 - closest_t) + b_ay * (0.5 + closest_t)


  a_bx, a_by = b[a_index_b]

  b_bx, b_by = b[b_index_b]

  closest_bx = a_bx * (0.5 - closest_t) + b_bx * (0.5 + closest_t)
  closest_by = a_by * (0.5 - closest_t) + b_by * (0.5 + closest_t)

  $gtk.args.outputs.lines << { x: closest_ax, y: closest_ay, x2: closest_bx, y2: closest_by, g: 255 }
 end

 def create_vertices rect
  x = rect.x; y = rect.y
  w = rect.w; h = rect.h
  cx = x + w * 0.5; cy = y + h * 0.5
  sin = Math.sin (rect.angle || 0.0) * DEG2RAD; cos = Math.cos (rect.angle || 0.0) * DEG2RAD
  [
    [(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy],                 # Bottom Left
    [(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy],         # Bottom Right
    [(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
    [(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy]          # Top Left
  ].reverse
 end

 def find_collision_polygon_polygon a, b
  a = create_vertices a
  b = create_vertices b
  ## Find Minimum Separation and Reference Edge ##
  separation = -Float::INFINITY

  k = 0
  while k < 2
    a_length = a.length
    b_length = b.length

    i = 0
    while i < b_length
      v1x, v1y = b[(i + 1) % b_length]
      v0x, v0y = b[i]

      nx = -(v1y - v0y)
      ny = v1x - v0x
      l = 1.0 / (Math.sqrt nx * nx + ny * ny)
      nx *= l
      ny *= l

      v2x, v2y = a[0]
      min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny

      j = 1
      while j < a_length
        v2x, v2y = a[j]
        projection = (v2x - v0x) * nx + (v2y - v0y) * ny
        min_projection = projection if projection < min_projection

        j += 1
      end

      # early exit if the axis are separating
      return if min_projection >= 0

      # bias to improve coherence
      if min_projection * 0.95 > separation + 0.01
        separation = min_projection
        reference_edge_ax = v0x
        reference_edge_ay = v0y
        reference_edge_bx = v1x
        reference_edge_by = v1y
        incident_shape = a
        normal_x = nx
        normal_y = ny
      end

      i += 1
    end

    # swap shapes for the next test
    t = a
    a = b
    b = t

    k += 1
  end

  ## Find Incident Edge ##
  i_length = incident_shape.length
  min_projection = Float::INFINITY

  i = 0
  while i < i_length
    v1x, v1y = incident_shape[(i + 1) % i_length]
    v0x, v0y = incident_shape[i]
    nx = -(v1y - v0y)
    ny = (v1x - v0x)

    projection = nx * normal_x + ny * normal_y
    if projection < min_projection
      min_projection = projection
      incident_edge_ax = v0x
      incident_edge_ay = v0y
      incident_edge_bx = v1x
      incident_edge_by = v1y
    end

    i += 1
  end

  ## Clipping ##
  contacts = []

  dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
  dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
  dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
  dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x

  reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
  incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)

  # project the segment ends onto the other segment, clamping t
  t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
  t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
  t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
  t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)

  reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
  reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
  incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
  incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)

  if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
    contacts << { r1x: reference_point_x,
                  r1y: reference_point_y,
                  r2x: incident_point_x,
                  r2y: incident_point_y,
                  depth: separation,
                  jn: 0, jt: 0 }
  end

  reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
  reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
  incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
  incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia

  if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
    contacts << { r1x: reference_point_x,
                  r1y: reference_point_y,
                  r2x: incident_point_x,
                  r2y: incident_point_y,
                  depth: separation,
                  jn: 0, jt: 0 }
  end

  { a: a, ax: 0, ay: 0, b: b, bx: 0, by: 0, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
 end
diff --git a/main.rb b/main.rb
 RAD2DEG = 180.0 / Math::PI
 DEG2RAD = Math::PI / 180.0
 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

  box = args.state.box ||= { x: 300, y: 150,  w: 50, h: 50, vx: 0, vy: 0, vw: 0, angle: 0, bounce: 0.8, friction: 0.8, mass: Math::PI*50*50, rotational_inertia: 0.83*(50*50+50*50) * (Math::PI*50*50), path: 'sprites/square/blue.png' }
  ground = args.state.ground ||= { x: 100, y: 50,  w: 1100, h: 50, vx: 0, vy: 0, vw: 0, bounce: 0.8, friction: 0.8, mass: Float::INFINITY, rotational_inertia: Float::INFINITY, path: 'sprites/square/green.png' }

  box.vy -= 100.0 * TIME_STEP

  args.outputs.sprites << ground
  args.outputs.sprites << box


  collision = find_collision_polygon_polygon box, ground
  if collision
    contact0 = collision.contacts[0]
    contact1 = collision.contacts[1]
    calc_collision collision
  end

  box.x += box.vx * TIME_STEP
  box.y += box.vy * TIME_STEP
  box.angle += box.vw * TIME_STEP * RAD2DEG

 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 find_collision_polygon_polygon poly_a, poly_b
  a_cx = poly_a.x + (poly_a.w/2)
  a_cy = poly_a.y + (poly_a.h/2)
  b_cx = poly_b.x + (poly_b.w/2)
  b_cy = poly_b.y + (poly_b.h/2)

  a = create_vertices poly_a
  b = create_vertices poly_b
  ## Find Minimum Separation and Reference Edge ##
  separation = -Float::INFINITY

  k = 0
  while k < 2
    a_length = a.length
    b_length = b.length

    i = 0
    while i < b_length
      v1x, v1y = b[(i + 1) % b_length]
      v0x, v0y = b[i]

      nx = -(v1y - v0y)
      ny = v1x - v0x
      l = 1.0 / (Math.sqrt nx * nx + ny * ny)
      nx *= l
      ny *= l

      v2x, v2y = a[0]
      min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny

      j = 1
      while j < a_length
        v2x, v2y = a[j]
        projection = (v2x - v0x) * nx + (v2y - v0y) * ny
        min_projection = projection if projection < min_projection

        j += 1
      end

      # early exit if the axis are separating
      return if min_projection >= 0

      # bias to improve coherence
      if min_projection * 0.95 > separation + 0.01
        separation = min_projection
        reference_edge_ax = v0x
        reference_edge_ay = v0y
        reference_edge_bx = v1x
        reference_edge_by = v1y
        incident_shape = a
        normal_x = nx
        normal_y = ny
      end

      i += 1
    end

    # swap shapes for the next test
    t = a
    a = b
    b = t

    k += 1
  end

  ## Find Incident Edge ##
  i_length = incident_shape.length
  min_projection = Float::INFINITY

  i = 0
  while i < i_length
    v1x, v1y = incident_shape[(i + 1) % i_length]
    v0x, v0y = incident_shape[i]
    nx = -(v1y - v0y)
    ny = (v1x - v0x)

    projection = nx * normal_x + ny * normal_y
    if projection < min_projection
      min_projection = projection
      incident_edge_ax = v0x
      incident_edge_ay = v0y
      incident_edge_bx = v1x
      incident_edge_by = v1y
    end

    i += 1
  end

  ## Clipping ##
  contacts = []

  dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
  dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
  dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
  dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x

  reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
  incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)

  # project the segment ends onto the other segment, clamping t
  t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
  t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
  t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
  t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)

  reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
  reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
  incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
  incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)

  if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
    contacts << { r1x: reference_point_x,
                  r1y: reference_point_y,
                  r2x: incident_point_x,
                  r2y: incident_point_y,
                  depth: separation,
                  jn: 0, jt: 0 }
  end

  reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
  reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
  incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
  incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia

  if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
    contacts << { r1x: reference_point_x,
                  r1y: reference_point_y,
                  r2x: incident_point_x,
                  r2y: incident_point_y,
                  depth: separation,
                  jn: 0, jt: 0 }
  end

  { a: poly_a, ax: a_cx, ay: a_cy, b: poly_b, bx: b_cx, by: b_cy, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
 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

 def create_vertices rect
  x = rect.x; y = rect.y
  w = rect.w; h = rect.h
  cx = x + w * 0.5; cy = y + h * 0.5
  sin = Math.sin (rect.angle || 0.0) * DEG2RAD; cos = Math.cos (rect.angle || 0.0) * DEG2RAD
  [
    [(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy],                 # Bottom Left
    [(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy],         # Bottom Right
    [(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
    [(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy]          # Top Left
  ].reverse
 end
diff --git a/quickhull.rb b/quickhull.rb
 module Physics
  class << self
    attr_accessor :debug_draw

    INV_3 = 1.0 / 3.0
    NUM_ITERATIONS = 10
    MAX_GJK_ITERATIONS = 30
    MAX_EPA_ITERATIONS = 30

    @debug_draw = false

    def new_polygon x, y, vertices, density = 1
      num_vertices = vertices.length
      hull_vertices = []
      hull_indices = []

      moment_of_inertia = 0
      polygon_area = 0
      center_x = 0
      center_y = 0

      right = 0
      left = vertices.at(0).at(0)

      i = 1
      while i < num_vertices
        hx, hy = vertices.at(i)

        if hx > left
          left = hx
          right = i
        elsif hx == left
          right = i if hy < vertices.at(right).at(1)
        end

        i += 1
      end

      index = right

      while true
        next_index = 0
        hull_indices << index
        hull_index = hull_indices.last

        i = 1
        while i < num_vertices
          if next_index == index
            next_index = i
            next
          end

          px, py = vertices.at(i)
          hx, hy = vertices.at(hull_index)
          nx, ny = vertices.at(next_index)

          e1x = nx - hx
          e1y = ny - hy
          e2x = px - hx
          e2y = py - hy
          cross = e1x * e2y - e1y * e2x
          next_index = i if cross.negative?
          next_index = i if cross.zero? && (e2x*e2x + e2y*e2y) > (e1x*e1x + e1y*e1y)

          i += 1
        end

        index = next_index
        break if next_index == right
      end

      num_vertices = hull_indices.length

      i = 0
      while i < num_vertices
        x0, y0 = vertices.at(hull_indices.at(i))
        x1, y1 = vertices.at(hull_indices.at((i + 1) % num_vertices))

        nx = y1 - y0
        ny = x0 - y1
        cross = x0*y1 - y0*x1
        triangle_area = 0.5*cross
        polygon_area += triangle_area
        center_x += triangle_area*INV_3*(x0 + x1)
        center_y += triangle_area*INV_3*(y0 + y1)
        moment_of_inertia += 0.25*INV_3*cross*(x0*x0 + x0*x1 + x1*x1 + y0*y0 + y0*y1 + y1*y1)

        hull_vertices << [x0, y0, nx, ny]
        i += 1
      end

      center_x *= 1.0 / polygon_area
      center_y *= 1.0 / polygon_area

      min_x = min_y = Float::INFINITY
      max_x = max_y = -Float::INFINITY

      i = 0
      while i < num_vertices
        vertex = hull_vertices.at i
        px = vertex[0] -= center_x - x
        py = vertex[1] -= center_y - y

        min_x = px if px < min_x
        min_y = py if py < min_y
        max_x = px if px > max_x
        max_y = py if py > max_y
        i += 1
      end

      w = max_x - min_x
      h = max_y - min_y
      inverse_mass = 1.0 / (density*polygon_area)
      inverse_moment_of_inertia = 1.0 / moment_of_inertia
      aabb = { x: min_x, y: min_y, w: w, h: h, tick: Kernel.tick_count }

      hash = {
        x: x,
        y: y,
        w: w,
        h: h,
        vertices: hull_vertices,
        velocity_x: 0.0,
        velocity_y: 0.0,
        angular_velocity: 0.0,
        aabb: aabb,
        radius: [w, h].max * 0.5,
        inverse_mass: inverse_mass,
        inverse_moment_of_inertia: inverse_moment_of_inertia
      }
      puts hash.to_s
      hash
    end

    def find_collision_polygon_polygon shape_a, shape_b
      ## Medium phase ##

      radii_sum = shape_a[:radius] + shape_b[:radius]
      dx = shape_b[:x] - shape_a[:x]
      dy = shape_b[:y] - shape_a[:y]
      distance = dx*dx + dy*dy
      #return unless distance < radii_sum*radii_sum

      current_tick = Kernel.tick_count

      if shape_a[:inverse_mass].positive? && shape_a[:aabb][:tick] != current_tick
        num_vertices = shape_a[:vertices].length
        max_x = max_y = -Float::INFINITY
        min_x = min_y = Float::INFINITY
        aabb = shape_a[:aabb]

        i = 0
        while i < num_vertices
          x, y = shape_a[:vertices].at i
          min_x = x if x < min_x
          min_y = y if y < min_y
          max_x = x if x > max_x
          max_y = y if y > max_y
          i += 1
        end

        aabb[:x] = min_x
        aabb[:y] = min_y
        aabb[:w] = max_x - min_x
        aabb[:h] = max_y - min_y
        aabb[:tick] = current_tick
      end

      if shape_b[:inverse_mass].positive? && shape_b[:aabb][:tick] != current_tick
        num_vertices = shape_b[:vertices].length
        max_x = max_y = -Float::INFINITY
        min_x = min_y = Float::INFINITY
        aabb = shape_b[:aabb]

        i = 0
        while i < num_vertices
          x, y = shape_b[:vertices].at i
          min_x = x if x < min_x
          min_y = y if y < min_y
          max_x = x if x > max_x
          max_y = y if y > max_y
          i += 1
        end

        aabb[:x] = min_x
        aabb[:y] = min_y
        aabb[:w] = max_x - min_x
        aabb[:h] = max_y - min_y
        aabb[:tick] = current_tick
      end

      #return false unless $gtk.args.geometry.intersect_rect? shape_a[:aabb], shape_b[:aabb]

      ## Narrow phase ##

      # Lookup cached points from the collision ID
      # Collision ID is modified from default--for polys it stores the index of 2 minkowski points in a bitfield

      support_x = 0
      support_y = 0
      support_id = 0
      support_point = proc do
        max_ax = max_bx = max_ay = max_by = max_projection_a = max_projection_b = -Float::INFINITY
        index_a = index_b = 0

        j = 0
        vertices = shape_a[:vertices]
        num_vertices = vertices.length
        while j < num_vertices
          x, y = vertices.at j
          projection = x*dx + y*dy
          if projection > max_projection_a
            max_projection_a = projection
            index_a = j
            max_ax = x
            max_ay = y
          end

          j += 1
        end

        j = 0
        vertices = shape_b[:vertices]
        num_vertices = vertices.length
        while j < num_vertices
          x, y = vertices.at j
          projection = x*dx + y*dy
          if projection > max_projection_b
            max_projection_b = projection
            index_b = j
            max_bx = x
            max_by = y
          end
          j += 1
        end

        support_x = max_ax - max_bx
        support_y = max_ay - max_by
        support_id = (index_a & 0xFF) << 8 | (index_b & 0xFF)
      end

      # Begin GJK Closest Points algorithm

      temp = dx
      dx = -dy
      dy = temp

      support_point.call
      point_ax = support_x
      point_ay = support_y

      dx = -dx
      dy = -dy
      support_point.call
      point_bx = support_x
      point_by = support_y

      i = 0
      while i < MAX_GJK_ITERATIONS
        if (point_ay - point_by)*(point_bx + point_ax) > (point_ax - point_bx)*(point_by + point_ay)
          temp = point_ax
          point_ax = point_bx
          point_bx = temp
          temp = point_ay
          point_by = point_ay
          point_ay = temp
          next
        end

        # To find the next search direction, interpolate segment ab towards the origin(0,0) of the simplex
        sx = point_bx - point_ax
        sy = point_by - point_ay
        t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)

        if t > -1 && t < 1
          # t wasn't clamped, choose a perpendicular direction
          dx = -(point_by - point_ay)
          dy = point_bx - point_ax
        else
          # t was clamped, inverse of interpolate point_a, point_b towards the origin with t
          t *= 0.5
          dx = -((point_ax*(0.5 - t)) + (point_bx*(0.5 + t)))
          dy = -((point_ay*(0.5 - t)) + (point_by*(0.5 + t)))
        end

        # Get a new support point based on the current direction
        support_point.call
        point_cx = support_x
        point_cy = support_y

        if @debug_draw
          cx = point_ax*0.5 + point_bx*0.5
          cy = point_ay*0.5 + point_by*0.5

          len = Math.sqrt(dx*dx + dy*dy)
          nx = dx*len
          ny = dy*len
          cx2 = cx + nx*5.0
          cy2 = cy + ny*5.0
          # Draw center of the screen
          $gtk.args.outputs.sprites << {x: 640-3, y: 360-3, w: 6,  h: 6, path: 'sprites/circle/white.png' }

          $gtk.args.outputs.lines << { x: point_ax + 640, y: point_ay + 360, x2: point_bx + 640, y2: point_by + 360, r: 255, g: 255, b: 255 }
          #$gtk.args.outputs.lines << { x: cx + 640, y: cy + 360, x2: cx2 + 640, y2: cy2 + 360, r: 255, g: 0, b: 255 }
        end

        if (point_ay - point_cy)*(point_cx + point_ax) > (point_ax - point_cx)*(point_cy + point_ay) &&
           (point_cy - point_by)*(point_bx + point_cx) > (point_cx - point_bx)*(point_by + point_cy)
          # The triangle three points contains the origin so we're overlapping
          # Begin EPA algorithm
          # For now, just return true that there was a collision
          $gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
          break
        else
          # check axis point_a, point_b, point_c, n
          if (point_cx*dx + point_cy*dx) <= [(point_ax*dx + point_ay*dy), (point_bx*dx + point_by*dy)].max
            break
          else
            # ClosestDist point_a, point_c < ClosestDistance point_c, point_b

            # Closest_t for point_a, point_c
            sx = point_cx - point_ax
            sy = point_cy - point_ay
            t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)
            t *= 0.5
            # lerp point_a, point_c
            ac_x = (point_ax*(0.5 - t)) + (point_cx*(0.5 + t))
            ac_y = (point_ay*(0.5 - t)) + (point_cy*(0.5 + t))
            ac_dist = ac_x*ac_x + ac_y*ac_y

            # closest_t for point_c, point_b
            sx = point_bx - point_cx
            sy = point_bx - point_cx
            t = -((sx*(point_ax + point_bx) + sy*(point_ay + point_by)) / (sx*sx + sy*sy)).clamp(-1.0, 1.0)
            t *= 0.5
            # lerp point_c, point_b
            cb_x = (point_cx*(0.5 - t)) + (point_bx*(0.5 + t))
            cb_y = (point_cy*(0.5 - t)) + (point_by*(0.5 + t))
            cb_dist = cb_x*cb_x + cb_y*cb_y

            if ac_dist < cb_dist
              point_bx = point_cx
              point_by = point_cy
            else
              point_ax = point_cx
              point_ay = point_cy
            end
          end
        end

        i += 1
      end

      if @debug_draw
        $gtk.args.outputs.lines << { x: point_ax+640, y: point_ay+360, x2: point_bx+640, y2: point_by+360, r: 255, g: 0, b: 0 }
      end
      # For now just return false if we got here
      false
    end

    def calc_collision collision
    end
  end
 end
	DEG2RAD = Math::PI / 180.0

	def tick args
	b = args.state.box_a \|\|= { x: 540, y: 360, w: 200, h: 200, angle: 45, path: 'sprites/square/blue.png' }
	a = args.state.box_b \|\|= { x: 340, y: 360, w: 100, h: 100, angle: 45, path: 'sprites/square/green.png' }
	$a = a

	a.y += 1 if args.inputs.up
	a.y -= 1 if args.inputs.down
	a.x -= 1 if args.inputs.left
	a.x += 1 if args.inputs.right
	a.angle -= 1 if args.inputs.keyboard.e
	a.angle += 1 if args.inputs.keyboard.q

	args.outputs.background_color = [32, 32, 32]
	args.outputs.sprites << b << a

	start = Time.now
	100.times do
	p = closest_points a, b

	#p = find_collision_polygon_polygon a, b
	end
	stop = (Time.now - start) * 1000

	#if p
	# args.outputs.sprites << p.contacts.map do \|c\|
	# args.outputs.sprites << { x: c.r1x - 4, y: c.r1y - 4, w: 8, h: 8, path: 'sprites/circle/yellow.png' }
	# args.outputs.sprites << { x: c.r2x - 4, y: c.r2y - 4, w: 8, h: 8, path: 'sprites/circle/orange.png' }
	# end
	#end
	#a.intersect_rect? b

	#args.outputs.labels << { x: 10, y: 500, text: "#{stop}", r: 255, g: 255, b: 255 }
	#puts stop
	end

	def furthest p, dx, dy
	p_len = p.length

	index = 0
	x, y = p[0]
	max = x * dx + y * dy

	i = 1
	while i < p_len
	x, y = p[i]
	d = x * dx + y * dy
	if d > max
	max = d
	index = i
	end
	i += 1
	end

	index
	end

	def closest_points a, b
	dx = a.x - b.x
	dy = a.y - b.y
	a = create_vertices a
	b = create_vertices b

	## Initialize the Simplex ##

	# Point A
	index_a = furthest a, -dx, -dy
	index_b = furthest b, dx, dy
	a_id = (index_a & 0xFF) << 8 \| (index_b & 0xFF)
	ax, ay = a[index_a]
	bx, by = b[index_b]
	simplex_a = [bx - ax, by - ay, index_a, index_b, a_id]

	# Point B
	index_a = furthest a, dx, dy
	index_b = furthest b, -dx, -dy
	b_id = (index_a & 0xFF) << 8 \| (index_b & 0xFF)
	ax, ay = a[index_a]
	bx, by = b[index_b]
	simplex_b = [bx - ax, by - ay, index_a, index_b, b_id]

	## Iterations ##
	while true
	simplex_ax, simplex_ay, a_index_a, a_index_b, a_id = simplex_a
	simplex_bx, simplex_by, b_index_a, b_index_b, b_id = simplex_b

	# Preserve winding order by flipping the points
	if (simplex_by - simplex_ay) * (simplex_ax + simplex_bx) < (simplex_bx - simplex_ax) * (simplex_ay + simplex_by)
	t = simplex_a
	simplex_a = simplex_b
	simplex_b = t
	next
	end

	# Find closest point on the line to the origin
	line_x = simplex_bx - simplex_ax
	line_y = simplex_by - simplex_ay
	closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
	(line_x * line_x + line_y * line_y)).clamp(-1, 1)

	# If the origin projection is in Voronoi region AB, the search direction is perpendicular to the simplex
	if closest_t > -1 && closest_t < 1
	dy = (simplex_bx - simplex_ax)
	dx = -(simplex_by - simplex_ay)
	else
	# The point is in Voronoi region A or B, lerp along the simplex towards the origin
	closest_t *= 0.5
	dx = -(simplex_ax * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t))
	dy = -(simplex_ay * (0.5 - closest_t) + simplex_by * (0.5 + closest_t))
	end

	# Point C
	index_a = furthest a, -dx, -dy
	index_b = furthest b, dx, dy
	id = (index_a & 0xFF) << 8 \| (index_b & 0xFF)
	break if id == a_id \|\| id == b_id \|\| index_a == a_index_a \|\| index_a == b_index_a \|\| index_b == a_index_b \|\| index_b == b_index_b

	ax, ay = a[index_a]
	bx, by = b[index_b]
	simplex_c = [bx - ax, by - ay, index_a, index_b, id]
	simplex_cx, simplex_cy = simplex_c

	# If CA and BC are to the right of the origin, the simplex contains the origin
	if ((simplex_ay - simplex_cy) * (simplex_cx + simplex_ax) > (simplex_ax - simplex_cx) * (simplex_cy + simplex_ay)) &&
	((simplex_cy - simplex_by) * (simplex_bx + simplex_cx) > (simplex_cx - simplex_bx) * (simplex_by + simplex_cy))
	$gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
	break
	end

	# Simplex A and B are closer to the origin than Simplex C
	na = simplex_ax * dx + simplex_ay * dy
	nb = simplex_bx * dx + simplex_by * dy
	break if (simplex_cx * dx + simplex_cy * dy) <= (na > nb ? na : nb)


	## Drop the furthest point from the origin ##

	# distance from origin A, C
	line_x = simplex_cx - simplex_ax
	line_y = simplex_cy - simplex_ay
	closest_t = -((line_x * (simplex_ax + simplex_cx) + line_y * (simplex_ay + simplex_cy)) /
	(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
	x = simplex_ax * (0.5 - closest_t) + simplex_cx * (0.5 + closest_t)
	y = simplex_ay * (0.5 - closest_t) + simplex_cy * (0.5 + closest_t)
	ac = x * x + y * y

	# distance from origin C, B
	line_x = simplex_bx - simplex_cx
	line_y = simplex_by - simplex_cy
	closest_t = -((line_x * (simplex_cx + simplex_bx) + line_y * (simplex_cy + simplex_by)) /
	(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5
	x = simplex_cx * (0.5 - closest_t) + simplex_bx * (0.5 + closest_t)
	y = simplex_cy * (0.5 - closest_t) + simplex_by * (0.5 + closest_t)
	cb = x * x + y * y

	ac < cb ? simplex_b = simplex_c : simplex_a = simplex_c
	end

	a_ax, a_ay, a_index_a, a_index_b, id = simplex_a
	b_ax, b_ay, b_index_a, b_index_b, id = simplex_b

	# closest point on the simplex
	line_x = simplex_bx - simplex_ax
	line_y = simplex_by - simplex_ay
	closest_t = -((line_x * (simplex_ax + simplex_bx) + line_y * (simplex_ay + simplex_by)) /
	(line_x * line_x + line_y * line_y)).clamp(-1, 1) * 0.5

	#closest_sx = a_ax * (0.5 - closest_t) + a_bx * (0.5 + closest_t)
	#closest_sy = a_ay * (0.5 - closest_t) + a_by * (0.5 + closest_t)

	# interpolate original points to get the closest points
	a_ax, a_ay = a[a_index_a]
	b_ax, b_ay = a[b_index_a]
	closest_ax = a_ax * (0.5 - closest_t) + b_ax * (0.5 + closest_t)
	closest_ay = a_ay * (0.5 - closest_t) + b_ay * (0.5 + closest_t)


	a_bx, a_by = b[a_index_b]

	b_bx, b_by = b[b_index_b]

	closest_bx = a_bx * (0.5 - closest_t) + b_bx * (0.5 + closest_t)
	closest_by = a_by * (0.5 - closest_t) + b_by * (0.5 + closest_t)

	$gtk.args.outputs.lines << { x: closest_ax, y: closest_ay, x2: closest_bx, y2: closest_by, g: 255 }
	end

	def create_vertices rect
	x = rect.x; y = rect.y
	w = rect.w; h = rect.h
	cx = x + w * 0.5; cy = y + h * 0.5
	sin = Math.sin (rect.angle \|\| 0.0) * DEG2RAD; cos = Math.cos (rect.angle \|\| 0.0) * DEG2RAD
	[
	[(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy], # Bottom Left
	[(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy], # Bottom Right
	[(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
	[(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy] # Top Left
	].reverse
	end

	def find_collision_polygon_polygon a, b
	a = create_vertices a
	b = create_vertices b
	## Find Minimum Separation and Reference Edge ##
	separation = -Float::INFINITY

	k = 0
	while k < 2
	a_length = a.length
	b_length = b.length

	i = 0
	while i < b_length
	v1x, v1y = b[(i + 1) % b_length]
	v0x, v0y = b[i]

	nx = -(v1y - v0y)
	ny = v1x - v0x
	l = 1.0 / (Math.sqrt nx * nx + ny * ny)
	nx *= l
	ny *= l

	v2x, v2y = a[0]
	min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny

	j = 1
	while j < a_length
	v2x, v2y = a[j]
	projection = (v2x - v0x) * nx + (v2y - v0y) * ny
	min_projection = projection if projection < min_projection

	j += 1
	end

	# early exit if the axis are separating
	return if min_projection >= 0

	# bias to improve coherence
	if min_projection * 0.95 > separation + 0.01
	separation = min_projection
	reference_edge_ax = v0x
	reference_edge_ay = v0y
	reference_edge_bx = v1x
	reference_edge_by = v1y
	incident_shape = a
	normal_x = nx
	normal_y = ny
	end

	i += 1
	end

	# swap shapes for the next test
	t = a
	a = b
	b = t

	k += 1
	end

	## Find Incident Edge ##
	i_length = incident_shape.length
	min_projection = Float::INFINITY

	i = 0
	while i < i_length
	v1x, v1y = incident_shape[(i + 1) % i_length]
	v0x, v0y = incident_shape[i]
	nx = -(v1y - v0y)
	ny = (v1x - v0x)

	projection = nx * normal_x + ny * normal_y
	if projection < min_projection
	min_projection = projection
	incident_edge_ax = v0x
	incident_edge_ay = v0y
	incident_edge_bx = v1x
	incident_edge_by = v1y
	end

	i += 1
	end

	## Clipping ##
	contacts = []

	dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
	dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
	dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
	dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x

	reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
	incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)

	# project the segment ends onto the other segment, clamping t
	t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
	t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
	t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
	t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)

	reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
	reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
	incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
	incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)

	if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
	contacts << { r1x: reference_point_x,
	r1y: reference_point_y,
	r2x: incident_point_x,
	r2y: incident_point_y,
	depth: separation,
	jn: 0, jt: 0 }
	end

	reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
	reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
	incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
	incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia

	if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
	contacts << { r1x: reference_point_x,
	r1y: reference_point_y,
	r2x: incident_point_x,
	r2y: incident_point_y,
	depth: separation,
	jn: 0, jt: 0 }
	end

	{ a: a, ax: 0, ay: 0, b: b, bx: 0, by: 0, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
	end
	RAD2DEG = 180.0 / Math::PI
	DEG2RAD = Math::PI / 180.0
	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

	box = args.state.box \|\|= { x: 300, y: 150, w: 50, h: 50, vx: 0, vy: 0, vw: 0, angle: 0, bounce: 0.8, friction: 0.8, mass: Math::PI5050, rotational_inertia: 0.83(5050+5050) (Math::PI5050), path: 'sprites/square/blue.png' }
	ground = args.state.ground \|\|= { x: 100, y: 50, w: 1100, h: 50, vx: 0, vy: 0, vw: 0, bounce: 0.8, friction: 0.8, mass: Float::INFINITY, rotational_inertia: Float::INFINITY, path: 'sprites/square/green.png' }

	box.vy -= 100.0 * TIME_STEP

	args.outputs.sprites << ground
	args.outputs.sprites << box


	collision = find_collision_polygon_polygon box, ground
	if collision
	contact0 = collision.contacts[0]
	contact1 = collision.contacts[1]
	calc_collision collision
	end

	box.x += box.vx * TIME_STEP
	box.y += box.vy * TIME_STEP
	box.angle += box.vw * TIME_STEP * RAD2DEG

	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 find_collision_polygon_polygon poly_a, poly_b
	a_cx = poly_a.x + (poly_a.w/2)
	a_cy = poly_a.y + (poly_a.h/2)
	b_cx = poly_b.x + (poly_b.w/2)
	b_cy = poly_b.y + (poly_b.h/2)

	a = create_vertices poly_a
	b = create_vertices poly_b
	## Find Minimum Separation and Reference Edge ##
	separation = -Float::INFINITY

	k = 0
	while k < 2
	a_length = a.length
	b_length = b.length

	i = 0
	while i < b_length
	v1x, v1y = b[(i + 1) % b_length]
	v0x, v0y = b[i]

	nx = -(v1y - v0y)
	ny = v1x - v0x
	l = 1.0 / (Math.sqrt nx * nx + ny * ny)
	nx *= l
	ny *= l

	v2x, v2y = a[0]
	min_projection = (v2x - v0x) * nx + (v2y - v0y) * ny

	j = 1
	while j < a_length
	v2x, v2y = a[j]
	projection = (v2x - v0x) * nx + (v2y - v0y) * ny
	min_projection = projection if projection < min_projection

	j += 1
	end

	# early exit if the axis are separating
	return if min_projection >= 0

	# bias to improve coherence
	if min_projection * 0.95 > separation + 0.01
	separation = min_projection
	reference_edge_ax = v0x
	reference_edge_ay = v0y
	reference_edge_bx = v1x
	reference_edge_by = v1y
	incident_shape = a
	normal_x = nx
	normal_y = ny
	end

	i += 1
	end

	# swap shapes for the next test
	t = a
	a = b
	b = t

	k += 1
	end

	## Find Incident Edge ##
	i_length = incident_shape.length
	min_projection = Float::INFINITY

	i = 0
	while i < i_length
	v1x, v1y = incident_shape[(i + 1) % i_length]
	v0x, v0y = incident_shape[i]
	nx = -(v1y - v0y)
	ny = (v1x - v0x)

	projection = nx * normal_x + ny * normal_y
	if projection < min_projection
	min_projection = projection
	incident_edge_ax = v0x
	incident_edge_ay = v0y
	incident_edge_bx = v1x
	incident_edge_by = v1y
	end

	i += 1
	end

	## Clipping ##
	contacts = []

	dist_reference_a = reference_edge_ax * normal_y - reference_edge_ay * normal_x
	dist_reference_b = reference_edge_bx * normal_y - reference_edge_by * normal_x
	dist_incident_a = incident_edge_ax * normal_y - incident_edge_ay * normal_x
	dist_incident_b = incident_edge_bx * normal_y - incident_edge_by * normal_x

	reference_denominator = 1.0 / (dist_reference_b - dist_reference_a)
	incident_denominator = 1.0 / (dist_incident_b - dist_incident_a)

	# project the segment ends onto the other segment, clamping t
	t_ibra = ((dist_incident_b - dist_reference_a) * reference_denominator).clamp(0, 1)
	t_raia = ((dist_reference_a - dist_incident_a) * incident_denominator).clamp(0, 1)
	t_iara = ((dist_incident_a - dist_reference_a) * reference_denominator).clamp(0, 1)
	t_rbia = ((dist_reference_b - dist_incident_a) * incident_denominator).clamp(0, 1)

	reference_point_x = reference_edge_ax + t_ibra * (reference_edge_bx - reference_edge_ax)
	reference_point_y = reference_edge_ay + t_ibra * (reference_edge_by - reference_edge_ay)
	incident_point_x = incident_edge_ax + t_raia * (incident_edge_bx - incident_edge_ax)
	incident_point_y = incident_edge_ay + t_raia * (incident_edge_by - incident_edge_ay)

	if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
	contacts << { r1x: reference_point_x,
	r1y: reference_point_y,
	r2x: incident_point_x,
	r2y: incident_point_y,
	depth: separation,
	jn: 0, jt: 0 }
	end

	reference_point_x = reference_edge_ax.lerp reference_edge_bx, t_iara
	reference_point_y = reference_edge_ay.lerp reference_edge_by, t_iara
	incident_point_x = incident_edge_ax.lerp incident_edge_bx, t_rbia
	incident_point_y = incident_edge_ay.lerp incident_edge_by, t_rbia

	if ((incident_point_x - reference_point_x) * normal_x + (incident_point_y - reference_point_y) * normal_y) <= 0
	contacts << { r1x: reference_point_x,
	r1y: reference_point_y,
	r2x: incident_point_x,
	r2y: incident_point_y,
	depth: separation,
	jn: 0, jt: 0 }
	end

	{ a: poly_a, ax: a_cx, ay: a_cy, b: poly_b, bx: b_cx, by: b_cy, normal_x: normal_x, normal_y: normal_y, contacts: contacts }
	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

	def create_vertices rect
	x = rect.x; y = rect.y
	w = rect.w; h = rect.h
	cx = x + w * 0.5; cy = y + h * 0.5
	sin = Math.sin (rect.angle \|\| 0.0) * DEG2RAD; cos = Math.cos (rect.angle \|\| 0.0) * DEG2RAD
	[
	[(x - cx) * cos - (y - cy) * sin + cx, (x - cx) * sin + (y - cy) * cos + cy], # Bottom Left
	[(x + w - cx) * cos - (y - cy) * sin + cx, (x + w - cx) * sin + (y - cy) * cos + cy], # Bottom Right
	[(x + w - cx) * cos - (y + h - cy) * sin + cx, (x + w - cx) * sin + (y + h - cy) * cos + cy], # Top Right
	[(x - cx) * cos - (y + h - cy) * sin + cx, (x - cx) * sin + (y + h - cy) * cos + cy] # Top Left
	].reverse
	end
	module Physics
	class << self
	attr_accessor :debug_draw

	INV_3 = 1.0 / 3.0
	NUM_ITERATIONS = 10
	MAX_GJK_ITERATIONS = 30
	MAX_EPA_ITERATIONS = 30

	@debug_draw = false

	def new_polygon x, y, vertices, density = 1
	num_vertices = vertices.length
	hull_vertices = []
	hull_indices = []

	moment_of_inertia = 0
	polygon_area = 0
	center_x = 0
	center_y = 0

	right = 0
	left = vertices.at(0).at(0)

	i = 1
	while i < num_vertices
	hx, hy = vertices.at(i)

	if hx > left
	left = hx
	right = i
	elsif hx == left
	right = i if hy < vertices.at(right).at(1)
	end

	i += 1
	end

	index = right

	while true
	next_index = 0
	hull_indices << index
	hull_index = hull_indices.last

	i = 1
	while i < num_vertices
	if next_index == index
	next_index = i
	next
	end

	px, py = vertices.at(i)
	hx, hy = vertices.at(hull_index)
	nx, ny = vertices.at(next_index)

	e1x = nx - hx
	e1y = ny - hy
	e2x = px - hx
	e2y = py - hy
	cross = e1x * e2y - e1y * e2x
	next_index = i if cross.negative?
	next_index = i if cross.zero? && (e2xe2x + e2ye2y) > (e1xe1x + e1ye1y)

	i += 1
	end

	index = next_index
	break if next_index == right
	end

	num_vertices = hull_indices.length

	i = 0
	while i < num_vertices
	x0, y0 = vertices.at(hull_indices.at(i))
	x1, y1 = vertices.at(hull_indices.at((i + 1) % num_vertices))

	nx = y1 - y0
	ny = x0 - y1
	cross = x0y1 - y0x1
	triangle_area = 0.5*cross
	polygon_area += triangle_area
	center_x += triangle_areaINV_3(x0 + x1)
	center_y += triangle_areaINV_3(y0 + y1)
	moment_of_inertia += 0.25INV_3cross(x0x0 + x0x1 + x1x1 + y0y0 + y0y1 + y1*y1)

	hull_vertices << [x0, y0, nx, ny]
	i += 1
	end

	center_x *= 1.0 / polygon_area
	center_y *= 1.0 / polygon_area

	min_x = min_y = Float::INFINITY
	max_x = max_y = -Float::INFINITY

	i = 0
	while i < num_vertices
	vertex = hull_vertices.at i
	px = vertex[0] -= center_x - x
	py = vertex[1] -= center_y - y

	min_x = px if px < min_x
	min_y = py if py < min_y
	max_x = px if px > max_x
	max_y = py if py > max_y
	i += 1
	end

	w = max_x - min_x
	h = max_y - min_y
	inverse_mass = 1.0 / (density*polygon_area)
	inverse_moment_of_inertia = 1.0 / moment_of_inertia
	aabb = { x: min_x, y: min_y, w: w, h: h, tick: Kernel.tick_count }

	hash = {
	x: x,
	y: y,
	w: w,
	h: h,
	vertices: hull_vertices,
	velocity_x: 0.0,
	velocity_y: 0.0,
	angular_velocity: 0.0,
	aabb: aabb,
	radius: [w, h].max * 0.5,
	inverse_mass: inverse_mass,
	inverse_moment_of_inertia: inverse_moment_of_inertia
	}
	puts hash.to_s
	hash
	end

	def find_collision_polygon_polygon shape_a, shape_b
	## Medium phase ##

	radii_sum = shape_a[:radius] + shape_b[:radius]
	dx = shape_b[:x] - shape_a[:x]
	dy = shape_b[:y] - shape_a[:y]
	distance = dxdx + dydy
	#return unless distance < radii_sum*radii_sum

	current_tick = Kernel.tick_count

	if shape_a[:inverse_mass].positive? && shape_a[:aabb][:tick] != current_tick
	num_vertices = shape_a[:vertices].length
	max_x = max_y = -Float::INFINITY
	min_x = min_y = Float::INFINITY
	aabb = shape_a[:aabb]

	i = 0
	while i < num_vertices
	x, y = shape_a[:vertices].at i
	min_x = x if x < min_x
	min_y = y if y < min_y
	max_x = x if x > max_x
	max_y = y if y > max_y
	i += 1
	end

	aabb[:x] = min_x
	aabb[:y] = min_y
	aabb[:w] = max_x - min_x
	aabb[:h] = max_y - min_y
	aabb[:tick] = current_tick
	end

	if shape_b[:inverse_mass].positive? && shape_b[:aabb][:tick] != current_tick
	num_vertices = shape_b[:vertices].length
	max_x = max_y = -Float::INFINITY
	min_x = min_y = Float::INFINITY
	aabb = shape_b[:aabb]

	i = 0
	while i < num_vertices
	x, y = shape_b[:vertices].at i
	min_x = x if x < min_x
	min_y = y if y < min_y
	max_x = x if x > max_x
	max_y = y if y > max_y
	i += 1
	end

	aabb[:x] = min_x
	aabb[:y] = min_y
	aabb[:w] = max_x - min_x
	aabb[:h] = max_y - min_y
	aabb[:tick] = current_tick
	end

	#return false unless $gtk.args.geometry.intersect_rect? shape_a[:aabb], shape_b[:aabb]

	## Narrow phase ##

	# Lookup cached points from the collision ID
	# Collision ID is modified from default--for polys it stores the index of 2 minkowski points in a bitfield

	support_x = 0
	support_y = 0
	support_id = 0
	support_point = proc do
	max_ax = max_bx = max_ay = max_by = max_projection_a = max_projection_b = -Float::INFINITY
	index_a = index_b = 0

	j = 0
	vertices = shape_a[:vertices]
	num_vertices = vertices.length
	while j < num_vertices
	x, y = vertices.at j
	projection = xdx + ydy
	if projection > max_projection_a
	max_projection_a = projection
	index_a = j
	max_ax = x
	max_ay = y
	end

	j += 1
	end

	j = 0
	vertices = shape_b[:vertices]
	num_vertices = vertices.length
	while j < num_vertices
	x, y = vertices.at j
	projection = xdx + ydy
	if projection > max_projection_b
	max_projection_b = projection
	index_b = j
	max_bx = x
	max_by = y
	end
	j += 1
	end

	support_x = max_ax - max_bx
	support_y = max_ay - max_by
	support_id = (index_a & 0xFF) << 8 \| (index_b & 0xFF)
	end

	# Begin GJK Closest Points algorithm

	temp = dx
	dx = -dy
	dy = temp

	support_point.call
	point_ax = support_x
	point_ay = support_y

	dx = -dx
	dy = -dy
	support_point.call
	point_bx = support_x
	point_by = support_y

	i = 0
	while i < MAX_GJK_ITERATIONS
	if (point_ay - point_by)(point_bx + point_ax) > (point_ax - point_bx)(point_by + point_ay)
	temp = point_ax
	point_ax = point_bx
	point_bx = temp
	temp = point_ay
	point_by = point_ay
	point_ay = temp
	next
	end

	# To find the next search direction, interpolate segment ab towards the origin(0,0) of the simplex
	sx = point_bx - point_ax
	sy = point_by - point_ay
	t = -((sx(point_ax + point_bx) + sy(point_ay + point_by)) / (sxsx + sysy)).clamp(-1.0, 1.0)

	if t > -1 && t < 1
	# t wasn't clamped, choose a perpendicular direction
	dx = -(point_by - point_ay)
	dy = point_bx - point_ax
	else
	# t was clamped, inverse of interpolate point_a, point_b towards the origin with t
	t *= 0.5
	dx = -((point_ax(0.5 - t)) + (point_bx(0.5 + t)))
	dy = -((point_ay(0.5 - t)) + (point_by(0.5 + t)))
	end

	# Get a new support point based on the current direction
	support_point.call
	point_cx = support_x
	point_cy = support_y

	if @debug_draw
	cx = point_ax0.5 + point_bx0.5
	cy = point_ay0.5 + point_by0.5

	len = Math.sqrt(dxdx + dydy)
	nx = dx*len
	ny = dy*len
	cx2 = cx + nx*5.0
	cy2 = cy + ny*5.0
	# Draw center of the screen
	$gtk.args.outputs.sprites << {x: 640-3, y: 360-3, w: 6, h: 6, path: 'sprites/circle/white.png' }

	$gtk.args.outputs.lines << { x: point_ax + 640, y: point_ay + 360, x2: point_bx + 640, y2: point_by + 360, r: 255, g: 255, b: 255 }
	#$gtk.args.outputs.lines << { x: cx + 640, y: cy + 360, x2: cx2 + 640, y2: cy2 + 360, r: 255, g: 0, b: 255 }
	end

	if (point_ay - point_cy)(point_cx + point_ax) > (point_ax - point_cx)(point_cy + point_ay) &&
	(point_cy - point_by)(point_bx + point_cx) > (point_cx - point_bx)(point_by + point_cy)
	# The triangle three points contains the origin so we're overlapping
	# Begin EPA algorithm
	# For now, just return true that there was a collision
	$gtk.args.outputs.labels << { x: 10, y: 600, text: 'EPA', r: 255, g: 255, b: 255 }
	break
	else
	# check axis point_a, point_b, point_c, n
	if (point_cxdx + point_cydx) <= [(point_axdx + point_aydy), (point_bxdx + point_bydy)].max
	break
	else
	# ClosestDist point_a, point_c < ClosestDistance point_c, point_b

	# Closest_t for point_a, point_c
	sx = point_cx - point_ax
	sy = point_cy - point_ay
	t = -((sx(point_ax + point_bx) + sy(point_ay + point_by)) / (sxsx + sysy)).clamp(-1.0, 1.0)
	t *= 0.5
	# lerp point_a, point_c
	ac_x = (point_ax(0.5 - t)) + (point_cx(0.5 + t))
	ac_y = (point_ay(0.5 - t)) + (point_cy(0.5 + t))
	ac_dist = ac_xac_x + ac_yac_y

	# closest_t for point_c, point_b
	sx = point_bx - point_cx
	sy = point_bx - point_cx
	t = -((sx(point_ax + point_bx) + sy(point_ay + point_by)) / (sxsx + sysy)).clamp(-1.0, 1.0)
	t *= 0.5
	# lerp point_c, point_b
	cb_x = (point_cx(0.5 - t)) + (point_bx(0.5 + t))
	cb_y = (point_cy(0.5 - t)) + (point_by(0.5 + t))
	cb_dist = cb_xcb_x + cb_ycb_y

	if ac_dist < cb_dist
	point_bx = point_cx
	point_by = point_cy
	else
	point_ax = point_cx
	point_ay = point_cy
	end
	end
	end

	i += 1
	end

	if @debug_draw
	$gtk.args.outputs.lines << { x: point_ax+640, y: point_ay+360, x2: point_bx+640, y2: point_by+360, r: 255, g: 0, b: 0 }
	end
	# For now just return false if we got here
	false
	end

	def calc_collision collision
	end
	end
	end