Skip to content

Instantly share code, notes, and snippets.

@komasaru komasaru/blh2enu.f95
Last active May 10, 2019

Embed
What would you like to do?
Fortran 95 source code to convert WGS84(BLH) to ENU coordinate.
!*******************************************************************************
! BLH -> ENU 変換
! : WGS84 の緯度(Beta)/経度(Lambda)/楕円体高(Height)を
! ENU (East/North/Up; 地平) 座標に変換する。
! * 途中、 ECEF(Earth Centered Earth Fixed; 地球中心・地球固定直交座標系)座標
! への変換を経由。
!
! date name version
! 2019.02.22 mk-mode.com 1.00 新規作成
!
! Copyright(C) 2019 mk-mode.com All Rights Reserved.
! ---
! 引数 : BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT
! (BETA: Latitude, LAMBDA: Longitude)
! (前半3つは原点、後半3つは対象)
!*******************************************************************************
!
program blh2enu
implicit none
! SP: 単精度(4), DP: 倍精度(8)
integer, parameter :: SP = kind(1.0)
integer(SP), parameter :: DP = selected_real_kind(2 * precision(1.0_SP))
character(*), parameter :: USAGE = "[USAGE] ./blh2enu BETA_O LAMBDA_O HEIGHT_O BETA LAMBDA HEIGHT"
real(DP), parameter :: A = 6378137.0_DP ! a(地球楕円体長半径(赤道面平均半径))
real(DP), parameter :: ONE_F = 298.257223563_DP ! 1 / f(地球楕円体扁平率=(a - b) / a)
real(DP), parameter :: B = A * (1.0_DP - 1.0_DP / ONE_F) ! b(地球楕円体短半径)
real(DP), parameter :: E2 = (1.0_DP / ONE_F) * (2.0_DP - (1.0_DP / ONE_F))
! e^2 = 2 * f - f * f
! = (a^2 - b^2) / a^2
real(DP), parameter :: ED2 = E2 * A * A / (B * B) ! e'^2= (a^2 - b^2) / b^2
real(DP), parameter :: PI = atan(1.0_DP) * 4.0_DP ! 円周率
real(DP), parameter :: PI_180 = PI / 180.0_DP ! 0.0174532925199433
real(DP) :: blh_o(3), blh(3), enu(3) ! BLH 座標(原点、対象), ENU 座標
real(DP) :: az, el, dst ! 方位角、仰角、距離
real(DP) :: e_2, n_2, u_2 ! e^2, n^2, u^2 用
! コマンドライン引数取得
call get_arg(blh_o, blh)
if (blh_o(1) == 0) stop
print '("BLH_O: LATITUDE(BETA) = ", F12.8, "°")', blh_o(1)
print '(" LONGITUDE(LAMBDA) = ", F12.8, "°")', blh_o(2)
print '(" HEIGHT = ", F12.3, "m" )', blh_o(3)
print '("BLH: LATITUDE(BETA) = ", F12.8, "°")', blh(1)
print '(" LONGITUDE(LAMBDA) = ", F12.8, "°")', blh(2)
print '(" HEIGHT = ", F12.3, "m" )', blh(3)
! BLH -> ENU
call conv_blh2enu(blh_o, blh, enu)
! 方位角、仰角、距離
az = atan2(enu(1), enu(2)) / PI_180
if (az < 0.0_DP) az = az + 360.0_DP
e_2 = enu(1) * enu(1)
n_2 = enu(2) * enu(2)
u_2 = enu(3) * enu(3)
el = atan2(enu(3), sqrt(e_2 + n_2)) / PI_180
dst = sqrt(e_2 + n_2 + u_2)
! 結果出力
print '(A)', "--->"
print '("ENU: E = ", F12.3, "m")', enu(1)
print '(" N = ", F12.3, "m")', enu(2)
print '(" U = ", F12.3, "m")', enu(3)
print '("方位角 = ", F12.3, "°")', az
print '(" 仰角 = ", F12.3, "°")', el
print '(" 距離 = ", F12.3, "°")', dst
stop
contains
! コマンドライン引数取得
!
! :param(out) real(8) blh_o(3): 原点 BLH 座標
! :param(out) real(8) blh(3): 対象 BLH 座標
subroutine get_arg(blh_o, blh)
implicit none
real(DP), intent(out) :: blh_o(3), blh(3)
character(20) :: a
blh_o = 0.0_DP
blh = 0.0_DP
if (iargc() < 6) then
print *, USAGE
return
end if
call getarg(1, a)
read (a, *) blh_o(1)
call getarg(2, a)
read (a, *) blh_o(2)
call getarg(3, a)
read (a, *) blh_o(3)
call getarg(4, a)
read (a, *) blh(1)
call getarg(5, a)
read (a, *) blh(2)
call getarg(6, a)
read (a, *) blh(3)
end subroutine get_arg
! BLH -> ENU
!
! :param(in) real(8) blh_o(3): BLH 座標(原点)
! :param(in) real(8) blh(3): BLH 座標(対象)
! :param(out) real(8) enu(3): ENU 座標
subroutine conv_blh2enu(blh_o, blh, enu)
implicit none
real(DP), intent(in) :: blh_o(3), blh(3)
real(DP), intent(out) :: enu(3)
real(DP) :: xyz_o(3), xyz(3)
real(DP) :: mtx_0(3, 3), mtx_1(3, 3), mtx_2(3, 3), mtx_r(3, 3)
call conv_blh2ecef(blh_o, xyz_o)
call conv_blh2ecef(blh, xyz)
call mtx_z(90.0_DP, mtx_0)
call mtx_y(90.0_DP - blh_o(1), mtx_1)
call mtx_z(blh_o(2), mtx_2)
mtx_r = matmul(mtx_0, mtx_1)
mtx_r = matmul(mtx_r, mtx_2)
enu = matmul(mtx_r, xyz - xyz_o)
end subroutine conv_blh2enu
! BLH -> ECEF
!
! :param(in) real(8) blh(3): BLH 座標
! :param(out) real(8) xyz(3): ECEF 座標
subroutine conv_blh2ecef(blh, xyz)
implicit none
real(DP), intent(in) :: blh(3)
real(DP), intent(out) :: xyz(3)
real(DP) :: c_lat, c_lon, s_lat, s_lon
c_lat = cos(blh(1) * PI_180)
c_lon = cos(blh(2) * PI_180)
s_lat = sin(blh(1) * PI_180)
s_lon = sin(blh(2) * PI_180)
xyz = (/ &
& (f(blh(1)) + blh(3)) * c_lat * c_lon, &
& (f(blh(1)) + blh(3)) * c_lat * s_lon, &
& (f(blh(1)) * (1.0_DP - E2) + blh(3)) * s_lat &
& /)
end subroutine conv_blh2ecef
! Function for blh2ecef
!
! :param(in) real(8) x
! :return real(8) f
real(DP) function f(x)
implicit none
real(DP), intent(in) :: x
real(DP) :: s
s = sin(x * PI_180)
f = A / sqrt(1.0_DP - E2 * s * s)
end function f
! x 軸を軸とした回転行列
!
! :param(in) real(8) ang: 回転量(°)
! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
subroutine mtx_x(ang, mtx)
implicit none
real(DP), intent(in) :: ang
real(DP), intent(out) :: mtx(3, 3)
real(DP) :: a, c, s
a = ang * PI_180
c = cos(a)
s = sin(a)
mtx= transpose(reshape( &
& (/1.0_DP, 0.0_DP, 0.0_DP, 0.0_DP, c, s, 0.0_DP, -s, c/), &
& (/3, 3/) &
& ))
end subroutine mtx_x
! y 軸を軸とした回転行列
!
! :param(in) real(8) ang: 回転量(°)
! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
subroutine mtx_y(ang, mtx)
implicit none
real(DP), intent(in) :: ang
real(DP), intent(out) :: mtx(3, 3)
real(DP) :: a, c, s
a = ang * PI_180
c = cos(a)
s = sin(a)
mtx = transpose(reshape( &
& (/c, 0.0_DP, -s, 0.0_DP, 1.0_DP, 0.0_DP, s, 0.0_DP, c/), &
& (/3, 3/) &
& ))
end subroutine mtx_y
! z 軸を軸とした回転行列
!
! :param(in) real(8) ang: 回転量(°)
! :param(out) real(8) mtx(3, 3): 回転行列(3x3)
subroutine mtx_z(ang, mtx)
implicit none
real(DP), intent(in) :: ang
real(DP), intent(out) :: mtx(3, 3)
real(DP) :: a, c, s
a = ang * PI_180
c = cos(a)
s = sin(a)
mtx = transpose(reshape( &
& (/c, s, 0.0_DP, -s, c, 0.0_DP, 0.0_DP, 0.0_DP, 1.0_DP/), &
& (/3, 3/) &
& ))
end subroutine mtx_z
end program blh2enu
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.