🦠

Soda Constructor のデータ構造とアルゴリズム

2023/11/15に公開

Ruby

Soda Constructor を元にした Open Constructor のソースコードから学んだデータ構造とアルゴリズムについて簡単に文書化しておく。

質量 (Node) クラス

attribute	意味
location	位置
velocity	速度
acceleration	加速度

頂点の部分を表わす。Open Constructor では MASS クラスとしている。しかし、そのクラスの中に mass 属性が存在してややこしかったのでコードとしては抽象的な名前 Node に変更した。

筋肉 (Spring) クラス

attribute	意味
node_a	Nodeへの参照
node_b	Nodeへの参照
rest_length	静止長
amplitude	振幅
phase	位相

線の部分を表わす。一般的には「ばね」だが Soda Constructor の世界では伸び縮みする「筋肉」と称している。その筋肉を人間の筋肉に当てはめてしまうと骨があるせいで不自然に感じる。そこはヘビのような動物をイメージした方がよいかもしれない。

質量と筋肉の接続方法

a = Node.new
b = Node.new
s = Spring.new do |o|
  o.node_a = a
  o.node_b = b
end

筋肉の両端に質量を繋ぐ。そのポインタは筋肉が保持している。

以前にばねの実験をしたときは、疎結合化を意識しすぎて接続部分を別クラスとしたのだけどそれはいまいちだった。密結合でも自然に扱えたらそれでよい。

筋肉を動かすもっとも重要な部分

伸び縮みする静止長(スカラー)

def dynamic_rest_length
  theta = @phase + $model.wave_phase
  @rest_length * (1 - Math.sin(theta) * @amplitude * $model.wave_amplitude)
end

バネ定数を考慮した筋肉の力(ベクトル)

def stiffness_force
  gap = @node_b.location - @node_a.location
  stretch = gap.length - dynamic_rest_length
  gap.normalize_or_zero * stretch * $model.stiffness
end

筋肉の力を接続する二つの質量に伝える

def stiffness_force_apply
  force = stiffness_force
  @node_a.force += force
  @node_b.force -= force
end

伸び縮みする静止長を求める部分が筋肉ごとに「静止長」「振幅」「位相」を持っているため、各筋肉でずれが生じる。それが地面の摩擦と関係して歩行に繋がる。

コード

開く

require "gosu"
require "#{__dir__}/../../物理/ベクトル/vec2"

class Node
  attr_accessor :location
  attr_accessor :velocity
  attr_accessor :acceleration
  attr_accessor :force

  def initialize
    if block_given?
      yield self
    end

    @location ||= V.zero
    @velocity ||= V.zero
    @acceleration ||= V.zero
    @force ||= V.zero
  end

  def force_clear
    @force *= 0.0
  end

  def gravity_force_apply
    @force += $model.gravity
  end

  def friction_force_apply
    @force += @velocity * -$model.friction
  end

  def integrate(dt = 1.0)
    @location += @velocity * dt
    @velocity += @acceleration * dt
    @acceleration = @force
  end

  def collide
    collide_ground
    collide_wall
  end

  private

  def collide_ground
    if @location.y < 0
      @location.y = 0.0
      @velocity *= V[1.0 - $model.surface_friction, $model.surface_reflection]
    end
  end

  def collide_wall
    if @location.x < 0 || @location.x > $model.width
      if @location.x < 0
        @location.x = 0.0
      else
        @location.x = $model.width
      end
      @velocity *= V[$model.surface_reflection, 1.0 - $model.surface_friction]
      if @location.x <= 0
        if $model.last_wall != :left
          $model.last_wall = :left
          $model.wave_direction *= -1
        end
      else
        if $model.last_wall != :right
          $model.last_wall = :right
          $model.wave_direction *= -1
        end
      end
    end
  end
end

class Spring
  attr_accessor :node_a
  attr_accessor :node_b
  attr_accessor :rest_length
  attr_accessor :amplitude
  attr_accessor :phase

  def initialize
    if block_given?
      yield self
    end

    @amplitude ||= 0.0
    @phase ||= 0.0
    @rest_length ||= (node_b.location - node_a.location).length
  end

  def nodes
    [node_a, node_b]
  end

  def spring_exist?(node_a, node_b)
    (@node_a == node_a && @node_b == node_b) || (@node_a == node_b && @node_b == node_a)
  end

  def middle_location
    (node_a.location + node_b.location).fdiv(2)
  end

  def stiffness_force_apply
    force = stiffness_force
    node_a.force += force
    node_b.force -= force
  end

  def stiffness_force
    gap = node_b.location - node_a.location
    stretch = gap.length - dynamic_rest_length
    gap.normalize_or_zero * stretch * $model.stiffness
  end

  def dynamic_rest_length
    theta = @phase + $model.wave_phase
    @rest_length * (1 - Math.sin(theta) * @amplitude * $model.wave_amplitude)
  end
end

class Model
  attr_accessor :friction
  attr_accessor :stiffness
  attr_accessor :surface_friction
  attr_accessor :surface_reflection

  attr_accessor :last_wall

  attr_accessor :nodes
  attr_accessor :springs

  attr_accessor :wave_amplitude
  attr_accessor :wave_phase
  attr_accessor :wave_speed
  attr_accessor :wave_direction

  attr_accessor :gravity

  attr_accessor :width
  attr_accessor :height

  def initialize
    $model = self

    @last_wall = :unknown
  end

  def spring_exist?(...)
    @springs.any? { |e| e.spring_exist?(...) }
  end

  def import(hv)
    @gravity = V[*hv[:gravity]]
    @friction = hv[:friction]
    @stiffness = hv[:stiffness]

    @width  = hv[:width]
    @height = hv[:height]

    @surface_friction   = hv[:surface_friction]
    @surface_reflection = hv[:surface_reflection]

    @wave_amplitude = hv[:wave_amplitude]
    @wave_phase     = hv[:wave_phase]
    @wave_speed     = hv[:wave_speed]
    @wave_direction = hv[:wave_direction]

    @nodes = []
    hv[:nodes].each do |e|
      new_node = Node.new do |o|
        o.location     = V[*e[:location]]
        o.velocity     = V[*e[:velocity]]
        o.acceleration = V[*e[:acceleration]]
      end
      node_add(new_node)
    end

    @springs = []
    hv[:springs].each do |e|
      new_spring = Spring.new do |o|
        o.node_a = @nodes[e[:node_a]]
        o.node_b = @nodes[e[:node_b]]
        o.rest_length = e[:rest_length]
        o.amplitude = e[:amplitude]
        o.phase = e[:phase]
      end
      spring_add(new_spring)
    end
  end

  def node_add(node)
    if @nodes.include?(node)
      return
    end

    @nodes << node
  end

  def spring_add(s)
    if spring_exist?(s.node_a, s.node_b)
      return
    end
    if @springs.include?(s)
      return
    end

    node_add(s.node_a)
    node_add(s.node_b)
    @springs << s
  end

  def wh
    V[width, height]
  end

  def wh=(value)
    @width, @height = *value
  end

  def force_clear
    nodes.each(&:force_clear)
  end

  def gravity_force_apply
    nodes.each(&:gravity_force_apply)
  end

  def friction_force_apply
    nodes.each(&:friction_force_apply)
  end

  def stiffness_force_apply
    springs.each(&:stiffness_force_apply)
  end

  def integrate(dt = 1.0)
    nodes.each { |e| e.integrate(dt) }
  end

  def collide
    nodes.each(&:collide)
  end

  def subtick(dt = 1.0)
    force_clear
    gravity_force_apply
    friction_force_apply
    stiffness_force_apply
    integrate(dt)
    collide
  end

  def tick(dt = 1.0)
    @wave_phase += wave_speed * wave_direction

    k_frameskip = 10
    k_oversampling = 10

    k_frameskip.times do
      k_oversampling.times do
        subtick(dt / k_oversampling)
      end
    end
  end
end

class Window < Gosu::Window
  PADDING = V.splat(32)
  PIXEL_PER_METER = 240.0

  def initialize
    $window = self

    $model = Model.new
    $model.import(DaintyWalker)

    window_size = $model.wh * PIXEL_PER_METER + PADDING * 2
    super(*window_size.ceil, resizable: true, borderless: false)

    @model_panel = ModelPanel.new(PADDING, $model.wh * PIXEL_PER_METER)
  end

  def button_down(id)
    if id == Gosu::KB_ESCAPE || id == Gosu::KB_Q
      close
    end
  end

  def draw
    Gosu.draw_rect(0, 0, *wh, Gosu::Color::WHITE)
    $model.tick(1.0 / 60)
    @model_panel.draw
  end

  def line_draw(v0, v1)
    Gosu.draw_line(*v0, Gosu::Color::BLACK, *v1, Gosu::Color::BLACK)
  end

  def point_draw(v, radius: 1)
    rect_draw(v, V[radius, radius] * 2)
  end

  def rect_draw(xy, wh, color: Gosu::Color::BLACK, fill: true, center: true)
    if center
      xy = xy - wh / 2
    end
    if fill
      Gosu.draw_rect(*xy, *wh, color, 0)
    else
      line_draw(xy, xy + wh * V.x)
      line_draw(xy, xy + wh * V.y)
      line_draw(xy + wh * V.y, xy + wh * V.y + wh * V.x)
      line_draw(xy + wh * V.x, xy + wh * V.y + wh * V.x)
    end
  end

  def wh
    V[width, height]
  end
end

class ModelPanel
  attr_accessor :xy
  attr_accessor :wh

  def initialize(xy, wh)
    @xy = xy
    @wh = wh
  end

  def draw
    $window.rect_draw(xy, wh, center: false, fill: false)
    adjust_to_resize
    springs_draw
    nodes_draw
  end

  private

  def adjust_to_resize
    $model.wh = ($window.wh - Window::PADDING * 2) / Window::PIXEL_PER_METER
    @wh = $model.wh * Window::PIXEL_PER_METER
  end

  def nodes_draw
    $model.nodes.each do |node|
      $window.point_draw(meter_to_px(node.location), radius: 4)
    end
  end

  def springs_draw
    $model.springs.each do |spring|
      p1 = meter_to_px(spring.node_a.location)
      p2 = meter_to_px(spring.node_b.location)
      $window.line_draw(p1, p2)
      if spring.amplitude.nonzero?
        $window.point_draw(meter_to_px(spring.middle_location), radius: 2)
      end
    end
  end

  def meter_to_px(v)
    v *= Window::PIXEL_PER_METER
    xy + V[v.x, wh.y - v.y]
  end
end

DaintyWalker = {
  :name               => "Dainty Walker",
  :author             => "ed",
  :gravity            => [0.0, -0.02],
  :friction           => 0.137,
  :stiffness          => 6.25,
  :width              => 4.0,
  :height             => 2.0,
  :surface_friction   => 0.7,
  :surface_reflection => -0.75,
  :wave_amplitude     => 0.15,
  :wave_phase         => 0.11999999999999994,
  :wave_speed         => 0.06,
  :wave_direction     => 1,
  :nodes => [
    {
      :location     => [ 1.750499570147691, 0.7554135719641994,         ],
      :velocity     => [ 0.010396670975938842, -0.018675110524989408,   ],
      :acceleration => [ -0.10832455506214488, 0.02183982923269993,     ],
    }, {
      :location     => [ 0.8143431867485943, 0.7700184931721614,        ],
      :velocity     => [ 0.04949556383183008, 0.037794087938637824,     ],
      :acceleration => [ 0.08453605287430359, 0.055288345204614606,     ],
    }, {
      :location     => [ 0.23655202730264485, 0.7478260534766035,       ],
      :velocity     => [ 0.028459931475753632, 0.05439471763022323,     ],
      :acceleration => [ 0.05909228245379487, -0.039285960711164455,    ],
    }, {
      :location     => [ 0.6806472181818575, 3.7779324789727294e-06,    ],
      :velocity     => [ -0.0012136445168939938, 0.0002321729963490406, ],
      :acceleration => [ -0.07702209677372815, -0.07899589456459076,    ],
    }, {
      :location     => [ 1.2989493048509524, 0.4972857598677362,        ],
      :velocity     => [ 0.01797957799060217, -0.0005710022945987695,   ],
      :acceleration => [ -0.060333392387647636, 0.014125828193676991,   ],
    }, {
      :location     => [ 1.8915955542220217, 1.1347315654083353e-05,    ],
      :velocity     => [ -0.001128946260354464, -0.0010026689943898961, ],
      :acceleration => [ -0.027036558726106233, -0.12011654679321412,   ],
    }, {
      :location     => [ 1.2815119546481435, 1.0679988270511467,        ],
      :velocity     => [ 0.02667375491522965, 0.01031826571925057,      ],
      :acceleration => [ 0.049838411387536065, -0.027762683976993573,   ],
    }, {
      :location     => [ 2.083791940539898, 0.03548767659775574,        ],
      :velocity     => [ 0.06247664777444883, -0.01755919659225326,     ],
      :acceleration => [ -0.011545748498310988, 0.020560497911721754,   ],
    }, {
      :location     => [ 0.4654908270515556, 0.06018344762330983,       ],
      :velocity     => [ 0.08776820716971893, 0.02234455503877207,      ],
      :acceleration => [ 0.021099128204172018, -0.07956410867437957,    ],
    }, {
      :location     => [ 2.298090391126824, 0.740384683876986,          ],
      :velocity     => [ 0.03059693265944378, -0.021300429366038764,    ],
      :acceleration => [ 0.03448981404102219, 0.02641638947583707,      ],
    },
  ],
  :springs => [
    { :node_a => 9, :node_b => 0, :rest_length => 0.55,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 0, :node_b => 1, :rest_length => 0.93,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 1, :node_b => 2, :rest_length => 0.58,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 1, :node_b => 4, :rest_length => 0.546, :amplitude => 0,   :phase => 0,        },
    { :node_a => 4, :node_b => 0, :rest_length => 0.519, :amplitude => 0,   :phase => 0,        },
    { :node_a => 0, :node_b => 6, :rest_length => 0.555, :amplitude => 0,   :phase => 0,        },
    { :node_a => 6, :node_b => 1, :rest_length => 0.563, :amplitude => 0,   :phase => 0,        },
    { :node_a => 6, :node_b => 4, :rest_length => 0.57,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 4, :node_b => 2, :rest_length => 1.09,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 2, :node_b => 6, :rest_length => 1.09,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 6, :node_b => 9, :rest_length => 1.07,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 9, :node_b => 4, :rest_length => 1.03,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 9, :node_b => 1, :rest_length => 1.48,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 0, :node_b => 2, :rest_length => 1.51,  :amplitude => 0,   :phase => 0,        },
    { :node_a => 2, :node_b => 3, :rest_length => 0.795, :amplitude => 0.5, :phase => 4.963716, },
    { :node_a => 0, :node_b => 5, :rest_length => 0.778, :amplitude => 0.5, :phase => 6.201504, },
    { :node_a => 5, :node_b => 9, :rest_length => 0.802, :amplitude => 0.5, :phase => 4.963716, },
    { :node_a => 9, :node_b => 7, :rest_length => 0.789, :amplitude => 0.5, :phase => 1.796991, },
    { :node_a => 7, :node_b => 0, :rest_length => 0.771, :amplitude => 0.5, :phase => 3.336371, },
    { :node_a => 1, :node_b => 8, :rest_length => 0.795, :amplitude => 0.5, :phase => 0,        },
    { :node_a => 8, :node_b => 2, :rest_length => 0.795, :amplitude => 0.5, :phase => 1.796991, },
    { :node_a => 3, :node_b => 1, :rest_length => 0.788, :amplitude => 0.5, :phase => 3.39292,  },
  ],
}

if $0 == __FILE__
  Window.new.show
end