Skip to content

Commit

Permalink
enu2aer and ecef2geodetic singularity handling
Browse files Browse the repository at this point in the history 
normal singularity

fix geodetic2ecef singularity

addtest

aer2ecef singularity test
  • Loading branch information
@scivision
scivision committed Jan 8, 2019
1 parent 42394cf commit 7676e74
Show file tree
Hide file tree
Showing 2 changed files with 111 additions and 30 deletions.
89 changes: 67 additions & 22 deletions src/maptran.F90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ elemental subroutine ecef2geodetic(x, y, z, lat, lon, alt, spheroid, deg)
real(wp), intent(out) :: lat, lon
real(wp), intent(out), optional :: alt

real(wp) :: ea, eb, r, E, u, Q, huE, Beta, eps
real(wp) :: ea, eb, r, E, u, Q, huE, Beta, eps, sinBeta, cosBeta
type(Ellipsoid) :: ell
logical :: d, inside

Expand All @@ -135,21 +135,29 @@ elemental subroutine ecef2geodetic(x, y, z, lat, lon, alt, spheroid, deg)

huE = hypot(u, E)

! eqn. 4b
!> eqn. 4b
Beta = atan(huE / u * z, hypot(x, y))

! eqn. 13
eps = ((eb * u - ea * huE + E**2) * sin(Beta)) / (ea * huE * 1 / cos(Beta) - E**2 * cos(Beta))

Beta = Beta + eps
! final output
lat = atan(ea / eb * tan(Beta))
!> final output
if (abs(beta-pi/2) <= epsilon(pi/2)) then !< singularity
lat = pi/2
cosBeta = 0._wp
sinBeta = 1._wp
else
!> eqn. 13
eps = ((eb * u - ea * huE + E**2) * sin(Beta)) / (ea * huE * 1 / cos(Beta) - E**2 * cos(Beta))
Beta = Beta + eps

lat = atan(ea / eb * tan(Beta))
cosBeta = cos(Beta)
sinBeta = sin(Beta)
endif

lon = atan(y, x)

! eqn. 7
if (present(alt)) then
alt = hypot(z - eb * sin(Beta), Q - ea * cos(Beta))
alt = hypot(z - eb * sinBeta, Q - ea * cosBeta)

!> inside ellipsoid?
inside = x**2 / ea**2 + y**2 / ea**2 + z**2 / eb**2 < 1._wp
Expand Down Expand Up @@ -187,7 +195,7 @@ elemental subroutine geodetic2ecef(lat,lon,alt, x,y,z, spheroid, deg)
type(Ellipsoid), intent(in), optional :: spheroid
logical, intent(in), optional :: deg

real(wp) :: N
real(wp) :: N, sinLat, cosLat, cosLon, sinLon
type(Ellipsoid) :: ell
logical :: d

Expand All @@ -202,13 +210,37 @@ elemental subroutine geodetic2ecef(lat,lon,alt, x,y,z, spheroid, deg)
lon = radians(lon)
endif

! Radius of curvature of the prime vertical section
!> Radius of curvature of the prime vertical section
N = radius_normal(lat, ell)
! Compute cartesian (geocentric) coordinates given (curvilinear) geodetic coordinates.

x = (N + alt) * cos(lat) * cos(lon)
y = (N + alt) * cos(lat) * sin(lon)
z = (N * (ell%SemiminorAxis / ell%SemimajorAxis)**2 + alt) * sin(lat)
!> Compute cartesian (geocentric) coordinates given (curvilinear) geodetic coordinates.

if (abs(lat) <= epsilon(lat)) then
cosLat = 1._wp
sinLat = 0._wp
elseif (abs(lat-pi/2) <= epsilon(lat)) then
cosLat = 0._wp
sinLat = 1._wp
else
cosLat = cos(lat)
sinLat = sin(lat)
endif

if (abs(lon) <= epsilon(lon)) then
cosLon = 1._wp
sinLon = 0._wp
elseif (abs(lon-pi/2) <= epsilon(lon)) then
cosLon = 0._wp
sinLon = 1._wp
else
cosLon = cos(lon)
sinLon = sin(lon)
endif


x = (N + alt) * cosLat * cosLon
y = (N + alt) * cosLat * sinLon
z = (N * (ell%SemiminorAxis / ell%SemimajorAxis)**2 + alt) * sinLat

end subroutine geodetic2ecef

Expand Down Expand Up @@ -460,14 +492,19 @@ elemental subroutine enu2aer(east, north, up, az, elev, slantRange, deg)
logical, intent(in), optional :: deg
real(wp), intent(out) :: az, elev, slantRange

real(wp) :: r
real(wp) :: r, e
logical :: d

!> singularity, 1mm precision
e = east
if (abs(e) < 1e-3_wp) e = 0._wp

r = hypot(east, north)
r = hypot(e, north)
slantRange = hypot(r, up)
! radians

!> radians
elev = atan(up, r)
az = modulo(atan(east, north), 2._wp * atan(0._wp, -1._wp))
az = modulo(atan(e, north), 2._wp * pi)

d=.true.
if (present(deg)) d = deg
Expand Down Expand Up @@ -612,10 +649,18 @@ end subroutine enu2uvw


elemental real(wp) function radius_normal(lat,E)
real(wp), intent(in) :: lat
type(Ellipsoid), intent(in) :: E

radius_normal = E%SemimajorAxis**2 / sqrt( E%SemimajorAxis**2 * cos(lat)**2 + E%SemiminorAxis**2 * sin(lat)**2 )
real(wp), intent(in) :: lat
type(Ellipsoid), intent(in) :: E

!> singularity
if (abs(lat) <= epsilon(lat)) then
radius_normal = E%SemimajorAxis
!elseif (abs(lat-pi/2) <= epsilon(lat)) then
! radius_normal = E%SemiminorAxis
else
radius_normal = E%SemimajorAxis**2 / sqrt( E%SemimajorAxis**2 * cos(lat)**2 + E%SemiminorAxis**2 * sin(lat)**2 )
endif

end function radius_normal

Expand Down
52 changes: 44 additions & 8 deletions tests/test_mod.f90
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ program Test


real(wp) :: lat2, lon2, alt2,lat3,lon3,alt3,lat4,lon4,alt4, &
x1,y1,z1,x2,y2,z2,x3,y3,z3,&
x1,y1,z1,x2,y2,z2,x3,y3,z3, x7,y7,z7,&
az2,el2,rng2,az3,el3,rng3,az4,el4,rng4,azrd,elrd,rae,dae,jd, &
e1,n1,u1,e2,n2,u2,e3,n3,u3, &
lat5(3), lon5(3), rng5(3), az5(3), tilt(3)
Expand Down Expand Up @@ -67,37 +67,72 @@ program Test
call geodetic2ecef(lat,lon,alt, x1,y1,z1)
call assert_allclose([x1,y1,z1],[x0,y0,z0], &
err_msg='geodetic2ecef-degrees')

call geodetic2ecef(0._wp, 0._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7], [spheroid%SemimajorAxis-1, 0._wp, 0._wp])

call geodetic2ecef(0._wp, 90._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7], [0._wp, spheroid%SemimajorAxis-1, 0._wp])

call geodetic2ecef(90._wp, 0._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7], [0._wp, 0._wp, spheroid%SemiminorAxis-1])


call aer2enu(az,el,rng, e1,n1,u1)
call assert_allclose([e1,n1,u1], [er,nr,ur])


call aer2ecef(az,el,rng,lat,lon,alt,x2,y2,z2)
call assert_allclose([x2,y2,z2],[xl,yl,zl])

call aer2ecef(0._wp, -90._wp, 1._wp, 0._wp, 0._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7],[spheroid%SemimajorAxis-1._wp, 0._wp, 0._wp], atol=1e-6_wp)

call aer2ecef(0._wp, -90._wp, 1._wp, 0._wp, 90._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7],[0._wp, spheroid%SemimajorAxis-1._wp, 0._wp], atol=1e-6_wp)

call aer2ecef(0._wp, -90._wp, 1._wp, 90._wp, 0._wp, -1._wp, x7, y7, z7)
call assert_allclose([x7,y7,z7],[0._wp, 0._wp,spheroid%SemiminorAxis-1._wp], atol=1e-6_wp)

!> ECEF2GEODETIC tests
call ecef2geodetic(x1,y1,z1,lat2,lon2,alt2)
call assert_allclose([lat2,lon2,alt2],[lat,lon,alt], &
rtol=0.01_wp, err_msg='ecef2geodetic-degrees')

call ecef2geodetic(spheroid%SemimajorAxis-1._wp, 0._wp, 0._wp, lat2, lon2, alt2)
call ecef2geodetic(spheroid%SemimajorAxis-1, 0._wp, 0._wp, lat2, lon2, alt2)
call assert_allclose([lat2,lon2,alt2],[0._wp, 0._wp, -1._wp], &
rtol=0.01_wp, err_msg='ecef2geodetic-degrees')
err_msg='ecef2geodetic-degrees')

call ecef2geodetic(0._wp, spheroid%SemimajorAxis-1._wp, 0._wp, lat2, lon2, alt2)
call ecef2geodetic(0._wp, spheroid%SemimajorAxis-1, 0._wp, lat2, lon2, alt2)
call assert_allclose([lat2,lon2,alt2],[0._wp, 90._wp, -1._wp], &
rtol=0.01_wp, err_msg='ecef2geodetic-degrees')
err_msg='ecef2geodetic-degrees')

call ecef2geodetic(0._wp, 0._wp, spheroid%SemiminorAxis-1._wp, lat2, lon2, alt2)
call ecef2geodetic(0._wp, 0._wp, spheroid%SemiminorAxis-1, lat2, lon2, alt2)
call assert_allclose([lat2,lon2,alt2],[90._wp, 0._wp, -1._wp], &
rtol=0.01_wp, err_msg='ecef2geodetic-degrees')
err_msg='ecef2geodetic-degrees')

call enu2aer(e1,n1,u1, az2, el2, rng2)
call assert_allclose([az2,el2,rng2],[az,el,rng],err_msg='enu2aer-degrees')

!> ECEF2AER
call ecef2aer(x2,y2,z2, lat,lon,alt, az3, el3, rng3)
call assert_allclose([az3,el3,rng3],[az,el,rng], &
rtol=1e-3_wp, err_msg='ecef2aer-degrees')

call ecef2aer(spheroid%SemimajorAxis-1._wp, 0._wp, 0._wp, 0._wp, 0._wp, 0._wp, az3, el3, rng3)
call assert_allclose([az3,el3,rng3], [0._wp, -90._wp, 1._wp], &
err_msg='ecef2aer-degrees')

call ecef2aer(0._wp, spheroid%SemimajorAxis-1._wp, 0._wp, 0._wp, 90._wp, 0._wp, az3, el3, rng3)
call assert_allclose([az3,el3,rng3], [0._wp, -90._wp, 1._wp], &
err_msg='ecef2aer-degrees')

call ecef2aer(0._wp, 0._wp, spheroid%SemimajorAxis-1._wp, 90._wp, 0._wp, 0._wp, az3, el3, rng3)
call assert_allclose([az3,el3,rng3], [0._wp, -90._wp, 1._wp], &
rtol=1e6_wp, err_msg='ecef2aer-degrees')



call aer2geodetic(az,el,rng,lat,lon,alt, lat3,lon3,alt3)
call assert_allclose([lat3,lon3,alt3],[lat1,lon1,alt1], &
rtol=1e-3_wp, err_msg='aer2geodetic-degrees')
Expand All @@ -123,7 +158,7 @@ program Test

call assert_allclose(degrees(radians(deg0)), deg0, err_msg='deg<->rad')

! ------ scalar radians
!> scalar radians

call geodetic2ecef(radians(lat),radians(lon),alt, x1,y1,z1, deg=.false.)
call assert_allclose([x1,y1,z1],[x0,y0,z0])
Expand All @@ -142,6 +177,7 @@ program Test
call assert_allclose([degrees(az2),degrees(el2),rng2],[az,el,rng], &
err_msg='enu2aer: rad')

!> ECEF2AER
call ecef2aer(x2,y2,z2, radians(lat),radians(lon),alt, az3,el3,rng3, deg=.false.)
call assert_allclose([degrees(az3),degrees(el3),rng3],[az,el,rng], &
rtol=1e-3_wp, err_msg='ecef2aer-radians')
Expand Down

0 comments on commit 7676e74

Please sign in to comment.