root/lang/ruby/pitagora/trunk/ode_ruby/demo/test2.rb @ 14282

require "pp"
require "ode_cpp"
require "thread"
require 'monitor'
require 'yaml'

def log(s)
  puts s
end

# TODO
#   視点を複数切り替え
#   ポインタを拡充
#   colors
#   blog / coderepos / 動画
#   ppmを動画にする方法


#--------------------------------------------------------------
# init
Thread.abort_on_exception = true
include Ode_cpp

#--------------------------------------------------------------
# config
window_width  = 352*2
window_height = 228*2

#--------------------------------------------------------------
# globals, world/space
$monitor = Monitor.new
$world = DWorld.new
$world.setGravity(0.0, 0.0, -9.81)
$world.setCFM(0.00001)
$space = DHashSpace.new(nil) #DSimpleSpace.new(nil)
$contactgroup = DJointGroup.new
ground = DPlane.new($space.id, 0.0, 0.0, 1.0, 0.0) # ax+by+cz=d
$objects = []
$save_num = 0

#p Ode_cpp::dWorldGetContactMaxCorrectingVel($world.id)
#p Ode_cpp::dWorldGetContactSurfaceLayer($world.id)
Ode_cpp::dWorldSetContactMaxCorrectingVel($world.id, 1000.0)
Ode_cpp::dWorldSetContactSurfaceLayer($world.id, 0.001)
Ode_cpp::dWorldSetAutoDisableAverageSamplesCount($world.id, 10)

# Base/Box/Sphere/Capsule/Cylinder
require "./demo/objects.rb"
$pointer_sphere = Sphere.new(0.05, 1)

#----------------------------------------
$sim_running = false
def sim_start
  $sim_running = true
end

def sim_stop
  $sim_running = false
end

def sim_toggle_running
  if $sim_running
    sim_stop
  else
    sim_start
  end
end

def sim_init(fn=nil)
  sim_stop
  $objects.each do |o|
    o.destroy
  end
  $objects.clear
  # $objects << $pointer_sphere
  srand(1)
  $sim_step_count = 0

  if fn
    load_from_yaml(fn)
  else
    create_walls
    create_slope1
    create_capsule_cylinder
    create_pins
    #ray = Ray.new(1)
    #ray.position = [0, -4, 5]

    # a box
    box = Box.new([2,2,2], 1);
    box.position = [-20.0, 0.0, 1.0]
    box.color    = [1.0, 0.0, 1.0]

    #sphere = Sphere.new(3.0, 1.0)
    #sphere.position = [5.0, 5.0, 30.0]
    #sphere.color    = [1.0, 1.0, 0.0]
  end
end


def save_to_yaml
  yaml_obj = $objects.collect do |obj|
    { :class=>obj.class.name,
      :pars=>obj.pars,
      :color=>obj.color,
      :position=>obj.position,
      :rotation=>obj.rotation }
  end
  yaml_str = YAML.dump(yaml_obj)
  # puts yaml_str
  loop{
    $save_num += 1
    fn = "save/state_#{$save_num}.yaml"
    next if File.exists?(fn)
    open(fn, 'w'){|f| f.write(yaml_str) }
    puts "SAVED to #{fn}"
    break
  }
end

def load_from_yaml(fn)
  yaml_obj = YAML.load_file(fn)
  yaml_obj.each do |hash|
    klass = eval(hash[:class])
    obj = klass.new(*hash[:pars])
    obj.color = hash[:color]
    obj.position = hash[:position]
    # obj.rotation = hash[:rotation] # TODO!!
  end
end

#---------------------------------------
def create_walls
  w = 40.0
  h =  4.0
  d =  5.0

  # red
  wall = Box.new([w, d, h], 0.1)
  wall.position = [0.0, w*0.6, h/2]
  wall.color = [1.7, 0.0, 0.0]

  # green
  wall = Box.new([w, d, h], 0.1)
  wall.position = [0.0, -w*0.6, h/2]
  wall.color = [0.0, 1.7, 0.0]

  # blue
  wall = Box.new([d, w, h], 0.1)
  wall.position = [w*0.6, 0.0, h/2]
  wall.color = [0.0, 0.0, 1.7]

  # yellow
  wall = Box.new([d, w, h], 0.1)
  wall.position = [-w*0.6, 0.0, h/2]
  wall.color = [1.0, 1.0, 0.0]
end
create_walls

# 坂道
def create_slope1
  b = Box.new([2,10,3], 1);
  b.position = [15, 0, 1]
  b.color = [0.7, 0.5, 0]
  slope = Box.new([17, 10, 0.2], 0.1);
  slope.position = [7, 0, 3.2]
  slope.color = [1.0, 0.7, 0]
end

def create_capsule_cylinder
  c = Capsule.new(1, 3, 1) # (radius, length, density, direction[1,2,3])
  c.position = [10, -2-10, 5]
  c.color = [1.0, 0.5, 0.5]

  c = Cylinder.new(0.5, 3, 1) # (radius, length, density, direction[1,2,3])
  c.position = [10, 2-10, 5]
  c.color = [0.0, 0.7, 1.0]
end

def create_pins
  (1..4).each do |n|
    (1..n).each do |m|
      x = n*1.5
      y = (n/2.0-m)*1.5
      #c = Cylinder.new(0.2, 4, 0.05) # (radius, length, density, direction[1,2,3])
      c = Box.new([0.5,0.5,4], 0.01)
      c.position = [-3-x, y, 2]
      c.color = [0.0, 0.7, 1.0]
    end
  end
end

def rand_within(from, to)
  rand*(to-from)+from
end

def create_many_objects
  50.times{
    sp = Sphere.new(rand_within(1,5), 1)
    sp.position = [rand_within(-40,40), rand_within(-40,40), rand_within(100,150)]
    sp.color = [rand(2).to_f, rand(2).to_f, rand(2).to_f]
  }
  #10.times{
  #  bx = Box.new([rand_within(1,5), rand_within(1,5), rand_within(1,5)], 1)
  #  bx.position = [rand_within(-40,40), rand_within(-40,40), rand_within(15,100)]
  #  bx.color = [rand(2).to_f, rand(2).to_f, rand(2).to_f]
  #}
end

#box = Box.new([10.0, 50.0, 0.1], 1.0)
#box.position = [0.0, 0.0, 0.0]
#box.color    = [1.5, 1.5, 0.0]


require "draw_stuff"
path_to_textures = File.join(File.dirname(__FILE__), "../../ode-0.9_fix/drawstuff/textures")
ds = DrawStuff.new(:world=>$world,
                   :width  =>window_width,
                   :height =>window_height,
                   :path_to_textures=>path_to_textures)

ds.instance_eval do
  def start
    log "start"
    @cmd_proc = nil
    @draw_count = 0
    @normal_steps_per_sec = 200
    @steps_per_sec = @normal_steps_per_sec
    @normal_draws_per_sec = 60
    @draws_per_sec = @normal_draws_per_sec
    @process_wii_remo_mode = :move

    #log "init viewpoint=%s" % self.viewpoint.inspect
    #self.set_viewpoint([2,0,1], [180,0,0])
    # init viewpoint=[[2.0, 0.0, 1.0], [180.0, 0.0, 0.0]]

    #xyz = [  50.0,   0.0,   1.0] # 視点の位置
    #hpr = [-180.0,   0.0,   0.0] # 視線の方向
    xyz = [18.7891311645508, 10.3102083206177, 4.46602010726929]
    hpr = [-134.198913574219, -13.0999450683594, 0.0]
    self.set_viewpoint(xyz, hpr)
  end
  
  def _collide(o1, o2)
    # $spaceに属する物体で衝突する可能性のある組それぞれで呼ばれる
    # log "cb #{o1._ptr} #{o2._ptr} #{$ground._ptr} #{$ground._ptr==o1._ptr} #{$ground._ptr==o2._ptr}"

    ps_ptr = $pointer_sphere.geom.id._ptr
    if o1._ptr==ps_ptr || o2._ptr==ps_ptr
      return
    end
    #if $objects.last.geom.id._ptr == o1._ptr
    #  log "geom_id = %d" % $objects.last.geom.id._ptr
    #  log "o1        %d" % o1._ptr
    #end
    #if $objects.last.geom.id._ptr == o2._ptr
    #  log "geom_id = %d" % $objects.last.geom.id._ptr
    #  log "o2        %d" % o2._ptr
    #end

    b1 = dGeomGetBody(o1)
    b2 = dGeomGetBody(o2)
    if b1 && b2 && 0<Ode_cpp::dAreConnectedExcluding(b1, b2, Ode_cpp::DJointTypeContact)
      return
    end

    # 衝突検出
    contact_geoms = Ode_cpp::dCollide(o1, o2, 5) # 衝突情報の生成
    contact_geoms.each do |cg|
      # pp cg
      contact = DContact.new
      contact.geom = cg
      contact.surface.mode       = DContactBounce|DContactSoftCFM
      contact.surface.mu         = Float::MAX
      contact.surface.mu2        = 0.0
      contact.surface.bounce     = 0.1
      contact.surface.bounce_vel = 0.1
      contact.surface.soft_cfm   = 0.01
      # 接触ジョイントの生成
      #joint = Ode_c.dJointCreateContact($world._id, $contactgroup, contact)
      joint = DContactJoint.new($world.id, $contactgroup.id, contact)
      # 接触している２つの剛体を接触ジョイントにより拘束
      #Ode_c.dJointAttach(joint, Ode_c.dGeomGetBody(cg.g1), Ode_c.dGeomGetBody(cg.g2))
      joint.attach(Ode_cpp::dGeomGetBody(cg.g1), Ode_cpp::dGeomGetBody(cg.g2))
    end
  end

  def step(pause)
    $monitor.synchronize{
      self._step_without_lock(pause)
    }
  end

  def _step_without_lock(pause)
    3.times do
      break unless $sim_running
      $space.collide(Proc.new{|o1,o2|
        _collide(o1, o2)
      });
      # @world.step(0.01)
      @world.stepFast1(0.01, 1)
      # @world.stepFast1(0.01, 1)
      #Ode_cpp::dJointGroupEmpty($contactgroup) # ジョイントグループを空にする
      $contactgroup.empty
      
      # @steps_per_sec
      _step = 100
      if $sim_step_count%_step==0
        if @step_per_sec_start_time
          sec = Time.now-@step_per_sec_start_time
          @steps_per_sec = _step/sec
          log "%.2f step/sec." % @steps_per_sec
        end
        @step_per_sec_start_time = Time.now
      end
      $sim_step_count += 1
    end
    
    # @steps_per_sec
    _step = 100
    if @draw_count%_step==0
      if @draws_per_sec_start_time
        sec = Time.now-@draws_per_sec_start_time
        @draws_per_sec = _step/sec
        log "%.2f draw/sec." % @draws_per_sec
      end
      @draws_per_sec_start_time = Time.now
    end
    @draw_count += 1
    
    # 描画
    $objects.each do |obj|
      draw = true
      if obj==@selected_object && (@draw_count/20)%3==0
        draw = false
      end
      if draw
        obj.draw
      end
    end

    # ポインタ位置計算
    # @pointer = [0.0,0.0,3.0]
    @pointer, hpr = self.viewpoint
    fwd = 8
    @pointer[0] += Math.cos(hpr[0]*DEG_TO_RAD)*fwd
    @pointer[1] += Math.sin(hpr[0]*DEG_TO_RAD)*fwd
    $pointer_sphere.position = @pointer
    
    # ポインタ描画
    [0,1,2].each do |n|
      if @process_wii_remo_mode==:move
        if @draw_count/10%3!=n
          next
        end
      end
      len = 0.5
      p_start = @pointer.dup
      p_start[n] -= len
      p_end   = @pointer.dup
      p_end[n]   += len
      color = [0.0, 0.0, 0.0]
      color[n] = 1.3
      DrawStuff::DSLib.dsSetColor(*color)
      DrawStuff::DSLib.dsDrawLineD(p_start, p_end)
    end
    
    # cmd_proc
    if @cmd_proc
      @cmd_proc.call
      @cmd_proc = nil
    end
    
    process_wii_remo
  end

  def command(cmd)
    cmd = cmd.chr if cmd.kind_of?(Fixnum)
    log "[command #{cmd}]"
    case cmd
    when 'v'
      @cmd_proc = proc{
        xyz, hpr = self.viewpoint
        log "xyz=%s hpr=%s" % [xyz.inspect, hpr.inspect]
      }
    when 'o'
      @cmd_proc = proc{
        obj_types = Hash.new(0)
        ObjectSpace.each_object do |obj|
          obj_types[obj.class] += 1
        end
        obj_types.to_a.sort{|a,b| b[1]-a[1] }.each do |pair|
          log "%30s : %6d" % pair #if 10<=pair[1]
        end
      }
    when '\\' # start/stop
      sim_toggle_running
    when '^' # init
      sim_init
    when '_' # save
      save_to_yaml
    end
    
    # viatual Wii remo
    if @process_wii_remo_mode==:move
      case cmd
      when 'a','s','w','z','q','e','d','x'
        mode = nil
        dx,dy = 0,0
        case cmd
        when 'w','d' : dy =  20
        when 'z','x' : dy = -20
        when 'a','q' : dx =   6
        when 's','e' : dx =  -6
        end
        case cmd
        when 'q','e','d','x'
          mode = :z
        end
        self.motion(mode, dx, dy)
        # todo
        #   accXY
        #   modeでわける
      end
    end

    # mode command
    case @process_wii_remo_mode
    when :move
      obj = nil
      case cmd
      when 'b': obj = Box.new([1,1,1], 1)
      when 'p': obj = Sphere.new(0.5, 1)
      when 'c': obj = Capsule.new(0.5, 1, 1)
      when 'y': obj = Cylinder.new(0.5, 1, 1)
      when ' '
        @selected_object = _select_object
        if @selected_object
          _change_process_wii_remo_mode(:edit)
        end
      end
      if obj
        obj.position = @pointer
        unless $sim_running
          @selected_object = obj
          _change_process_wii_remo_mode(:edit)
        end
      end
    when :edit
      case cmd
      when 'r' # remove object
        $objects.delete(@selected_object)
        _change_process_wii_remo_mode(:move)
      when ' '
        _change_process_wii_remo_mode(:move)
      when 'm','n' # increace/reduce  density
        log "TODO no impl"
      when '1','2','3','4','5','6','7','8','9','0' # color
        log "TODO no impl"
      end
    end
  end

  def process_wii_remo
    return unless @wii_remote_client
    case @process_wii_remo_mode
    when :move
      process_wii_remo__move
    when :edit
      process_wii_remo__edit
    end
  end

  def process_wii_remo__move
    buttons = @wii_remote_client.buttons_by_chars

    # ヌンチャク
    if @wii_remote_client.nuncyaku_joystick
      x, y = @wii_remote_client.nuncyaku_joystick
      dx = -((x-128)/16.0).truncate
      dy =  ((y-128)/16.0).truncate
      mode = nil
      mode = :z if buttons.include?('z')
      mode = :c if buttons.include?('c')
      self.motion(mode, dx, dy)
    end

    # ボタン(エッジ)
    pushed = _wii_button_pushed
    unless pushed.empty?
      if pushed.include?('A')
        command(' ')
        return
      end
      if pushed.include?('H')
        if buttons.include?('B')
          sim_init
        else
          sim_toggle_running
        end
        return
      end
      if pushed.include?('u'); command('b') end
      if pushed.include?('d'); command('p') end
      if pushed.include?('l'); command('c') end
      if pushed.include?('r'); command('y') end
    end
  end

  # pointerにぶつかっているobjectを返す
  def _select_object
    selected_object = nil
    $objects.each do |obj|
      contact_geoms = Ode_cpp::dCollide($pointer_sphere.geom.id, obj.geom.id, 1)
      unless contact_geoms.empty?
        return obj
      end
    end
    return nil
  end

  def process_wii_remo__edit
    buttons = @wii_remote_client.buttons_by_chars

    # ボタン(エッジ)
    pushed = _wii_button_pushed
    unless pushed.empty?
      if pushed.include?('A')
        command(' ')
        return
      end
    end

    # ヌンチャク
    dx,dy = 0,0
    if @wii_remote_client.nuncyaku_joystick
      x, y = @wii_remote_client.nuncyaku_joystick
      dx = -((x-128)/16.0).truncate
      dy =  ((y-128)/16.0).truncate

#      mode = nil
#      mode = :z if buttons.include?('z')
#      mode = :c if buttons.include?('c')
#      self.motion(mode, dx, dy)
    end

    # ボタン(連続)
    if !buttons.empty? || dx!=0 || dy!=0
      obj = @selected_object
      delta = 0.01
      
      # 移動
      m = false
      position = obj.position
      if dy!=0;                 position[0]-= delta*dy/2; m=true; end
      if dx!=0;                 position[1]-= delta*dx/2; m=true; end # TODO方向を視点からの方向で
      if buttons.include?('c'); position[2]+= delta;      m=true; end
      if buttons.include?('z'); position[2]-= delta;      m=true; end
      position[2] = 0 if position[2]<0
      obj.position = position if m

      m = false
      case obj
      when Box
        lengths, density = obj.pars
        if buttons.include?('u'); lengths[0] += delta; m=true; end
        if buttons.include?('d'); lengths[0] -= delta; m=true; end
        if buttons.include?('r'); lengths[1] += delta; m=true; end
        if buttons.include?('l'); lengths[1] -= delta; m=true; end
        if buttons.include?('1'); lengths[2] += delta; m=true; end
        if buttons.include?('2'); lengths[2] -= delta; m=true; end
        [0,1,2].each{|n| lengths[n] = 0.1 if lengths[n]<0.1 }
        obj.pars = [lengths, density] if m
      when Sphere
        radius, density = obj.pars
        if buttons.include?('r'); radius += delta; m=true; end
        if buttons.include?('l'); radius -= delta; m=true; end
        radius = 0.1 if radius<0.1
        obj.pars = [radius, density] if m
      when Capsule,Cylinder
        radius, length, density = obj.pars
        if buttons.include?('r'); radius += delta; m=true; end
        if buttons.include?('l'); radius -= delta; m=true; end
        if buttons.include?('u'); length += delta; m=true; end
        if buttons.include?('d'); length -= delta; m=true; end
        radius = 0.1 if radius<0.1
        length = 0.1 if length<0.1
        obj.pars = [radius, length, density] if m
      end
    end
  end

  def _wii_button_pushed
    buttons = @wii_remote_client.buttons_by_chars
    @pre_buttons ||= []
    pushed = buttons-@pre_buttons
    @pre_buttons = buttons
    return pushed
  end

  def _change_process_wii_remo_mode(mode)
    case mode
    when :move
      @selected_object = nil
      @process_wii_remo_mode = :move
      log "MOVE mode!!"
    when :edit
      @process_wii_remo_mode = :edit
      log "EDIT mode!!"
    end
  end
  
  def stop
    log "stop"
  end

  def wii_remote_client=(c) @wii_remote_client = c; end
  DEG_TO_RAD = Math::PI/180.0
  #// call this to update the current camera position. the bits in `mode' say
  #// if the left (1), middle (2) or right (4) mouse button is pressed, and
  #// (deltax,deltay) is the amount by which the mouse pointer has moved.
  def motion__0(mode, dx, dy)
    return if dx==0 && dy==0
    xyz, hpr = self.viewpoint

    side = 0.01*dx
    fwd = (mode==4) ? (0.01*dy) : 0.0
    s = Math.sin(hpr[0]*DEG_TO_RAD)
    c = Math.cos(hpr[0]*DEG_TO_RAD)
    if mode==1
      hpr[0] += dx*0.5
      hpr[1] += dy*0.5
    else
      xyz[0] += -s*side + c*fwd
      xyz[1] +=  c*side + s*fwd
      if mode==2 || mode==5
        xyz[2] += 0.01*dy
      end
    end
    self.set_viewpoint(xyz, hpr)
  end

  def motion(mode, dx, dy)
    return if dx==0 && dy==0
    rate = 1.0*@normal_draws_per_sec/@draws_per_sec
    dx *= rate
    dy *= rate

    xyz, hpr = self.viewpoint
    s = Math.sin(hpr[0]*DEG_TO_RAD)
    c = Math.cos(hpr[0]*DEG_TO_RAD)
    if mode==nil
      # 前後移動/左右回転
      fwd  = 0.01*dy
      #dx *= -1 if dy<0
      xyz[0] += c*fwd
      xyz[1] += s*fwd
      hpr[0] += dx*0.5
    elsif mode==:z
      # 左右/高さ移動
      side = 0.01*dx
      xyz[0] += -s*side
      xyz[1] +=  c*side
      xyz[2] += 0.01*dy
      xyz[2] = 0.0 if xyz[2]<0
    elsif mode==:c
      # 視線方向
      hpr[0] += dx*0.5
      hpr[1] += dy*0.5
    else
    end
    self.set_viewpoint(xyz, hpr)
  end
end # ds.instance_eval

opts = {:irb=>true, :wii=>true}

if opts[:wii]
  require "wii_remo"
  wii_remo = WiiRemo.instance
  wii_remo.callback = proc{|pars|
    case pars[:type]
    when :discovered
      r = pars[:remote]
      client = WiiRemoSimpleClient.new
      r.bind(client)
      ds.wii_remote_client = client
      log client.inspect
      wii_remo.stop_search
    when :error
      code = pars[:code]
      log "ERROR %x" % code
    else
      log "ELSE #{pars.inspect}"
    end
  }
  wii_remo.start_search
  log "Start WiiRemote SEARCH...."
end

fn = ARGV.shift
log "ARGV=#{ARGV}"

threads = []
if opts[:irb]
  threads << Thread.start{
    require 'irb'
    IRB.start
    log "irb EXIT."
  }
end

sim_init(fn)
#sim_start
ds.run
threads.each{|t| t.join }