物理エンジンを作る3

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

    def cross(c)
      self.real * c.imag - self.imag * c.real
    end

    def prep
      Complex(-self.imag, self.real)
    end

    def rotate(ang)
      Complex.polar(self.magnitude, self.angle + ang * Math::PI / 180)
    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, (o1.xy + o2.xy) / 2)

      # 残りの時間ぶん進める
      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-o1.w/2, o2.x+o2.w/2, -1+0i], [o2.x-o2.w/2, o1.x+o1.w/2, 1+0i], [o1.y-o1.h/2, o2.y+o2.h/2, -1i], [o2.y-o2.h/2, o1.y+o1.h/2, 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

  # 点が四角の中にあるかどうかを調べる
  # Sprite#collisionを自分で設定するとうまくいかないだろうから後でなおす
  def check_obb_point(o1, p1)
    p1 = p1 - o1.xy - o1.center_xy
    p1 = p1.rotate(-o1.angle)
    w, h = o1.collision[2], o1.collision[3]
    return (p1.real.abs < w and p1.imag.abs < h)
  end

  # 撃力を加える
  def apply_impulse(ary, normal, cp)
    o1, o2 = *ary

    vn = normal.dot(o1.v - o2.v) # 相対速度
    cp1 = cp - o1.xy
    cp2 = cp - o2.xy

    # 撃力計算
    j = (-(1+o1.e*o2.e) * vn) /
        ((1.0/o1.mass + 1.0/o2.mass) +
        normal.dot(cp1.prep * (cp1.cross(normal) / o1.moment)) + 
        normal.dot(cp2.prep * (cp2.cross(normal) / o2.moment)))

    # 速度への反映
    o1.v += normal * j / o1.mass
    o2.v -= normal * j / o2.mass

    # 回転への反映
    o1.av += cp1.cross(normal * j) / o1.moment
    o2.av += cp2.cross(normal * j) / o2.moment
  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 center_xy
    Complex(self.center_x, self.center_y)
  end

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

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

    # 移動と回転
    self.xy += @v
    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.x += self.center_x
    self.y += self.center_y
    self.offset_sync = true
    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(100, 0, 20, 20)
s1.force[:gravity] = 0.1i # 重力加速度を設定
s1.moment = Float::INFINITY  #慣性モーメント
world.add(s1)

s1 = PhysicsBox.new(300, 0, 20, 20)
s1.force[:gravity] = 0.1i # 重力加速度を設定
s1.moment = Float::INFINITY  #慣性モーメント
s1.e = 0.5 # 反発係数
world.add(s1)

#　床
s2 = PhysicsBox.new(0, 460, 640, 20)
s2.mass = Float::INFINITY
s2.moment = Float::INFINITY  #慣性モーメント
world.add(s2)

# 空中の床
s3 = PhysicsBox.new(220, 360, 200, 20)
s3.mass = Float::INFINITY
s3.moment = Float::INFINITY  #慣性モーメント
world.add(s3)

# 空中の壁
s4 = PhysicsBox.new(200, 340, 20, 40)
s4.mass = Float::INFINITY
s4.moment = Float::INFINITY  #慣性モーメント
world.add(s4)

# 空中の壁
s5 = PhysicsBox.new(420, 340, 20, 40)
s5.mass = Float::INFINITY
s5.moment = 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