aim_and_shoot.rb

require 'matrix'
require 'byebug'
require 'rspec'

include Math

def sqr(x)
  x * x
end

def vector(*parts)
  parts[3] = 1.0
  Matrix.rows(parts.map { |c| [c.to_f] })
end

def translate(x, y, z)
  Matrix.rows([
    [1, 0, 0, x],
    [0, 1, 0, y],
    [0, 0, 1, z],
    [0, 0, 0, 1]])
end

def rotate_x(a)
  Matrix.rows([
    [1, 0,       0,        0],
    [0, cos(-a), -sin(-a), 0],
    [0, sin(-a), cos(-a),  0],
    [0, 0,       0,        1]
  ])
end

def rotate_y(a)
  Matrix.rows([
    [cos(-a),  0,       sin(-a),  0],
    [0,        1,       0,        0],
    [-sin(-a), 0,       cos(-a),  0],
    [0,        0,       0,        1]
  ])
end

def rotate_z(a)
  Matrix.rows([
    [cos(-a), -sin(-a), 0, 0],
    [sin(-a), cos(-a),  0, 0],
    [0,       0,        1, 0],
    [0,       0,        0, 1]
  ])
end


def aim(pos_x, pos_y, pos_z, target_x, target_y, target_z, yaw, pitch)
  m_player = rotate_z(-yaw / 180.0 * Math::PI) * translate(-pos_x, -pos_y, -pos_z)
  target_relative = m_player * vector(target_x, target_y, target_z)
  distance = target_relative[1, 0]
  off_center = Math.sqrt(sqr(target_relative[0, 0]) + sqr(target_relative[2, 0]))
  { distance: distance, off_center: off_center }
end

RSpec.describe "aiming" do
  it "shoud hit right => left" do
    result = aim(10, 0, 0,
                 0, 0, 0,
                 270, 0)
    expect(result[:distance].round).to eq 10
    expect(result[:off_center].round).to eq 0
  end

  it "shoud hit left => right" do
    result = aim(-10, 0, 0,
                 0, 0, 0,
                 90, 0)
    expect(result[:distance].round).to eq 10
    expect(result[:off_center].round).to eq 0
  end

  it "shoud hit north => south" do
    result = aim(0, 10, 0,
                 0, 0, 0,
                 180, 0)
    expect(result[:distance].round).to eq 10
    expect(result[:off_center].round).to eq 0
  end

  it "shoud hit south => north" do
    result = aim(0, -10, 0,
                 0, 0, 0,
                 0, 0)
    expect(result[:distance].round).to eq 10
    expect(result[:off_center].round).to eq 0
  end
end