物理エンジンを作る4

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 perp
      Complex(-self.imag, self.real)
    end

    def rperp
      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
  EPS = 0.00001

  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

      d = # 作用線、衝突深さ、衝突点の配列
      if o1.collision.size == 4 and o2.collision.size == 4 # 四角と四角
        test_obb_obb(o1, o2)
      end

      return if d.size == 0

      normal, depth, _ = d.max{|ary|ary[1]}

      # 当たってない位置まで戻す補正処理
      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)

      # 撃力を加える
      d.each do |ary|
        apply_impulse(o1, o2, ary[0], ary[2])
      end

      # 残りの時間ぶん進める
      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(o1, o2)
      result = []
  
      # 頂点をそれぞれ計算
      p11 = (Complex(o1.collision[0]  , o1.collision[1]  ) - o1.center_xy).rotate(o1.angle) + o1.xy + (o1.offset_sync ? 0 : o1.center_xy)
      p12 = (Complex(o1.collision[2]+1, o1.collision[1]  ) - o1.center_xy).rotate(o1.angle) + o1.xy + (o1.offset_sync ? 0 : o1.center_xy)
      p13 = (Complex(o1.collision[2]+1, o1.collision[3]+1) - o1.center_xy).rotate(o1.angle) + o1.xy + (o1.offset_sync ? 0 : o1.center_xy)
      p14 = (Complex(o1.collision[0]  , o1.collision[3]+1) - o1.center_xy).rotate(o1.angle) + o1.xy + (o1.offset_sync ? 0 : o1.center_xy)
      p21 = (Complex(o2.collision[0]  , o2.collision[1]  ) - o2.center_xy).rotate(o2.angle) + o2.xy + (o2.offset_sync ? 0 : o2.center_xy)
      p22 = (Complex(o2.collision[2]+1, o2.collision[1]  ) - o2.center_xy).rotate(o2.angle) + o2.xy + (o2.offset_sync ? 0 : o2.center_xy)
      p23 = (Complex(o2.collision[2]+1, o2.collision[3]+1) - o2.center_xy).rotate(o2.angle) + o2.xy + (o2.offset_sync ? 0 : o2.center_xy)
      p24 = (Complex(o2.collision[0]  , o2.collision[3]+1) - o2.center_xy).rotate(o2.angle) + o2.xy + (o2.offset_sync ? 0 : o2.center_xy)
  
      # 重心から頂点までの線分と交差する相手の辺を求め、作用線、深さ、衝突位置を取得する
      o1_center = o1.xy + (o1.offset_sync ? 0 : o1.center_xy)
      [p11, p12, p13, p14].each do |point|
        [[p21, p22], [p22, p23], [p23, p24], [p24, p21]].each do |line|
          if intersection(o1_center, point, line[0], line[1])
            normal = (line[1] - line[0]).rperp.normalize
            depth = Complex.polar((point - line[0]).magnitude, (point - line[0]).angle - (line[1] - line[0]).angle).imag
            if depth > 0
              result << [-normal, depth, point]
            end
          end
        end
      end
  
      o2_center = o2.xy + (o2.offset_sync ? 0 : o2.center_xy)
      [p21, p22, p23, p24].each do |point|
        [[p11, p12], [p12, p13], [p13, p14], [p14, p11]].each do |line|
          if intersection(o2_center, point, line[0], line[1])
            normal = (line[1] - line[0]).rperp.normalize
            depth = Complex.polar((point - line[0]).magnitude, (point - line[0]).angle - (line[1] - line[0]).angle).imag
            if depth > 0
              result << [normal, depth, point]
            end
          end
        end
      end

    result
  end

  # 線分の衝突判定
  def intersection(a1, a2, b1, b2)
    (a2-a1).cross(b1-a1) * (a2-a1).cross(b2-a1) < EPS and (b2-b1).cross(a1-b1) * (b2-b1).cross(a2-b1) < EPS
  end

  # 撃力を加える
  def apply_impulse(o1, o2, normal, cp)
    vn = normal.dot(o1.v - o2.v).magnitude # 法線方向の相対速度
    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.perp * (cp1.cross(normal) / o1.moment)) + 
        normal.dot(cp2.perp * (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 / Math::PI * 180
    o2.av -= cp2.cross(normal * j) / o2.moment / Math::PI * 180
  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 = 1      # 反発係数(とりあえず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, 190, 60, 60)
s1.v = 2
s1.e = 0.0 # 反発係数
world.add(s1)

s2 = PhysicsBox.new(500, 230, 40, 40)
s2.v = -2
s2.e = 0.0 # 反発係数
world.add(s2)

s3 = PhysicsBox.new(300, 400, 40, 40)
s3.v = -2i
s3.e = 0.0 # 反発係数
world.add(s3)

Window.loop do
  world.step
  break if Input.key_push?(K_ESCAPE)
end