物理エンジンを作る2

butsuri.rb

require 'dxruby'

module ComplexVector
  refine Complex do
    def normalize
      self / self.magnitude
    end

    def dot(c)
      self.real * c.real + self.imag * c.imag
    end
  end
end

# 物理演算空間
class PhysicsWorld
  using ComplexVector

  attr_reader :objects, :hit_pair

  def initialize
    @objects = []
    @hit_pair = []
  end

  # Worldにオブジェクトを追加
  def add(o)
    o.world = self
    @objects << o
  end

  # 1フレーム分進める
  def step
    # 更新
    Sprite.update(@objects)

    # 衝突検出はDXRubyにやらせる
    @hit_pair.clear
    Sprite.check(@objects)

    # 衝突解決
    collision_resolve

    # 描画と後処理
    Sprite.draw(@objects)
    Sprite.clean(@objects)
  end

  # 衝突解決
  def collision_resolve
    @hit_pair.each do |ary|
      o1, o2 = *ary

      # 当たってなかったら何もしない
      unless o1 === o2
        return
      end

      # 1フレーム前まで戻す
      toxy1 = o1.xy
      toa1  = o1.angle
      toxy2 = o2.xy
      toa2  = o2.angle

      o1.xy    -= o1.v
      o1.angle -= o1.av
      o2.xy    -= o2.v
      o2.angle -= o2.av

      # ちょっとずつ進めて当たった時間を求める
      tmp = [o1.v.magnitude, o2.v.magnitude].max
      time = 0
      flag = true
      1.upto(tmp.to_i) do |i|
        time = 1.0 / tmp * i
        o1.xy    += o1.v * (1.0 / tmp)
        o1.angle += o1.av * (1.0 / tmp)
        o2.xy    += o2.v * (1.0 / tmp)
        o2.angle += o2.av * (1.0 / tmp)

        if o1 === o2
          flag = false
          break
        end
      end

      # 当たらなかったので元の状態に戻す(これは当たってるはず)
      if flag
        time = 1
        o1.xy    = toxy1
        o1.angle = toa1
        o2.xy    = toxy2
        o2.angle = toa2
      end

      normal, depth = # 作用法線、衝突深さ
      if o1.collision.size == 4 and o2.collision.size == 4 # 四角と四角
        test_obb_obb(ary)
      end

      # 当たってない位置まで戻す補正処理
      o1.xy -= normal * depth / (1.0 / o1.mass + 1.0 / o2.mass) * (1.0 / o1.mass)
      o2.xy += normal * depth / (1.0 / o1.mass + 1.0 / o2.mass) * (1.0 / o2.mass)

      # 撃力を加える
      apply_impulse(ary, normal)

      # 残りの時間ぶん進める
      time = 1 - time
      o1.xy    += o1.v * time
      o1.angle += o1.av * time
      o2.xy    += o2.v * time
      o2.angle += o2.av * time
    end
  end

  # とりあえず回転しない四角と四角の判定
  def test_obb_obb(ary)
    o1, o2 = *ary
    depth = Float::INFINITY
    normal = nil

    [[o1.x, o2.x+o2.w, -1+0i], [o2.x, o1.x+o1.w, 1+0i], [o1.y, o2.y+o2.h, -1i], [o2.y, o1.y+o1.h, 1i]].each do |t|
      if t[0] < t[1]
        if t[1] - t[0] < depth
          depth = t[1] - t[0]
          normal = t[2]
        end
      end
    end

    [normal, depth]
  end

  # 撃力を加える(とりあえず止める)
  def apply_impulse(ary, normal)
    ary[0].v -= normal.dot(ary[0].v) * normal
    ary[1].v -= normal.dot(ary[1].v) * normal
  end
end

# 物理演算用Sprite
class PhysicsSprite < Sprite
  attr_accessor :v, :mass, :e, :av, :moment, :f, :force, :world

  def xy
    Complex(self.x, self.y)
  end

  def xy=(c)
    self.x, self.y = c.rect
  end

  def initialize(*)
    @v = 0      # 速度
    @mass = 1   # 質量(適当な値)
    @e = 1      # 反発係数(とりあえずMAX)
    @av = 0     # 角速度
    @moment = 1 # 慣性モーメント(適当な値)
    @f = 0      # 摩擦
    @force = {} # 力
    super
  end

  # 毎フレームの計算
  def update
    # 登録されている力を加える
    @force.each do |key, value|
      @v += value
    end

    # 移動と回転
    self.x += @v.real
    self.y += @v.imag
    self.angle += @av

    super
  end

  # 衝突したペアを保存する
  def hit(o)
    id1 = self.object_id
    id2 = o.object_id
    t = id1 < id2 ? [self, o] : [o, self]
    world.hit_pair << t unless world.hit_pair.include?(t)
  end
end

# 四角
class PhysicsBox < PhysicsSprite
  attr_accessor :w, :h
  def initialize(x, y, w, h, image=Image.new(w, h, C_WHITE))
    super(x, y, image)
    self.collision = [0, 0, w-1, h-1]   # 衝突判定範囲
    @w = w
    @h = h
    @mass = w * h                       # 質量
    @moment = @mass * (w * w + h * h) / 12.0 # 慣性モーメント
  end
end

world = PhysicsWorld.new

s1 = PhysicsBox.new(300, 0, 20, 20)
s1.force[:gravity] = 0.1i # 重力加速度を設定
world.add(s1)

#　床
s2 = PhysicsBox.new(0, 460, 640, 20)
s2.mass = Float::INFINITY
world.add(s2)

# 空中の床
s3 = PhysicsBox.new(220, 360, 200, 20)
s3.mass = Float::INFINITY
world.add(s3)

# 空中の壁
s4 = PhysicsBox.new(200, 340, 20, 40)
s4.mass = Float::INFINITY
world.add(s4)

# 空中の壁
s5 = PhysicsBox.new(420, 340, 20, 40)
s5.mass = Float::INFINITY
world.add(s5)

Window.loop do
  s1.v = Input.x * 3 + s1.v.imag * 1i
  s1.v = s1.v.real - 4i if Input.key_push?(K_Z)
  world.step
  break if Input.key_push?(K_ESCAPE)
end