Ruby-FFIでODEを動かす実験

Ruby-FFIでODE(Open Dynamics Engine)を試してみました。

最近、ODEの話題がめっきり減っている感じですが、ODEのサイトをのぞいてみるといつの間にかバージョンがode-0.12に上がっていました。
メンテナンスされているのは嬉しく思います。
さて、久しぶりにODEを触ってみようと思ったのですが、このごろC++じゃなくてRubyにモチベーションが移ってしまって、あと、また一からC++をコーディングするのもなんか嫌だったので、今回RubyでODEを動かすことにチャレンジしてみました。
RubyでODEを動かすにあたって、Ruby/DL、SWIG、rb++(rbplusplus)等いろいろ調べましたが、なんとなくRuby-FFIが自分のスキルにマッチしている感じがしたので選択しました。

以下にサンプルコードを置きます。
内容は、"球体が高い位置からから自由落下して地面に衝突する"ものです。

とりあえず実験なので必要最小限の関数しか用意しませんでした。
あとGUIは無しです。

require 'ffi'

module ODE
  extend FFI::Library
  ffi_lib '/usr/local/lib/libode.so'

  class DVector3 < FFI::Struct
    layout :x, :double,
           :y, :double,
           :z, :double,
           :w, :double
  end
  class DSurface < FFI::Struct
    layout :mode, :int,
           :mu, :double,
           :mu2, :double,
           :bounce, :double,
           :bounce_vel, :double,
           :soft_erp, :double,
           :soft_cfm, :double,
           :motion1, :double,
           :motion2, :double,
           :motionN, :double,
           :strip1, :double,
           :strip2, :double
  end
  class DContactGeom < FFI::Struct
    layout :pos, DVector3,
           :normal, DVector3,
           :depth, :double,
           :g1, :pointer,
           :g2, :pointer,
           :side1, :int,
           :side2, :int
  end
  class DContact < FFI::Struct
    layout :surface, DSurface,
           :geom, DContactGeom,
           :fdir1, DVector3
  end
  attach_function :dInitODE, [], :void
  attach_function :dWorldCreate, [], :pointer
  attach_function :dWorldSetGravity, [:pointer, :double, :double, :double], :void
  attach_function :dBodyCreate, [:pointer], :pointer
  attach_function :dWorldStep, [:pointer, :double], :void
  attach_function :dBodySetPosition, [:pointer, :double, :double, :double], :void
  attach_function :dBodyGetPosition, [:pointer], :pointer
  attach_function :dBodyGetRotation, [:pointer], :pointer
  attach_function :dJointGroupCreate, [:pointer], :pointer
  attach_function :dJointGroupEmpty, [:pointer], :void
  attach_function :dSimpleSpaceCreate, [:pointer], :pointer
  attach_function :dCreatePlane, [:pointer, :double, :double, :double, :double], :pointer
  attach_function :dCreateSphere, [:pointer, :double], :pointer
  attach_function :dGeomSetBody, [:pointer, :pointer], :void
  attach_function :dGeomGetBody, [:pointer], :pointer
  attach_function :dAreConnectedExcluding, [:pointer, :pointer, :int], :bool
  attach_function :dCollide, [:pointer, :pointer, :int, :pointer, :int], :int
  attach_function :dJointCreateContact, [:pointer, :pointer, :pointer], :pointer
  attach_function :dJointAttach, [:pointer, :pointer, :pointer], :void

  callback :near_callback,  [:pointer, :pointer, :pointer], :void
  attach_function :dSpaceCollide, [:pointer, :pointer, :near_callback], :void
end

require 'ode'

ODE.dInitODE
world = ODE.dWorldCreate
space = ODE.dSimpleSpaceCreate(nil)
contactgroup = ODE.dJointGroupCreate(nil)
ODE.dCreatePlane(space, 0.0, 0.0, 1.0, 0)
ODE.dWorldSetGravity(world, 0.0, 0.0, -9.8)

sphere_body = ODE.dBodyCreate(world)
ODE.dBodySetPosition(sphere_body, 0.0, 0.0, 10.0)

sphere_geom = ODE.dCreateSphere(space, 1.0)
ODE.dGeomSetBody(sphere_geom, sphere_body)

@nearCallback = Proc.new do |data_p, g1_p, g2_p|
  b1 = ODE.dGeomGetBody( g1_p )
  b2 = ODE.dGeomGetBody( g2_p )
  break if b1 and b2 and ODE.dAreConnectedExcluding(b1, b2, 4)
  contacts = []
  4.times do 
    c = ODE::DContact.new
    c[:surface][:mode] = 0x004 | 0x010
    c[:surface][:mu] = 10000.0
    c[:surface][:bounce] = 0.1
    c[:surface][:soft_cfm] = 0.01
    contacts << c
  end
  numc = ODE.dCollide(g1_p, g2_p, 4, contacts[0][:geom], ODE::DContact.size)
  if numc > 0
    numc.times do |i|
      c = ODE.dJointCreateContact(world, contactgroup, contacts[i])
      ODE.dJointAttach(c, b1, b2)
    end
  end
end

500.times do 
  ODE.dSpaceCollide(space, nil, @nearCallback)
  ODE.dWorldStep(world, 0.01)
  ODE.dJointGroupEmpty(contactgroup)

  pos_p = ODE.dBodyGetPosition(sphere_body)
  pos = ODE::DVector3.new(pos_p)
  puts "pos x=#{pos[:x]}, y=#{pos[:y]}, z=#{pos[:z]}"
end

実行結果(z座標)をグラフにしてみました。

なんかそれっぽく動いているようです。
コールバック関数も心配していた構造体のネストもうまく記述できました。
enumの値とかはまだベタ書きです。
GCがうまく動いているのかは確認していないので、そのあたりは今後の課題としてみれればと。

ライブラリ化とか目標にしているわけではなく、共通の関心をもった人に参考になれば幸いです。