#!/usr/bin/ruby
# coding: cp932

=begin
回転座標系について
=end

class RotatedCoordinates
  
  def RotatedCoordinates.adjust_equation(str)
    str.lines.collect{|line|
      line.chomp.split(/ +/) \
          .collect{|v| v.gsub(/([+-]?)([a-zA-Z]\w*)/, '\1@\2')} \
          .join.split(/,/)
    }
  end
  
  {
    :Body_NED => { # 機体軸とNED(Local Geodetic)軸の関係、回転順序はpsi => theta => phi
      :matrix => adjust_equation(<<-__STRING__),
         ctheta * cpsi, sphi * stheta * cpsi - cphi * spsi, cphi * stheta * cpsi + sphi * spsi
         ctheta * spsi, sphi * stheta * spsi + cphi * cpsi, cphi * stheta * spsi - sphi * cpsi
        -stheta,        sphi * ctheta,                      cphi * ctheta
        __STRING__
      :func => [:b2g, :g2b],
      :init => 
        {:s => :sin, :c => :cos}.collect{|prefix, f|
          {:psi => 0, :theta => 1, :phi => 2}.collect{|k, i|
            "@#{prefix}#{k} = Math.#{f}(euler[#{i}] || euler[:#{k}] || 0.0);"
          }
        }.join,
    },
    :Body_Wind => { # 機体軸と風軸の関係、回転順序はalpha => beta
      :matrix => adjust_equation(<<-__STRING__),
         cbeta * calpha, sbeta,  cbeta * salpha
        -sbeta * calpha, cbeta, -sbeta * salpha
        -salpha,         0,      calpha
        __STRING__
      :func => [:b2w, :w2b],
      :init => 
        {:s => :sin, :c => :cos}.collect{|prefix, f|
          {:alpha => 0, :beta => 1}.collect{|k, i|
            "@#{prefix}#{k} = Math.#{f}(euler[#{i}] || euler[:#{k}] || 0.0);"
          }
        }.join,
    },
  }.each{|klass, prop|
    eval <<-__STRING__
      class #{klass}
        def initialize(euler = {})
          #{prop[:init]}
          @#{prop[:func][0]}_mat = [#{prop[:matrix].collect{|row| "[#{row.join(',')}]"}.join(',')}]
          @#{prop[:func][1]}_mat = @#{prop[:func][0]}_mat.transpose
        end
        attr_reader :#{prop[:func][0]}_mat, :#{prop[:func][1]}_mat
        def #{prop[:func][0]}(values) # 回転後の座標系でのベクトルを、回転前の座標系で表す
          @#{prop[:func][0]}_mat.collect{|row|
            #{[0, 1, 2].collect{|i| "(row[#{i}] * values[#{i}])"}.join('+')}
          }
        end
        def #{prop[:func][1]}(values) # 回転前の座標系でのベクトルを、回転後の座標系で表す
          @#{prop[:func][1]}_mat.collect{|row|
            #{[0, 1, 2].collect{|i| "(row[#{i}] * values[#{i}])"}.join('+')}
          }
        end
      end
    __STRING__
    prop[:func].each{|f|
      eval <<-__STRING__
        def #{self.to_s}.#{f}(values, euler = {})
          #{klass}::new(euler).#{f}(values)
        end
        def #{self.to_s}.#{f}_mat(euler = {})
          #{klass}::new(euler).#{f}_mat
        end
      __STRING__
    }
  }  
end

if $0 == __FILE__
  [
    [[:b2g, :g2b], {:psi => Math::PI / 2, :theta => Math::PI / 4}],
    [[:b2w, :w2b], {:alpha => Math::PI / 4, :beta => Math::PI / 4}],
    [[:b2g, :g2b], [10, 20, 30].collect{|deg| Math::PI / 180 * deg}],
    [[:b2w, :w2b], [10, 20].collect{|deg| Math::PI / 180 * deg}],
  ].each{|funcs, euler|
    vec = [1, 0, 0]
    puts "orig: #{vec}"
    funcs.each{|f|
      puts "#{f}_mat: #{RotatedCoordinates.send("#{f}_mat".to_sym, euler)}"
      vec = RotatedCoordinates.send(f, vec, euler)
      puts "#{f}: #{vec}"
    }
    puts
  }
end