xenobrain · October 17, 2024 14:18 · Oct 17, 2024 · Apr 19, 2023
diff --git a/gjk.rb b/gjk.rb
@@ -0,0 +1,331 @@
+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
@@ -0,0 +1,332 @@
+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
@@ -0,0 +1,364 @@
+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