经纬度与WGS84坐标转换
2014-11-25 21:36:08   来源：Fcode研讨团队   评论：0 点击：

Module CorrTrans
  !// WGS84 系统BLH坐标与空间直角坐标转换
  !// Fortran Coder http://fcode.cn
  !// Adapted from Acheng's C++ code
  Implicit None
  Integer , parameter :: DP = Selected_Real_Kind( 12 )
  Real(kind=DP) , parameter :: PI = 3.14159265358979323846_DP
  Real(kind=DP) , parameter , private :: a = 6378137._DP
  Real(kind=DP) , parameter , private :: f = 1.0_DP / 298.257223563_DP
  Real(kind=DP) , parameter , private :: t = a * ( 1.0_DP - f )
  Real(kind=DP) , parameter , private :: e = sqrt( (a*a - t*t) / (a*a) )
contains
  Subroutine XYZ2BLH( x , y , z , B , L , H )
    Real(Kind=DP) , Intent( IN )  :: x , y , z
    Real(Kind=DP) , Intent( OUT ) :: B , L , H
    real(kind=DP) :: N
    integer :: i        
    L = atan( abs(y/x) )
    B = atan( abs(z/sqrt(x*x+y*y) ) )
    do i = 1 , 5
      N = a / sqrt( 1.0_DP - e*e*sin(B)*sin(B) )
      H = z / sin(B) - N * ( 1.0_DP - e*e )
      B = atan( z*(N+H) / ( sqrt(x*x+y*y)*(N*(1.0_DP-e*e)+H) ) )
    end do
    if ( x < 0._DP .and. y > 0._DP ) L = PI - L
    if ( x > 0._DP .and. y < 0._DP ) L = -1.0_DP * L
    if ( x < 0._DP .and. y < 0._DP ) L = -1.0 * PI + L
    B = B * 180._DP / PI
    L = L * 180._DP / PI
  End Subroutine XYZ2BLH
  
  Subroutine BLH2XYZ( B , L , H , x , y , z )
    Real(Kind=DP) , Intent( IN )  :: B , L , H  
    Real(Kind=DP) , Intent( OUT ) :: x , y , z
    real(kind=DP) :: N , Br , Lr
    Br = B * PI / 180._DP
    Lr = L * PI / 180._DP
    N = a / sqrt( 1.0_DP - e*e*sin(Br)*sin(Br) )
    x = ( N + H ) * cos(Br)*cos(Lr)
    y = ( N + H ) * cos(Br)*sin(Lr)
    z = ( N*(1.0_DP - e*e ) + H ) * sin(Br);
  End Subroutine BLH2XYZ

End Module CorrTrans

Program www_fcode_cn
  Use CorrTrans
  Implicit None
  Real(Kind=DP)  :: x , y , z
  Real(Kind=DP)  :: B , L , H  
  B = 39._DP ; L = 116._DP ; H = 33._DP
  call BLH2XYZ( B , L , H , x , y , z )
  write(*,*) 'WGS84:' , x , y , z
  call XYZ2BLH( x , y , z , B , L , H )
  write(*,*) 'BLH:' , B , L , H
End Program www_fcode_cn