Convex Collision

require 'gosu'
require 'matrix'

class Green < Gosu::Window

  def initialize
    super 640, 480, false
    self.caption = "Green Physics"

    @box1 = Polygon.new(
        vertices: [Vector[0, 0],
                   Vector[50, -50],
                   Vector[65, 0],
                   Vector[65, 75],
                   Vector[0, 65]],
        position: Vector[100, 100],
        angle: 1,
        velocity: Vector[0.0, 0.0]
    )

    @box2 = Polygon.new(
        vertices: [Vector[0, 0],
                   Vector[10, -10],
                   Vector[25, 0],
                   Vector[25, 25],
                   Vector[0, 25]],
                   position: Vector[100, 100],
                   angle: 1
    )
  end

  def update
    input

    w1, w2 = @box1.world, @box2.world
    polys, side, mins = Polygon.collides?(@box1.world, @box2.world)
    unless side.nil?
      v1, v2 = polys[0][side[0]], polys[0][side[1]]
      perp = Vector[v2.y - v1.y, v1.x - v2.x]
    end

    @box1.update(1)
    @box2.update(1)
  end

  def input
    if button_down?(Gosu::KbEscape)
      exit
    end

    if button_down?(Gosu::KbDown)
      @box2.position.y += 1
    end

    if button_down?(Gosu::KbUp)
      @box2.position.y -= 1
    end

    if button_down?(Gosu::KbRight)
      @box2.position.x += 1
    end

    if button_down?(Gosu::KbLeft)
      @box2.position.x -= 1
    end

    if button_down?(Gosu::KbR)
      @box2.angle += 0.1
    end
  end

  def draw
    @box1.draw(self)
    @box2.draw(self)

    w1, w2 = @box1.world, @box2.world
    polys, side, mins = Polygon.collides?(@box1.world, @box2.world)
    unless side.nil?
      v1, v2 = polys[0][side[0]], polys[0][side[1]]
      perp = Vector[v2.y - v1.y, v1.x - v2.x] * 10
      draw_line(@box1.position.x, @box1.position.y, Gosu::Color::RED,
                @box1.position.x + perp.x, @box1.position.y + perp.y, Gosu::Color::RED)
      perp = Vector[v2.y - v1.y, v1.x - v2.x] * 10
      draw_line(@box2.position.x, @box2.position.y, Gosu::Color::BLUE,
                @box2.position.x + perp.x, @box2.position.y + perp.y, Gosu::Color::BLUE)

      mag = (v1 - v2).magnitude
      dv1 = 50*(v1 - v2)/mag + v1
      dv2 = 50*(v2 - v1)/mag + v2
      draw_line(dv1.x, dv1.y, Gosu::Color::BLUE,
                dv2.x, dv2.y, Gosu::Color::BLUE)
    end

    #w1, w2 = @box1.world, @box2.world
    #polys, side, mins = Polygon.collides?(@box1.world, @box2.world)
    #unless side.nil?
    #  v1, v2 = polys[0][side[0]], polys[0][side[1]]
    #
    #  mag = (v1 - v2).magnitude
    #  dv1 = 50*(v1 - v2)/mag + v1
    #  dv2 = 50*(v2 - v1)/mag + v2
    #
    #  draw_line(dv1.x, dv1.y, Gosu::Color::BLUE,
    #            dv2.x, dv2.y, Gosu::Color::BLUE)
    #
    #  mins.each do |i|
    #
    #    p = polys[1][i]
    #    draw_quad(
    #        p.x - 5, p.y - 5, Gosu::Color::BLUE,
    #        p.x + 5, p.y - 5, Gosu::Color::BLUE,
    #        p.x + 5, p.y + 5, Gosu::Color::BLUE,
    #        p.x - 5, p.y + 5, Gosu::Color::BLUE
    #    )
    #  end
    #end

  end

  def needs_cursor?
    true
  end

end

class Matrix

  def self.rotate(angle)
    Matrix[
        [Math.cos(angle), -Math.sin(angle)],
        [Math.sin(angle), Math.cos(angle)]
    ]
  end

end

class Vector

  def x
    self[0]
  end

  def x=(v)
    self[0] = v
  end

  def y
    self[1]
  end

  def y=(v)
    self[1] = v
  end

end

class Polygon

  attr_accessor :vertices, :color, :mass,
                :position, :velocity,
                :angle, :angular_velocity,
                :center_of_mass

  def initialize(args)
    {
        color: Gosu::Color::WHITE,
        mass: 1,
        position: Vector[0, 0],
        velocity: Vector[0, 0],
        angle: 0,
        angular_velocity: 0
    }.merge(args).each { |k, v| send(:"#{k}=", v) }

    center_of_mass = vertices.reduce(:+) / vertices.size
    vertices.map! { |v| v - center_of_mass }
  end

  def update(dt)
    @position += velocity * dt
    @angle += angular_velocity * dt
  end

  def draw(gosu)
    w = world

    lines = w.each_cons(2).to_a << [w[0], w[-1]]
    lines.each do |l1, l2|
      gosu.draw_triangle(
          l1.x, l1.y, color,
          l2.x, l2.y, color,
          position.x, position.y, color
      )
    end

  end

  def world
    rot = Matrix.rotate(angle)
    vertices.map do |v|
      rot*v + position
    end
  end

  def impulse(v)
    velocity += v
  end

  def self.collides?(p1, p2)
    line1, projection1 = dividing_line(p1, p2)
    line2, projection2 = dividing_line(p2, p1)

    return nil if projection1.nil? and projection2.nil?
    min1 = projection1.nil? ? Float::INFINITY : projection1.min
    min2 = projection2.nil? ? Float::INFINITY : projection2.min

    if min1 < min2
      mins = projection1.size.times.select { |i| projection1[i] == min1 }
      return [p1, p2], line1, mins
    else
      mins = projection2.size.times.select { |i| projection2[i] == min2 }
      return [p2, p1], line2, mins
    end

    return nil
  end

  def self.dividing_line(p1, p2)

    lines = (0..p1.size).map{ |e| e % p1.size }.each_cons(2)
    lines.each do |i, j|
      v1, v2, = p1[i], p1[j]
      perp = Vector[v2.y - v1.y, v1.x - v2.x].normalize
      origin = perp.inner_product(v1)
      projection = p2.map { |v| perp.inner_product(v) - origin }
      divides = projection.all? { |e| e > 0 } # I can put in slack for the collision detector here
      return [i, j], projection if divides
    end

    return nil
  end

end

window = Green.new
window.show