Skip to content

Instantly share code, notes, and snippets.

@komasaru

komasaru/calc_vincenty.rb

Created August 22, 2019 00:57
Show Gist options
  • Save komasaru/1b6c207c2b9e3d8b39f75b4857ed2a2e to your computer and use it in GitHub Desktop.
Save komasaru/1b6c207c2b9e3d8b39f75b4857ed2a2e to your computer and use it in GitHub Desktop.
Download ZIP
Ruby script to calculate geodesical values by Vincenty's formulae.
Raw
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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment