enu2aer and ecef2geodetic singularity handling

normal singularity

fix geodetic2ecef singularity

addtest

aer2ecef singularity test

Author: scivision

src/maptran.F90

@@ -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
 
@@ -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
@@ -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
 
@@ -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
 
@@ -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
@@ -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
 

tests/test_mod.f90

@@ -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)
@@ -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')
@@ -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])
@@ -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')