Ruby script to calculate geodesical values by Vincenty's formulae.

calc_vincenty.rb

#! /usr/local/bin/ruby
#---------------------------------------------------------------------------------
#= Vincenty 法で、楕円体上の2点間の距離を計算したり、1点から指定の方位角・距離に
#  ある点を求めたりする
#  * CalcVincenty クラスは実行クラス。
#  * Vincenty クラスが計算の本質部分。
#    + 前提とする地球楕円体は GRS-80, WGS-84, Bessel を想定。
#      使用する地球楕円体は、インスタンス化時に指定可能(default: "GRS80")。
#  * 地点1の緯度・経度(°)で Vincenty クラスのオブジェクトを生成後、
#    + calc_dist(calc_distance, calc_inverse) メソッドを、地点2の緯度・経度を引数
#      にして実行すれば、地点1と地点2の距離と、地点1,2における方位角(°)を計算する。
#      (by Vincenty 法の Inverse formula (逆解法))
#    + calc_dest(calc_destination, calc_direct) メソッドを、地点1からの方位角・距
#      離を引数にして実行すれば、地点2の緯度・経度(°)と地点1,2における方位角(°)
#      を計算する。
#      (by Vincenty 法の Direct formula (順解法))
#
#   date          name            version
#   2019.08.13    mk-mode.com     1.00 新規作成
#
# Copyright(C) 2019 mk-mode.com All Rights Reserved.
#---------------------------------------------------------------------------------
#
# 例として、
#
#   1.松江市役所 35.4681  (35°28′05″)N, 133.0486  (133°02′55″)E
#     と
#     島根県庁   35.472222(35°28′20″)N, 133.050556(133°03′02″)E
#     の距離、それぞれの方位角を計算
#
#   2.松江市役所の、1で求めた松江市役所から見た方位角の、1で求めた距離
#     にある地点を計算
#     （元の島根県庁の位置が求まるはず）
#
class CalcVincenty
  POINT_1 = [35.4681,   133.0486  ]  # 松江市役所
  #POINT_1 = [35.5382,   132.9998  ]  # 島根原発1号機
  POINT_2 = [35.472222, 133.050556]  # 島根県庁
  #POINT_1 = [0.0,   0.0]  # 収束しない例
  #POINT_2 = [0.5, 179.7]  # 収束しない例

  def initialize
    @v = Vincenty.new(*POINT_1)
  end

  def exec
    begin
      # 地点1,2が与えられたとき、
      # 2地点間の距離と、地点1,2における方位角を計算
      puts "  POINT-1: %13.8f°, %13.8f°" % POINT_1
      puts "  POINT-2: %13.8f°, %13.8f°" % POINT_2
      puts "-->"
      s, az_1, az_2 = @v.calc_dist(*POINT_2)
      puts "   LENGTH: %17.8f m"  % s
      puts "AZIMUTH-1: %17.8f °" % az_1
      puts "AZIMUTH-2: %17.8f °" % az_2
      puts "==="
      # 地点1と地点1における方位角、距離が与えられたとき、
      # 地点2の位置と地点2における方位角を計算
      puts "  POINT-1: %13.8f°, %13.8f°" % POINT_1
      puts "AZIMUTH-1: %17.8f °"          % az_1
      puts "   LENGTH: %17.8f m"           % s
      puts "-->"
      pt_2, az_2 = @v.calc_dest(az_1, s)
      puts "  POINT-2: %13.8f°, %13.8f°" % pt_2
      puts "AZIMUTH-2: %17.8f °"          % az_2
    rescue => e
      $stderr.puts "[#{e.class}] #{e.message}\n"
      e.backtrace.each { |tr| $stderr.puts "\t#{tr}" }
      exit 1
    end
  end
end

class Vincenty
  # 地球楕円体(長半径, 扁平率の逆数)
  ELLIPSOID = {
    "GRS80"  => [6_378_137.0  , 298.257_222_101],
    "WGS84"  => [6_378_137.0  , 298.257_223_563],
    "BESSEL" => [6_377_397.155, 299.152_813_000]
  }
  # その他定数
  PI2        = Math::PI * 2
  PI_180     = Math::PI / 180
  PI_180_INV = 1 / PI_180
  EPS        = 1e-11  # 1^(-11) = 10^(-12) で 0.06mm の精度

  # 初期化
  #
  # @param  latitude: 緯度(°); 北緯+, 南緯-
  # @param longitude: 経度(°); 東経+, 西経-
  # @param ellipsoid(optional): 地球楕円体("GRS80" or "WGS80" or "BESSEL")
  def initialize(lat, lng, ellipsoid="GRS80")
    el       = ELLIPSOID[ellipsoid]
    @a       = el[0]
    @f       = 1 / el[1]
    @b       = @a * (1 - @f)
    @phi_1   = deg2rad(lat)
    @l_1     = deg2rad(lng)
    @tan_u_1 = (1 - @f) * Math.tan(@phi_1)
    @u_1     = Math.atan(@tan_u_1)
  end

  # 距離と方位角1,2を計算
  # * Vincenty 法の逆解法
  #
  # @param  lat: 緯度(°); φ_1; 北緯+, 南緯-
  # @param  lng: 経度(°);  L_1; 東経+, 西経-
  # @return [
  #           s(距離(m)),
  #           az_1(方位角1(°); α_1(rad)),
  #           az_2(方位角2(°); α_2(rad))
  #         ]
  def calc_inverse(lat, lng)
    begin
      phi_2   = deg2rad(lat)
      l_2     = deg2rad(lng)
      u_2     = Math.atan((1 - @f) * Math.tan(phi_2))
      cos_u_1 = Math.cos(@u_1)
      cos_u_2 = Math.cos(u_2)
      sin_u_1 = Math.sin(@u_1)
      sin_u_2 = Math.sin(u_2)
      su1_su2 = sin_u_1 * sin_u_2
      su1_cu2 = sin_u_1 * cos_u_2
      cu1_su2 = cos_u_1 * sin_u_2
      cu1_cu2 = cos_u_1 * cos_u_2
      l = norm_lng(l_2 - @l_1)
      lmd = l
      lmd_prev = PI2
      cos_lmd = Math.cos(lmd)
      sin_lmd = Math.sin(lmd)
      begin
        t_0 = cos_u_2 * sin_lmd
        t_1 = cu1_su2 - su1_cu2 * cos_lmd
        sin_sgm = Math.sqrt(t_0 * t_0 + t_1 * t_1)
        cos_sgm = su1_su2 + cu1_cu2 * cos_lmd
        sgm = Math.atan2(sin_sgm, cos_sgm)
        sin_alp = cu1_cu2 * sin_lmd / sin_sgm
        cos2_alp = 1 - sin_alp * sin_alp
        cos_2_sgm_m = cos_sgm - 2 * su1_su2 / cos2_alp
        c = calc_c(cos2_alp)
        lmd_prev = lmd
        lmd = l + (1 - c) * @f * sin_alp \
            * (sgm + c * sin_sgm \
            * (cos_2_sgm_m + c * cos_sgm \
            * (-1 + 2 * cos_2_sgm_m * cos_2_sgm_m)))
        cos_lmd = Math.cos(lmd)
        sin_lmd = Math.sin(lmd)
        if lmd > Math::PI
          lmd = Math::PI
          break
        end
      end while (lmd - lmd_prev).abs > EPS
      u2 = cos2_alp * (@a * @a - @b * @b) / (@b * @b)
      a, b = calc_a_b(u2)
      d_sgm = calc_dlt_sgm(b, cos_sgm, sin_sgm, cos_2_sgm_m)
      s = @b * a * (sgm - d_sgm)
      alp_1 = Math.atan2(cos_u_2 * sin_lmd,  cu1_su2 - su1_cu2 * cos_lmd)
      alp_2 = Math.atan2(cos_u_1 * sin_lmd, -su1_cu2 + cu1_su2 * cos_lmd) \
            + Math::PI
      return [s, rad2deg(norm_az(alp_1)), rad2deg(norm_az(alp_2))]
    rescue => e
      raise
    end
  end
  alias_method :calc_distance, :calc_inverse
  alias_method :calc_dist,     :calc_distance

  # 位置2と方位角2を計算
  # * Vincenty 法の順解法
  #
  # @param  az_1: 方位角1(°); α_1(rad)
  # @param     s: 距離(m)
  # @return [
  #           [
  #             lat(緯度(°); φ_2(rad)),
  #             lng(経度(°); L_2(rad))
  #           ],
  #           az_2(方位角2(°); α_2(rad))
  #         ]
  def calc_direct(az_1, s)
    begin
      alp_1 = deg2rad(az_1)
      cos_alp_1 = Math.cos(alp_1)
      sin_alp_1 = Math.sin(alp_1)
      sgm_1     = Math.atan2(@tan_u_1, cos_alp_1)
      sin_alp   = Math.cos(@u_1) * Math.sin(alp_1)
      sin2_alp  = sin_alp * sin_alp
      cos2_alp  = 1 - sin2_alp
      u2        = cos2_alp * (@a * @a - @b * @b) / (@b * @b)
      a, b      = calc_a_b(u2)
      sgm = s / (@b * a)
      sgm_prev = PI2
      begin
        cos_sgm = Math.cos(sgm)
        sin_sgm = Math.sin(sgm)
        cos_2_sgm_m = Math.cos(2 * sgm_1 + sgm)
        d_sgm = calc_dlt_sgm(b, cos_sgm, sin_sgm, cos_2_sgm_m)
        sgm_prev = sgm
        sgm = s / (@b * a) + d_sgm
      end while (sgm - sgm_prev).abs > EPS
      cos_u_1 = Math.cos(@u_1)
      sin_u_1 = Math.sin(@u_1)
      cu1_cs  = cos_u_1 * cos_sgm
      cu1_ss  = cos_u_1 * sin_sgm
      su1_cs  = sin_u_1 * cos_sgm
      su1_ss  = sin_u_1 * sin_sgm
      tmp     = su1_ss - cu1_cs * cos_alp_1
      phi_2 = Math.atan2(
        su1_cs + cu1_ss * cos_alp_1,
        (1 - @f) * Math.sqrt(sin_alp * sin_alp + tmp * tmp)
      )
      lmd = Math.atan2(sin_sgm * sin_alp_1, cu1_cs - su1_ss * cos_alp_1)
      c = calc_c(cos2_alp)
      l = lmd - (1 - c) * @f * sin_alp \
          * (sgm + c * sin_sgm \
          * (cos_2_sgm_m + c * cos_sgm \
          * (-1 + 2 * cos_2_sgm_m * cos_2_sgm_m)))
      l_2 = l + @l_1
      alp_2 = Math.atan2(sin_alp, -su1_ss + cu1_cs * cos_alp_1) + Math::PI
      return [
        [rad2deg(phi_2), rad2deg(norm_lng(l_2))],
        rad2deg(norm_az(alp_2))
      ]
    rescue => e
      raise
    end
  end
  alias_method :calc_destination, :calc_direct
  alias_method :calc_dest,        :calc_destination

private
  # 度 => ラジアン
  #
  # @param  deg: 度(°)
  # @return rad: ラジアン
  def deg2rad(deg)
    return deg * PI_180
  rescue => e
    raise
  end

  # ラジアン => 度
  #
  # @param  rad: ラジアン
  # @return deg: 度(°)
  def rad2deg(rad)
    return rad * PI_180_INV
  rescue => e
    raise
  end

  # A, B 計算
  #
  # @param      u2: u^2 の値
  # @return [a, b]: [A の値, B の値]
  def calc_a_b(u2)
    # 以下、 Vincenty の 1975 年の論文による計算式
    return [
      1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2))),
      u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))
    ]
    # 以下、 Vincenty による修正(1979)
    #tmp = Math.sqrt(1 + u2)
    #k  = (tmp - 1) / (tmp + 1)
    #k2 = k * k
    #return [(1 + k2 / 4) / (1 - k), k * (1 - 3 / 8 * k2)]
  rescue => e
    raise
  end

  # C 計算
  #
  # @param cos2_alp: cos^2(α) の値
  # @return       c: C の値
  def calc_c(cos2_alp)
    return @f * cos2_alp * (4 + @f * (4 - 3 * cos2_alp)) / 16
  rescue => e
    raise
  end

  # Δσ 計算
  #
  # @param           b: B の値
  # @param     cos_sgm: cos(σ) の値
  # @param     sin_sgm: sin(σ) の値
  # @param cos_2_sgm_m: cos(2σ_m) の値
  # @return    dlt_sgm: Δσ の値
  def calc_dlt_sgm(b, cos_sgm, sin_sgm, cos_2_sgm_m)
    return b * sin_sgm * (cos_2_sgm_m \
           + b / 4 * (cos_sgm * (-1 + 2 * cos_2_sgm_m * cos_2_sgm_m) \
           - b / 6 * cos_2_sgm_m * (-3 + 4 * sin_sgm * sin_sgm) \
           * (-3 + 4 * cos_2_sgm_m * cos_2_sgm_m)))
  rescue => e
    raise
  end

  # 経度正規化
  #
  # @param  lng: 正規化前の経度(rad)
  # @return lng: 正規化後の経度(rad)
  def norm_lng(lng)
    while lng < -Math::PI; lng += PI2; end
    while lng >  Math::PI; lng -= PI2; end
    return lng
  rescue => e
    raise
  end

  # 方位角正規化
  #
  # @param  alp: 正規化前の方位角(rad)
  # @return alp: 正規化後の方位角(rad)
  def norm_az(alp)
    case
    when alp < 0.0; alp += PI2
    when alp > PI2; alp -= PI2
    end
    return alp
  rescue => e
    raise
  end
end

CalcVincenty.new.exec if __FILE__ == $0