Skip to content

Commit ffefb49

Browse files
scivisionscivision
committed
submodule aer,ecef
submodule ecef submod enu
1 parent 491b99a commit ffefb49

6 files changed

Lines changed: 600 additions & 549 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ if(NOT realbits)
1212
endif()
1313

1414
# --- Maptran library
15-
add_library(maptran src/maptran.F90 src/vallado.F90)
15+
add_library(maptran src/maptran.F90 src/aer.f90 src/ecef.f90 src/enu.f90 src/vallado.F90)
1616
target_compile_options(maptran PRIVATE ${FFLAGS})
1717
target_compile_definitions(maptran PUBLIC REALBITS=${realbits})
1818

‎meson.build‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ if fc.get_id() == 'gcc'
88
endif
99

1010
# --- Maptran library
11-
maptran = library('maptran', 'src/maptran.F90', 'src/vallado.F90',
11+
maptran = library('maptran', 'src/maptran.F90', 'src/vallado.F90', 'src/aer.f90', 'src/ecef.f90', 'src/enu.f90',
1212
fortran_args : REALBITS)
1313

1414
# --- testing

‎src/aer.f90‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
submodule (maptran) aer
2+
implicit none
3+
4+
contains
5+
6+
7+
module procedure ecef2aer
8+
!! ecef2aer convert ECEF of target to azimuth, elevation, slant range from observer
9+
!!
10+
!! Inputs
11+
!! ------
12+
!! x,y,z: Earth Centered Earth Fixed (ECEF) coordinates of test point (meters)
13+
!! lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
14+
!! spheroid: Ellipsoid parameter struct
15+
!! deg: .true.: degrees
16+
!!
17+
!! Outputs
18+
!! -------
19+
!! az, el, slantrange: look angles and distance to point under test (degrees, degrees, meters)
20+
!! az: azimuth clockwise from local north
21+
!! el: elevation angle above local horizon
22+
23+
real(wp) :: east, north, up
24+
25+
call ecef2enu(x, y, z, lat0, lon0, alt0, east, north, up, spheroid, deg)
26+
call enu2aer(east, north, up, az, el, slantRange, deg)
27+
28+
end procedure ecef2aer
29+
30+
31+
module procedure aer2ecef
32+
! aer2ecef convert azimuth, elevation, range to target from observer to ECEF coordinates
33+
!
34+
! Inputs
35+
! ------
36+
! az, el, slantrange: look angles and distance to point under test (degrees, degrees, meters)
37+
! az: azimuth clockwise from local north
38+
! el: elevation angle above local horizon
39+
! lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
40+
! spheroid: Ellipsoid parameter struct
41+
! deg: .true. degrees
42+
!
43+
! outputs
44+
! -------
45+
! x,y,z: Earth Centered Earth Fixed (ECEF) coordinates of test point (meters)
46+
47+
real(wp) :: x0,y0,z0, e,n,u,dx,dy,dz
48+
49+
50+
!> Origin of the local system in geocentric coordinates.
51+
call geodetic2ecef(lat0, lon0, alt0,x0, y0, z0, spheroid,deg)
52+
!> Convert Local Spherical AER to ENU
53+
call aer2enu(az, el, slantRange, e, n, u,deg)
54+
!> Rotating ENU to ECEF
55+
call enu2uvw(e, n, u, lat0, lon0, dx, dy, dz,deg)
56+
!> Origin + offset from origin equals position in ECEF
57+
x = x0 + dx
58+
y = y0 + dy
59+
z = z0 + dz
60+
61+
end procedure aer2ecef
62+
63+
64+
module procedure geodetic2aer
65+
! geodetic2aer from an observer's perspective, convert target coordinates to azimuth, elevation, slant range.
66+
!
67+
! Inputs
68+
! ------
69+
! lat,lon, alt: ellipsoid geodetic coordinates of point under test (degrees, degrees, meters)
70+
! lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
71+
! spheroid: Ellipsoid parameter struct
72+
! angleUnit: string for angular units. Default 'd': degrees, otherwise Radians
73+
!
74+
! Outputs
75+
! -------
76+
! az, el, slantrange: look angles and distance to point under test (degrees, degrees, meters)
77+
! az: azimuth clockwise from local north
78+
! el: elevation angle above local horizon
79+
80+
real(wp) :: east,north,up
81+
82+
83+
call geodetic2enu(lat, lon, alt, lat0, lon0, alt0, east,north,up, spheroid, deg)
84+
call enu2aer(east, north, up, az, el, slantRange, deg)
85+
86+
end procedure geodetic2aer
87+
88+
89+
module procedure aer2geodetic
90+
!! aer2geodetic convert azimuth, elevation, range of target from observer to geodetic coordiantes
91+
!!
92+
!! ## Inputs
93+
!!
94+
!! * az, el, slantrange: look angles and distance to point under test (degrees, degrees, meters)
95+
!! * az: azimuth clockwise from local north
96+
!! * el: elevation angle above local horizon
97+
!! * lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
98+
!! * spheroid: Ellipsoid parameter struct
99+
!! * deg: .true.: degrees
100+
!!
101+
!! ## Outputs
102+
!!
103+
!! * lat1,lon1,alt1: geodetic coordinates of test points (degrees,degrees,meters)
104+
105+
real(wp) :: x,y,z
106+
107+
call aer2ecef(az, el, slantRange, lat0, lon0, alt0, x, y, z, spheroid, deg)
108+
109+
call ecef2geodetic(x, y, z, lat1, lon1, alt1, spheroid, deg)
110+
end procedure aer2geodetic
111+
112+
end submodule aer

‎src/ecef.f90‎

Lines changed: 247 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,247 @@
1+
submodule (maptran) ecef
2+
implicit none
3+
4+
contains
5+
6+
7+
module procedure ecef2geodetic
8+
!! convert ECEF (meters) to geodetic coordintes
9+
!!
10+
!! based on:
11+
!! You, Rey-Jer. (2000). Transformation of Cartesian to Geodetic Coordinates without Iterations.
12+
!! Journal of Surveying Engineering. doi: 10.1061/(ASCE)0733-9453
13+
14+
real(wp) :: ea, eb, r, E, u, Q, huE, Beta, eps, sinBeta, cosBeta
15+
type(Ellipsoid) :: ell
16+
logical :: d, inside
17+
18+
ell = wgs84Ellipsoid
19+
if (present(spheroid)) ell = spheroid
20+
21+
ea = ell%SemimajorAxis
22+
eb = ell%SemiminorAxis
23+
24+
r = sqrt(x**2 + y**2 + z**2)
25+
26+
E = sqrt(ea**2 - eb**2)
27+
28+
! eqn. 4a
29+
u = sqrt(0.5 * (r**2 - E**2) + 0.5 * sqrt((r**2 - E**2)**2 + 4 * E**2 * z**2))
30+
31+
Q = hypot(x, y)
32+
33+
huE = hypot(u, E)
34+
35+
!> eqn. 4b
36+
Beta = atan2(huE / u * z, hypot(x, y))
37+
38+
!> final output
39+
if (abs(beta-pi/2) <= epsilon(beta)) then !< singularity
40+
lat = pi/2
41+
cosBeta = 0._wp
42+
sinBeta = 1._wp
43+
elseif (abs(beta+pi/2) <= epsilon(beta)) then !< singularity
44+
lat = -pi/2
45+
cosBeta = 0._wp
46+
sinBeta = -1._wp
47+
else
48+
!> eqn. 13
49+
eps = ((eb * u - ea * huE + E**2) * sin(Beta)) / (ea * huE * 1 / cos(Beta) - E**2 * cos(Beta))
50+
Beta = Beta + eps
51+
52+
lat = atan(ea / eb * tan(Beta))
53+
cosBeta = cos(Beta)
54+
sinBeta = sin(Beta)
55+
endif
56+
57+
lon = atan2(y, x)
58+
59+
! eqn. 7
60+
if (present(alt)) then
61+
alt = hypot(z - eb * sinBeta, Q - ea * cosBeta)
62+
63+
!> inside ellipsoid?
64+
inside = x**2 / ea**2 + y**2 / ea**2 + z**2 / eb**2 < 1._wp
65+
if (inside) alt = -alt
66+
endif
67+
68+
69+
d=.true.
70+
if (present(deg)) d = deg
71+
72+
if (d) then
73+
lat = degrees(lat)
74+
lon = degrees(lon)
75+
endif
76+
77+
end procedure ecef2geodetic
78+
79+
80+
module procedure geodetic2ecef
81+
!! # geodetic2ecef
82+
!! convert from geodetic to ECEF coordiantes
83+
!!
84+
!! ## Inputs
85+
!!
86+
!! * lat,lon, alt: ellipsoid geodetic coordinates of point(s) (degrees, degrees, meters)
87+
!! * spheroid: Ellipsoid parameter struct
88+
!! * deg: .true. degrees
89+
!!
90+
!! ## outputs
91+
!!
92+
!! * x,y,z: ECEF coordinates of test point(s) (meters)
93+
94+
real(wp) :: N, sinLat, cosLat, cosLon, sinLon, lt, ln
95+
type(Ellipsoid) :: ell
96+
logical :: d
97+
98+
d = .true.
99+
if (present(deg)) d = deg
100+
101+
ell = wgs84Ellipsoid
102+
if (present(spheroid)) ell = spheroid
103+
104+
lt = lat
105+
ln = lon
106+
if (d) then
107+
lt = radians(lat)
108+
ln = radians(lon)
109+
endif
110+
111+
!> Radius of curvature of the prime vertical section
112+
N = radius_normal(lt, ell)
113+
114+
!! Compute cartesian (geocentric) coordinates given (curvilinear) geodetic coordinates.
115+
116+
!> singularities. Benchmark shows nearly zero runtime impact of these if statements for any real precision
117+
if (abs(lt) <= epsilon(lt)) then
118+
cosLat = 1._wp
119+
sinLat = 0._wp
120+
elseif (abs(lt-pi/2) <= epsilon(lt)) then
121+
cosLat = 0._wp
122+
sinLat = 1._wp
123+
elseif (abs(lt+pi/2) <= epsilon(lt)) then
124+
cosLat = 0._wp
125+
sinLat = -1._wp
126+
else
127+
cosLat = cos(lt)
128+
sinLat = sin(lt)
129+
endif
130+
131+
if (abs(ln) <= epsilon(ln)) then
132+
cosLon = 1._wp
133+
sinLon = 0._wp
134+
elseif (abs(ln-pi/2) <= epsilon(ln)) then
135+
cosLon = 0._wp
136+
sinLon = 1._wp
137+
elseif (abs(ln+pi/2) <= epsilon(ln)) then
138+
cosLon = 0._wp
139+
sinLon = -1._wp
140+
elseif (abs(ln+pi) <= epsilon(ln) .or. abs(ln-pi) <= epsilon(ln)) then
141+
cosLon = -1._wp
142+
sinLon = 0._wp
143+
else
144+
cosLon = cos(ln)
145+
sinLon = sin(ln)
146+
endif
147+
148+
149+
x = (N + alt) * cosLat * cosLon
150+
y = (N + alt) * cosLat * sinLon
151+
z = (N * (ell%SemiminorAxis / ell%SemimajorAxis)**2 + alt) * sinLat
152+
153+
end procedure geodetic2ecef
154+
155+
156+
module procedure enu2ecef
157+
! enu2ecef convert from ENU to ECEF coordiantes
158+
!
159+
! Inputs
160+
! ------
161+
! e,n,u: East, North, Up coordinates of test points (meters)
162+
! lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
163+
! spheroid: Ellipsoid parameter struct
164+
! angleUnit: string for angular units. Default 'd': degrees
165+
!
166+
! outputs
167+
! -------
168+
! x,y,z: Earth Centered Earth Fixed (ECEF) coordinates of test point (meters)
169+
170+
real(wp) :: x0,y0,z0,dx,dy,dz
171+
172+
call geodetic2ecef(lat0, lon0, alt0, x0, y0, z0, spheroid, deg)
173+
call enu2uvw(e, n, u, lat0, lon0, dx, dy, dz, deg)
174+
175+
x = x0 + dx
176+
y = y0 + dy
177+
z = z0 + dz
178+
end procedure enu2ecef
179+
180+
181+
module procedure ecef2enu
182+
!! ecef2enu convert ECEF to ENU
183+
!!
184+
!! ## Inputs
185+
!!
186+
!! * x,y,z: Earth Centered Earth Fixed (ECEF) coordinates of test point (meters)
187+
!! * lat0, lon0, alt0: ellipsoid geodetic coordinates of observer/reference (degrees, degrees, meters)
188+
!! * spheroid: Ellipsoid parameter struct
189+
!! * angleUnit: string for angular units. Default 'd': degrees
190+
!!
191+
!! ## outputs
192+
!!
193+
!! * e,n,u: East, North, Up coordinates of test points (meters)
194+
195+
real(wp) :: x0,y0,z0
196+
197+
call geodetic2ecef(lat0, lon0, alt0, x0,y0,z0, spheroid,deg)
198+
call ecef2enuv(x - x0, y - y0, z - z0, lat0, lon0, east, north, up, deg)
199+
end procedure ecef2enu
200+
201+
202+
module procedure ecef2enuv
203+
!! ecef2enuv convert *vector projection* UVW to ENU
204+
!!
205+
!! ## Inputs
206+
!!
207+
!! * u,v,w: meters
208+
!! * lat0,lon0: geodetic latitude and longitude (degrees)
209+
!! * deg: .true. degrees
210+
!!
211+
!! ## Outputs
212+
!!
213+
!! * east,north,Up: East, North, Up vector
214+
215+
real(wp) :: t
216+
logical :: d
217+
218+
d=.true.
219+
if (present(deg)) d = deg
220+
221+
if (d) then
222+
lat0 = radians(lat0)
223+
lon0 = radians(lon0)
224+
endif
225+
226+
t = cos(lon0) * u + sin(lon0) * v
227+
east = -sin(lon0) * u + cos(lon0) * v
228+
up = cos(lat0) * t + sin(lat0) * w
229+
north = -sin(lat0) * t + cos(lat0) * w
230+
end procedure ecef2enuv
231+
232+
233+
elemental real(wp) function radius_normal(lat,E)
234+
235+
real(wp), intent(in) :: lat
236+
type(Ellipsoid), intent(in) :: E
237+
238+
!> singularity pi/2 issue is inherent to real32
239+
if (abs(lat) <= epsilon(lat)) then
240+
radius_normal = E%SemimajorAxis
241+
else
242+
radius_normal = E%SemimajorAxis**2 / sqrt( E%SemimajorAxis**2 * cos(lat)**2 + E%SemiminorAxis**2 * sin(lat)**2 )
243+
endif
244+
245+
end function radius_normal
246+
247+
end submodule ecef

0 commit comments

Comments
 (0)