Ruby script to calculate geodesical values by Vincenty's formulae.
#! /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