map_3d/
lib.rs

1//! map_3d: geographic coordinates conversions
2
3/// Returns the tuple (x,y,z) of coordinates in the ECEF system
4/// with desired reference frame.
5/// 
6/// ## Inputs:
7/// - lat = latitude [rad]
8/// - lon = longitude [rad]
9/// - alt = altitude [m]
10/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
11/// 
12/// ## Outputs:
13/// - x = x ECEF coordinate [m]
14/// - y = y ECEF coordinate [m]
15/// - z = z ECEF coordinate [m]
16pub fn geodetic2ecef(lat: f64, lon: f64, alt: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
17    let n = get_radius_normal(lat, r_ellips);
18    let (major,minor,_,_) = r_ellips.parameters();
19
20    let x = (n + alt) * lat.cos() * lon.cos();
21    let y = (n + alt) * lat.cos() * lon.sin();
22    let z = (n * (minor / major) * (minor / major) + alt) * lat.sin();
23
24    (x,y,z)
25}
26
27/// Returns the tuple (azimuth,elevation,slant range) of coordinates in the AER system
28/// 
29/// ## Inputs:
30/// - lat = input latitude [rad]
31/// - lon = input longitude [rad]
32/// - alt = input altitude [m]
33/// - lat0 = reference latitude [rad]
34/// - lon0 = reference longitude [rad]
35/// - alt0 = reference altitude [m]
36/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
37/// 
38/// ## Outputs:
39/// - az = azimuth angle [rad] of input geodetic location from reference geodetic location
40/// - el = elevation angle [rad] of input geodetic location from reference geodetic location
41/// - slant_range = slant range [m] of input geodetic location from reference geodetic location
42pub fn geodetic2aer(lat: f64, lon: f64, alt: f64,lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
43    let (e,n,u) = geodetic2enu(lat,lon,alt,lat0,lon0,alt0,r_ellips);
44    let (az,el,slant_range) = enu2aer(e, n, u);
45
46    (az,el,slant_range)
47}
48
49/// Returns the tuple (east,north,up) of coordinates in the ENU system
50/// 
51///  ## Inputs:
52/// - lat = input latitude [rad]
53/// - lon = input longitude [rad]
54/// - alt = input altitude [m]
55/// - lat0 = reference latitude [rad]
56/// - lon0 = reference longitude [rad]
57/// - alt0 = reference altitude [m]
58/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
59/// 
60/// ## Outputs:
61/// - e = east coordinate [m] of input geodetic location from reference geodetic location
62/// - n = north coordinate [m] of input geodetic location from reference geodetic location
63/// - u = up coordinate [m] of input geodetic location from reference geodetic location
64pub fn geodetic2enu(lat: f64, lon: f64, alt: f64,lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
65    let (x1,y1,z1) = geodetic2ecef(lat, lon, alt, r_ellips);
66    let (x2,y2,z2) = geodetic2ecef(lat0, lon0, alt0, r_ellips);
67
68    let (e,n,u) = uvw2enu(x1-x2, y1-y2, z1-z2, lat0, lon0);
69
70    (e,n,u)
71} 
72
73/// Returns the tuple (north,east,down) of coordinates in the NED system
74/// 
75///  ## Inputs:
76/// - lat = input latitude [rad]
77/// - lon = input longitude [rad]
78/// - alt = input altitude [m]
79/// - lat0 = reference latitude [rad]
80/// - lon0 = reference longitude [rad]
81/// - alt0 = reference altitude [m]
82/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
83/// 
84/// ## Outputs:
85/// - n = north coordinate [m] of input geodetic location from reference geodetic location
86/// - e = east coordinate [m] of input geodetic location from reference geodetic location
87/// - d = down coordinate [m] of input geodetic location from reference geodetic location
88pub fn geodetic2ned(lat: f64, lon: f64, alt: f64,lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
89    let enu =  geodetic2enu(lat, lon, alt, lat0, lon0, alt0, r_ellips);
90    (enu.1,enu.0,-enu.2)
91}
92
93/// Returns the tuple (x,y,z) of coordinates in the ECEF system
94/// 
95/// ## Inputs:
96/// - az = azimuth angle [rad]
97/// - el = elevation angle [rad]
98/// - slant_range = slant range [m]
99/// - lat0 = reference latitude [rad]
100/// - lon0 = reference longitude [rad]
101/// - alt0 = reference altitude [m]
102/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
103/// 
104/// ## Outputs:
105/// - x = x ECEF coordinate [m]
106/// - y = y ECEF coordinate [m]
107/// - z = z ECEF coordinate [m]
108pub fn aer2ecef(az : f64, el: f64,slant_range :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid)-> (f64,f64,f64){
109
110    let (x0,y0,z0) = geodetic2ecef(lat0, lon0, alt0, r_ellips);
111    let (e,n,u) = aer2enu(az,el,slant_range);
112    let (dx,dy,dz) = enu2uvw(e,n,u,lat0,lon0);
113    (x0+dx,y0+dy,z0+dz)
114}
115
116/// Returns the tuple (east,north,up) of coordinates in the ENU system
117/// 
118/// ## Inputs:
119/// - az = azimuth angle [rad]
120/// - el = elevation angle [rad]
121/// - slant_range = slant range [m]
122/// 
123/// ## Outputs:
124/// - e = east coordinate [m] of input location from reference geodetic location
125/// - n = north coordinate [m] of input location from reference geodetic location
126/// - u = up coordinate [m] of input location from reference geodetic location
127pub fn aer2enu(az : f64, el: f64,slant_range :f64) -> (f64,f64,f64){
128    let r = slant_range*el.cos();
129    (r*az.sin(),r*az.cos(),slant_range*el.sin())
130}
131
132/// Returns the tuple (x,y,z) of coordinates in the ECI system
133/// 
134/// ## Inputs:
135/// - az = azimuth angle [rad]
136/// - el = elevation angle [rad]
137/// - slant_range = slant range [m]
138/// - lat0 = reference latitude [rad]
139/// - lon0 = reference longitude [rad]
140/// - alt0 = reference altitude [m]
141/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
142/// 
143/// ## Outputs:
144/// - x = x ECI coordinate [m]
145/// - y = y ECI coordinate [m]
146/// - z = z ECI coordinate [m]
147pub fn aer2eci(gst :f64, az : f64, el: f64,slant_range :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
148    let (x1,y1,z1) = aer2ecef(az, el, slant_range, lat0, lon0, alt0, r_ellips);
149    ecef2eci(gst, x1, y1, z1)
150}
151
152/// Returns the tuple (north,east,down) of coordinates in the NED system
153/// 
154/// ## Inputs:
155/// - az = azimuth angle [rad]
156/// - el = elevation angle [rad]
157/// - slant_range = slant range [m]
158/// 
159/// ## Outputs:
160/// - n = north coordinate [m] of input location from reference geodetic location
161/// - e = east coordinate [m] of input location from reference geodetic location
162/// - d = down coordinate [m] of input location from reference geodetic location
163pub fn aer2ned(az : f64, el: f64,slant_range :f64) -> (f64,f64,f64){
164    let enu = aer2enu(az, el, slant_range);
165    (enu.1,enu.0,-enu.2)
166}
167
168/// Returns the tuple (latitude,longitude,altitude) of coordinates in the Geodetic system
169/// 
170/// ## Inputs:
171/// - az = azimuth angle [rad]
172/// - el = elevation angle [rad]
173/// - slant_range = slant range [m]
174/// - lat0 = reference latitude [rad]
175/// - lon0 = reference longitude [rad]
176/// - alt0 = reference altitude [m]
177/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
178/// 
179/// ## Outputs:
180/// - lat = input latitude [rad]
181/// - lon = input longitude [rad]
182/// - alt = input altitude [m]
183pub fn aer2geodetic(az : f64, el: f64,slant_range :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid)-> (f64,f64,f64) {
184    let (x,y,z) = aer2ecef(az, el, slant_range, lat0, lon0, alt0, r_ellips);
185    ecef2geodetic(x,y,z,r_ellips)
186}
187
188/// Returns the tuple (u,v,w) of coordinates in the local vector system
189/// 
190/// ## Inputs:
191/// - e = east coordinate [m] from reference geodetic location
192/// - n = north coordinate [m] from reference geodetic location
193/// - u = up coordinate [m] from reference geodetic location
194/// - lat0 = reference latitude [rad]
195/// - lon0 = reference longitude [rad]
196/// 
197/// ## Outputs:
198/// - u = tangent vector component
199/// - v = tangent vector component
200/// - w = tangent vector component
201pub fn enu2uvw(et : f64, nt: f64,up :f64, lat0: f64, lon0: f64) -> (f64,f64,f64){
202    let t = lat0.cos()*up - lat0.sin()*nt;
203
204    let u = lon0.cos()*t-lon0.sin()*et;
205    let v = lon0.sin()*t+lon0.cos()*et;
206    let w = lat0.sin()*up+lat0.cos()*nt;
207    (u,v,w)
208}
209
210/// Returns the tuple (azimuth,elevation,slant range) of coordinates in the AER system
211/// 
212/// ## Inputs:
213/// - e = east coordinate [m] from reference geodetic location
214/// - n = north coordinate [m] from reference geodetic location
215/// - u = up coordinate [m] from reference geodetic location
216/// 
217/// ## Outputs:
218/// - az = azimuth angle [rad] of input location from reference geodetic location
219/// - el = elevation angle [rad] of input location from reference geodetic location
220/// - slant_range = slant range [m] of input location from reference geodetic location
221pub fn enu2aer(e : f64, n: f64,u :f64) -> (f64,f64,f64){
222    let r = (e*e+n*n).sqrt();
223
224    let slant_range = (r*r+u*u).sqrt();
225    let el = u.atan2(r);
226    let az = e.atan2(n).rem_euclid(2.0*std::f64::consts::PI);
227
228    (az,el,slant_range)
229
230}
231
232/// Returns the tuple (x,y,z) of coordinates in the ECEF system
233/// 
234/// ## Inputs:
235/// - e = east coordinate [m] from reference geodetic location
236/// - n = north coordinate [m] from reference geodetic location
237/// - u = up coordinate [m] from reference geodetic location
238/// - lat0 = reference latitude [rad]
239/// - lon0 = reference longitude [rad]
240/// - alt0 = reference altitude [m]
241/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
242/// 
243/// ## Outputs:
244/// - x = x ECEF coordinate [m]
245/// - y = y ECEF coordinate [m]
246/// - z = z ECEF coordinate [m]
247pub fn enu2ecef(e : f64, n: f64, u: f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
248    let (x0,y0,z0) = geodetic2ecef(lat0, lon0, alt0, r_ellips);
249    let (dx,dy,dz) = enu2uvw(e, n, u, lat0, lon0);
250
251    (x0+dx,y0+dy,z0+dz)
252}
253
254/// Returns the tuple (latitude,longitude,altitude) of coordinates in the Geodetic system
255/// 
256/// ## Inputs:
257/// - e = east coordinate [m]  from reference geodetic location
258/// - n = north coordinate [m]  from reference geodetic location
259/// - u = up coordinate [m]  from reference geodetic location
260/// - lat0 = reference latitude [rad]
261/// - lon0 = reference longitude [rad]
262/// - alt0 = reference altitude [m]
263/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
264/// ## Outputs:
265/// - lat = latitude [rad]
266/// - lon = longitude [rad]
267/// - alt = altitude [m]
268pub fn enu2geodetic(e : f64, n: f64,u :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
269    let (x,y,z) = enu2ecef(e, n, u, lat0, lon0, alt0, r_ellips);
270    let (lat,lon,alt) = ecef2geodetic(x, y, z, r_ellips);
271
272    (lat,lon,alt)
273}
274
275/// Returns the tuple (x,y,z) of coordinates in the ECI system
276///
277/// ## Inputs:
278/// - x = x ECEF coordinate [m]
279/// - y = y ECEF coordinate [m]
280/// - z = z ECEF coordinate [m]
281/// 
282/// ## Outputs:
283/// - x = x ECI coordinate [m]
284/// - y = y ECI coordinate [m]
285/// - z = z ECI coordinate [m]
286pub fn ecef2eci(gst :f64,x: f64, y: f64, z: f64) -> (f64,f64,f64){
287    let arr = matmul3(transpose3(r3(gst)),[x,y,z]);
288    (arr[0],arr[1],arr[2])
289
290}
291
292/// Returns the tuple (latitude,longitude,altitude) of coordinates in the Geodetic system
293/// with desired reference frame.
294/// 
295/// ## Inputs:
296/// - x = x ECEF coordinate [m]
297/// - y = y ECEF coordinate [m]
298/// - z = z ECEF coordinate [m]
299/// - r_ellips = reference ellipsoid, defaults to WGS84 
300/// 
301/// ## Outputs:
302/// - lat = latitude [rad]
303/// - lon = longitude [rad]
304/// - alt = altitude [m]
305pub fn ecef2geodetic(x: f64, y: f64, z: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
306    let (major, minor, _,_) = r_ellips.parameters();
307
308    let r = (x*x+y*y+z*z).sqrt();
309    let e = (major*major-minor*minor).sqrt();
310    let var = r*r-e*e;
311    let u = (0.5*var+0.5*(var*var+4.0*e*e*z*z).sqrt()).sqrt();
312
313    let q = (x*x+y*y).sqrt();
314    let hu_e = (u*u+e*e).sqrt();
315    let mut beta = (hu_e/u * z/q).atan();
316
317    let eps =  ((minor * u - major * hu_e + e*e)*beta.sin())/
318                    (major * hu_e/beta.cos() - e*e*beta.cos());
319    beta += eps;
320
321    let lat = (major/minor*beta.tan()).atan();
322    let lon = y.atan2(x);
323
324    let v1 = z - minor * beta.sin();
325    let v2 = q - major*beta.cos();
326    let alt;
327
328    let inside = (x*x/major/major)+(y*y/major/major)+(z*z/minor/minor)<1.0;
329    if inside {
330        alt = -(v1*v1+v2*v2).sqrt();
331    }else {
332        alt = (v1*v1+v2*v2).sqrt();
333    };
334
335    (lat,lon,alt)
336} 
337
338/// Returns the tuple (east,north,up) of coordinates in the ENU system
339/// 
340/// ## Inputs:
341/// - x = x ECEF coordinate [m]
342/// - y = y ECEF coordinate [m]
343/// - z = z ECEF coordinate [m]
344/// - lat0 = reference latitude [rad]
345/// - lon0 = reference longitude [rad]
346/// - alt0 = reference altitude [m]
347/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
348/// 
349/// ## Outputs:
350/// - e = east coordinate [m] of input ECEF location from reference geodetic location
351/// - n = north coordinate [m] of input ECEF location from reference geodetic location
352/// - u = up coordinate [m] of input ECEF location from reference geodetic location
353pub fn ecef2enu(x: f64, y: f64, z: f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid)-> (f64,f64,f64){
354    let (x0,y0,z0) = geodetic2ecef(lat0, lon0, alt0,r_ellips);
355    let (e,n,u) = uvw2enu(x-x0,y-y0,z-z0,lat0,lon0);
356    (e,n,u)
357}
358
359/// Returns the tuple (north,east,down) of coordinates in the NED system
360/// 
361/// ## Inputs:
362/// - x = x ECEF coordinate [m]
363/// - y = y ECEF coordinate [m]
364/// - z = z ECEF coordinate [m]
365/// - lat0 = reference latitude [rad]
366/// - lon0 = reference longitude [rad]
367/// - alt0 = reference altitude [m]
368/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
369/// 
370/// ## Outputs:
371/// - n = north coordinate [m] of input location from reference geodetic location
372/// - e = east coordinate [m] of input location from reference geodetic location
373/// - d = down coordinate [m] of input location from reference geodetic location
374pub fn ecef2ned(x: f64, y: f64, z: f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid)-> (f64,f64,f64){
375    let enu = ecef2enu(x, y, z, lat0, lon0, alt0, r_ellips);
376    (enu.1,enu.0,-enu.2)
377}
378
379/// Returns the tuple (east,north,up) of coordinates in the ENU system
380/// 
381/// ## Inputs:
382/// - u = tangent vector component
383/// - v = tangent vector component
384/// - w = tangent vector component
385/// - lat0 = reference latitude [rad]
386/// - lon0 = reference longitude [rad]
387/// 
388/// ## Outputs:
389/// - e = east coordinate [m] of input location from reference geodetic location
390/// - n = north coordinate [m] of input location from reference geodetic location
391/// - u = up coordinate [m] of input location from reference geodetic location
392pub fn uvw2enu(u : f64, v: f64,w :f64, lat0: f64, lon0: f64) -> (f64,f64,f64){
393    let t = lon0.cos() * u + lon0.sin() * v;
394    let e = -lon0.sin() * u + lon0.cos() * v;
395    let n = -lat0.sin() * t + lat0.cos() * w;
396    let u = lat0.cos() * t + lat0.sin() * w;
397    (e,n,u)
398}
399
400/// Returns the tuple (azimuth,elevation,slant range) of coordinates in the AER system
401/// 
402/// ## Inputs:
403/// - x = x ECEF coordinate [m]
404/// - y = y ECEF coordinate [m]
405/// - z = z ECEF coordinate [m]
406/// - lat0 = reference latitude [rad]
407/// - lon0 = reference longitude [rad]
408/// - alt0 = reference altitude [m]
409/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
410/// 
411/// ## Outputs:
412/// - az = azimuth angle [rad] of input location from reference geodetic location
413/// - el = elevation angle [rad] of input location from reference geodetic location
414/// - slant_range = slant range [m] of input location from reference geodetic location
415pub fn ecef2aer(x: f64, y: f64, z: f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid)-> (f64,f64,f64){
416    let (e,n,u) = ecef2enu(x, y, z, lat0, lon0, alt0, r_ellips);
417    let (az,el,slant_range) = enu2aer(e,n,u);
418
419    (az,el,slant_range)
420}
421
422/// Returns the tuple (azimuth,elevation,slant range) of coordinates in the AER system
423///  
424/// ## Inputs:
425/// - x = x ECI coordinate [m]
426/// - y = y ECI coordinate [m]
427/// - z = z ECI coordinate [m]
428/// - lat = reference latitude [rad]
429/// - lon = reference longitude [rad]
430/// - alt = reference altitude [m]
431/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
432/// 
433/// ## Outputs:
434/// - az = azimuth angle [rad] of input location from reference geodetic location
435/// - el = elevation angle [rad] of input location from reference geodetic location
436/// - slant_range = slant range [m] of input location from reference geodetic location
437pub fn eci2aer(gst :f64,x: f64, y: f64, z: f64, lat :f64, lon :f64, alt :f64, r_ellips: Ellipsoid)-> (f64,f64,f64){
438    let (x,y,z) = eci2ecef(gst, x, y, z);
439    let (az,el,slant_range) = ecef2aer(x, y, z, lat, lon, alt, r_ellips);
440    (az,el,slant_range)
441}
442
443/// Returns the tuple (x,y,z) of coordinates in the ECEF system
444/// 
445/// ## Inputs:
446/// - gst = greenwhich sidereal time
447/// - x = x ECI coordinate [m]
448/// - y = y ECI coordinate [m]
449/// - z = z ECI coordinate [m]
450/// 
451/// ## Outputs:
452/// - x = x ECEF coordinate [m]
453/// - y = y ECEF coordinate [m]
454/// - z = z ECEF coordinate [m]
455pub fn eci2ecef(gst :f64,x: f64, y: f64, z: f64)-> (f64,f64,f64){
456    let arr = matmul3(r3(gst),[x,y,z]);
457    (arr[0],arr[1],arr[2])
458}
459
460/// Returns the tuple (azimuth,elevation,slant range) of coordinates in the AER system
461/// 
462/// ## Inputs:
463/// - n = north coordinate [m] of input location from reference geodetic location
464/// - e = east coordinate [m] of input location from reference geodetic location
465/// - d = down coordinate [m] of input location from reference geodetic location
466/// 
467/// ## Outputs:
468/// - az = azimuth angle [rad] of input location from reference geodetic location
469/// - el = elevation angle [rad] of input location from reference geodetic location
470/// - slant_range = slant range [m] of input location from reference geodetic location
471pub fn ned2aer(n : f64, e: f64,d :f64) -> (f64,f64,f64){
472    let aer = enu2aer(e, n, -d);
473    aer
474}
475
476/// Returns the tuple (latitude,longitude,altitude) of coordinates in the Geodetic system
477/// 
478/// ## Inputs:
479/// - n = north coordinate [m] of input location from reference geodetic location
480/// - e = east coordinate [m] of input location from reference geodetic location
481/// - d = down coordinate [m] of input location from reference geodetic location
482/// - lat0 = reference latitude [rad]
483/// - lon0 = reference longitude [rad]
484/// - alt0 = reference altitude [m]
485/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
486/// 
487/// ## Outputs:
488/// - lat = latitude [rad]
489/// - lon = longitude [rad]
490/// - alt = altitude [m]
491pub fn ned2geodetic(n : f64, e: f64,d :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
492    let geo = enu2geodetic(e, n, -d, lat0, lon0, alt0, r_ellips);
493    geo
494}
495
496/// Returns the tuple (x,y,z) of coordinates in the ECEF system
497/// 
498/// ## Inputs:
499/// - n = north coordinate [m] from reference geodetic location
500/// - e = east coordinate [m] from reference geodetic location
501/// - d = down coordinate [m] from reference geodetic location
502/// - lat0 = reference latitude [rad]
503/// - lon0 = reference longitude [rad]
504/// - alt0 = reference altitude [m]
505/// - r_ellips = reference ellipsoid, defaults ref. frame is WGS84 
506/// 
507/// ## Outputs:
508/// - x = x ECEF coordinate [m]
509/// - y = y ECEF coordinate [m]
510/// - z = z ECEF coordinate [m]
511pub fn ned2ecef(n: f64,e : f64, d :f64, lat0: f64, lon0: f64, alt0: f64, r_ellips: Ellipsoid) -> (f64,f64,f64){
512    let ecef = enu2ecef(e, n, -d, lat0, lon0, alt0, r_ellips);
513    ecef
514}
515
516/// Returns the array result of 3-by-3-matrix that multiplies a 3-by-1 column array
517pub fn matmul3(matrix: [f64;9],col:[f64;3])->[f64;3]{
518    let out : [f64;3] = [ matrix[0]*col[0]+matrix[1]*col[1]+matrix[2]*col[2],
519                        matrix[3]*col[0]+matrix[4]*col[1]+matrix[5]*col[2],
520                        matrix[6]*col[0]+matrix[7]*col[1]+matrix[8]*col[2]];
521    out
522}
523
524/// Returns the array representing a 3-by-3 rotation matrix of the input
525pub fn r3(x : f64) -> [f64; 9] {
526    [x.cos(),x.sin(),0.0,-x.sin(),x.cos(),0.0,0.0,0.0,1.0]
527}
528
529/// Returns the array representing the transpose of the input 3-by-3 matrix
530pub fn transpose3(x: [f64;9]) -> [f64;9] {
531    [x[0],x[3],x[6],x[1],x[4],x[7],x[2],x[5],x[8]]
532}
533
534#[derive(Debug, Copy, Clone)]
535pub enum Ellipsoid {
536    /// WGS84: GPS Ellipsoid frame  
537    /// semi-major axis: 6378137.0 [m]  
538    /// flattening: 1.0/298.2572235630
539    WGS84,
540    /// WGS72: semi-major axis: 6378135.0 [m]    
541    /// flattening: 1.0/298.26
542    WGS72,
543    /// WGS66: semi-major axis: 6378145.0 [m]    
544    /// flattening: 1.0/298.25
545    WGS66,
546    /// WGS60: semi-major axis: 6378165.0 [m]    
547    /// flattening: 1.0/298.3
548    WGS60,
549    /// PZ90: Glonass Ellipsoid frame  
550    /// semi-major axis: 6378136.0 [m] 
551    /// flattening: 1/298.257839303
552    PZ90,
553    /// BDC, also known as CGCS2000,
554    /// is the reference frame used by the
555    /// Beidou constellation.  
556    /// Semi-major axis: 6378137.0 [m] 
557    /// flattening: 1/298.257222101
558    BDC,
559    /// GRS80 reference ellipsoid  
560    /// semi-major axis: 6378137.0 [m]  
561    /// flattening: 1.0/298.257222101
562    GRS80,
563    /// Bessel reference ellipsoid   
564    /// semi-major axis: 6377397.155 [m] 
565    /// flattening: 1.0/299.1528128
566    Bessel,
567    /// Airy reference ellipsoid   
568    /// semi-major axis: 6377563.396 [m]  
569    /// flattening: 1.0/299.3249646
570    Airy,
571    /// International reference ellipsoid   
572    /// semi-major axis: 6378388.0 [m]  
573    /// flattening: 1.0/297.0
574    International,
575}
576
577impl Default for Ellipsoid {
578    fn default() -> Ellipsoid {
579        Ellipsoid::WGS84
580    }
581}
582
583impl Ellipsoid {
584    /// Returns the tuple representing the Ellipsoid frame.
585    /// 
586    /// ## Outputs:
587    /// - tuple.0 = semi-major axis [m]
588    /// - tuple.1 = semi-minor axis [m]
589    /// - tuple.2 = flattening [-]
590    /// - tuple.3 = squared eccentricity [rad^2]
591    pub fn parameters(&self) -> (f64,f64,f64,f64) {
592        let (major, flattening): (f64, f64) = match self {
593            Ellipsoid::WGS84 => (6378137.0, 1.0/298.257223563),
594            Ellipsoid::WGS72 => (6378135.0, 1.0/298.26),
595            Ellipsoid::WGS66 => (6378145.0, 1.0/298.25),
596            Ellipsoid::WGS60 => (6378165.0, 1.0/298.3),
597            Ellipsoid::PZ90 =>  (6378136.0, 1.0/298.257839303),
598            Ellipsoid::BDC =>   (6378137.0, 1.0/298.257222101),
599            Ellipsoid::GRS80 => (6378137.0, 1.0/298.2572221009),
600            Ellipsoid::Bessel =>(6377397.155, 299.1528128),
601            Ellipsoid::Airy  => (6377563.396, 299.3249646),
602            Ellipsoid::International => (6378388.0, 297.0),
603        };
604
605        let minor = major * (1.0 - flattening);
606        let ecc_sq = ((major*major)-(minor*minor))/(major*major);
607        (major,minor,flattening,ecc_sq)
608    }
609}
610
611/// Returns the normal radius based on given latitude
612/// and desired reference frame
613pub fn get_radius_normal(lat: f64, r_ellips: Ellipsoid)->f64 {
614    let (major,_,_,squared_eccentricity) = r_ellips.parameters();
615    major/((1.0-squared_eccentricity*lat.sin()*lat.sin()).sqrt())
616}
617
618/// Returns the radians [rad] value of the decimal degree [deg] input
619pub fn deg2rad(x: f64) -> f64 {
620    x/180.0*std::f64::consts::PI
621}
622
623/// Returns the decimal degree [deg] value of the radians [rad] input
624pub fn rad2deg(x: f64) -> f64 {
625    x*180.0/std::f64::consts::PI
626}
627
628/// Returns the GST time as f64
629/// 
630/// ## Input
631/// UTC time defined as: [year,month,day,hour,minute,second]
632/// 
633/// ## Output
634/// Gst time as f64
635pub fn utc2gst(utc: [i32;6]) -> f64 {
636
637    let mut year = utc[0] as f64;
638    let mut month = utc[1] as f64;
639    let day = utc[2] as f64;
640    let h = utc[3] as f64;
641    let m = utc[4] as f64;
642    let s = utc[5] as f64;
643
644    if month<3.0 {
645        year = year - 1.0;
646        month = month + 12.0;
647    }
648
649    let a = fix(year/100.0);
650
651    let b = 2.0 - a + fix(a/4.0);
652
653    let c = ((s/60.0 + m)/60.0 + h)/24.0;
654    
655    let jd = fix(365.25 * (year + 4716.0)) + fix(30.6001*(month + 1.0)) + day + b - 1524.5 + c;
656    
657    let t_ut1 = (jd - 2451545.0)/36525.0;
658
659    let gmst_sec = 67310.54841 + 3.164400184812866e+09 * t_ut1 + 0.093104 * t_ut1 * t_ut1 
660                        - 6.2e-6 * t_ut1 * t_ut1 * t_ut1;
661    
662    let gst = (gmst_sec * 2.0 * std::f64::consts::PI / 86400.0).rem_euclid(2.0 * std::f64::consts::PI);
663    gst
664}
665
666/// Return the round toward zero value of the input
667pub fn fix(x : f64) -> f64 {
668    let mut out = x;
669    if out<0.0 {
670        out = x.ceil();
671    } else {
672        out = x.floor();
673    }
674    out
675}
676
677/// Earth radius (m)
678pub const EARTH_RADIUS: f64 = 6371E3_f64;
679
680/// Returns distance (m) between two decimal degrees coordinates:: 
681/// coord1: (lat,lon), coord2: (lat, lon)
682pub fn distance (coord1: (f64,f64), coord2: (f64,f64)) -> f64 {
683    let dphi = deg2rad(coord2.0) - deg2rad(coord1.0);
684    let d_lambda = deg2rad(coord2.1) - deg2rad(coord1.1);
685    let a: f64 = (dphi / 2.0_f64).sin().powf(2.0_f64) 
686        + deg2rad(coord1.0).cos() * deg2rad(coord2.0).cos() 
687            * (d_lambda/2.0_f64).sin().powf(2.0_f64);
688    let c = 2.0_f64 * a.powf(0.5_f64).atan2((1.0-a).powf(0.5_f64));
689    EARTH_RADIUS * c
690}
691
692#[cfg(test)]
693mod tests {
694    use super::*;
695
696    #[test]
697    fn test_utc2gst() {
698
699        let datetime: [i32;6] = [2020,5,12,18,2,10];
700        let gst_ref = 2.469809475597415;
701
702        let gst = utc2gst(datetime);
703
704        assert!( (gst-gst_ref).abs()<1e-8);
705
706        let datetime2: [i32;6] = [2020,1,12,18,2,10];
707        let gst_ref2 = 0.388271658105431;
708
709        let gst2 = utc2gst(datetime2);
710
711        assert!( (gst2-gst_ref2).abs()<1e-8);
712    }
713
714    #[test]
715    fn test_fix() {
716        let x1 = 3.7;
717        let x2 = -4.67;
718
719        assert_eq!(fix(x1),3.0);
720        assert_eq!(fix(x2),-4.0);
721    }
722    
723    #[test]
724    fn test_deg2rad() {
725        assert!((deg2rad(0.0)-0.0).abs()<1e-4);
726        assert!((deg2rad(90.0)-std::f64::consts::PI/2.0).abs()<1e-4);
727        assert!((deg2rad(-90.0)+std::f64::consts::PI/2.0).abs()<1e-4);
728        assert!((deg2rad(45.0)-std::f64::consts::PI/4.0).abs()<1e-4);
729        assert!((deg2rad(270.0)-std::f64::consts::PI*6.0/4.0).abs()<1e-4);
730        assert!((deg2rad(360.0)-std::f64::consts::PI*2.0).abs()<1e-4);
731    }
732
733    #[test]
734    fn test_rad2deg() {
735        assert!((0.0-rad2deg(0.0)).abs()<1e-4);
736        assert!((90.0-rad2deg(std::f64::consts::PI/2.0)).abs()<1e-4);
737        assert!((-90.0+rad2deg(std::f64::consts::PI/2.0)).abs()<1e-4);
738        assert!((45.0-rad2deg(std::f64::consts::PI/4.0)).abs()<1e-4);
739        assert!((270.0-rad2deg(std::f64::consts::PI*6.0/4.0)).abs()<1e-4);
740        assert!((360.0-rad2deg(std::f64::consts::PI*2.0)).abs()<1e-4);
741    }
742
743    #[test]
744    fn test_geodetic2ecef() {
745
746        let lat = deg2rad(30.14988205);
747        let lon = deg2rad(91.38733072);
748        let alt = 4031.0;
749
750        let (x,y,z) = geodetic2ecef(lat,lon,alt, Ellipsoid::default());
751
752        let xref = -1.337281037300386e+05;
753        let yref = 5.521796910920261e+06;
754        let zref = 3.186776473672415e+06;
755
756        assert!((x-xref).abs()<1e-3);
757        assert!((y-yref).abs()<1e-3);
758        assert!((z-zref).abs()<1e-3);
759    }
760
761    #[test]
762    fn test_geodetic2aer() {
763        
764        let lat0 = deg2rad(42.0);
765        let lon0 = deg2rad(-82.0);
766        let alt0 = 200.0;
767
768        let lat = deg2rad(42.002581974253744);
769        let lon = deg2rad(-81.997751960067460);
770        let alt = 1.139701799575106e+03;
771        
772        let azref = deg2rad(32.999999999989740);
773        let elref = deg2rad(69.999999999945540);
774        let rangeref = 1000.0;
775
776        let (a,e,r) = geodetic2aer(lat, lon, alt, lat0, lon0, alt0, Ellipsoid::default());
777
778        assert!((a-azref).abs()<1e-3);
779        assert!((e-elref).abs()<1e-3);
780        assert!((r-rangeref).abs()<1e-3);
781    }
782
783    #[test]
784    fn test_geodetic2enu() {
785        let lat0 = deg2rad(42.0);
786        let lon0 = deg2rad(-82.0);
787        let alt0 = 200.0;
788
789        let lat = deg2rad(42.002581974253744);
790        let lon = deg2rad(-81.997751960067460);
791        let alt = 1.139701799575106e+03;
792
793        let eref = 1.862775208168244e+02;
794        let nref = 2.868422278521820e+02;
795        let uref = 9.396926207845534e+02;
796
797        let (e,n,u) = geodetic2enu(lat, lon, alt, lat0, lon0, alt0, Ellipsoid::default());
798
799        assert!((e-eref).abs()<1e-3);
800        assert!((n-nref).abs()<1e-3);
801        assert!((u-uref).abs()<1e-3);
802    }
803
804    #[test]
805    fn test_aer2ecef() {
806        let lat0 = deg2rad(42.0);
807        let lon0 = deg2rad(-82.0);
808        let alt0 = 200.0;
809
810        let az = deg2rad(33.0);
811        let el = deg2rad(70.0);
812        let slant_range = 1000.0;
813
814        let (x,y,z) = aer2ecef(az,el,slant_range,lat0,lon0,alt0, Ellipsoid::default());
815
816
817        let xref = 6.609301927610816e+05;
818        let yref = -4.701424222957011e+06;
819        let zref = 4.246579604632881e+06;
820
821        assert!((x-xref).abs()<1e-3);
822        assert!((y-yref).abs()<1e-3);
823        assert!((z-zref).abs()<1e-3);
824    }
825
826    #[test]
827    fn test_aer2enu() {
828        let az = deg2rad(33.0);
829        let el = deg2rad(70.0);
830        let slant_range = 1000.0;
831
832        let eref = 1.862775208165935e+02;
833        let nref = 2.868422278517140e+02;
834        let uref = 9.396926207859083e+02;
835
836        let (e,n,u)= aer2enu(az, el, slant_range);
837
838        assert!((e-eref).abs()<1e-3);
839        assert!((n-nref).abs()<1e-3);
840        assert!((u-uref).abs()<1e-3);
841    }
842
843    #[test]
844    fn test_aer2eci() {
845        let az = deg2rad(162.55);
846        let el = deg2rad(55.12);
847        let slant_range = 384013940.9;
848        let gst = 4.501012562811752;
849
850        let lat0 = deg2rad(28.4);
851        let lon0 = deg2rad(-80.5);
852        let alt0 = 2.7;
853
854        let xref = -3.849714979138141e+08;
855        let yref = -4.836588977863766e+07;
856        let zref = -3.143285462295778e+07;
857
858        let (x,y,z) = aer2eci(gst, az, el, slant_range, lat0, lon0, alt0, Ellipsoid::default());
859
860        assert!((x-xref).abs()<1e-3);
861        assert!((y-yref).abs()<1e-3);
862        assert!((z-zref).abs()<1e-3);
863    }
864 
865    #[test]
866    fn test_aer2geodetic() {
867        let lat0 = deg2rad(42.0);
868        let lon0 = deg2rad(-82.0);
869        let alt0 = 200.0;
870        
871        let az = deg2rad(32.999999999989740);
872        let el = deg2rad(69.999999999945540);
873        let slant_range = 1000.0;
874
875        let latref = deg2rad(42.002581974253744);
876        let lonref = deg2rad(-81.997751960067460);
877        let altref = 1.139701799575106e+03;
878        
879
880        let (lat,lon,alt) = aer2geodetic(az, el, slant_range, lat0, lon0, alt0, Ellipsoid::default());
881
882        assert!((lat-latref).abs()<1e-8);
883        assert!((lon-lonref).abs()<1e-8);
884        assert!((alt-altref).abs()<1e-8);
885    }
886
887    #[test]
888    fn test_enu2aer() {
889        let e = 1.862775210000000e+02;
890        let n = 2.868422200000000e+02;
891        let u = 9.396926200000000e+02;
892
893        let azref = deg2rad(33.0);
894        let elref = deg2rad(70.0);
895        let rangeref = 1000.0;
896
897        let (az,el,range) = enu2aer(e, n, u);
898
899        assert!((az-azref).abs()<1e-3);
900        assert!((el-elref).abs()<1e-3);
901        assert!((range-rangeref).abs()<1e-3);        
902    }
903
904    #[test]
905    fn test_enu2ecef() {
906        let lat0 = deg2rad(42.0);
907        let lon0 = deg2rad(-82.0);
908        let alt0 = 200.0;
909        let e = 1.862775210000000e+02;
910        let n = 2.868422200000000e+02;
911        let u = 9.396926200000000e+02;
912
913        let xref = 6.609301927610815e+05;
914        let yref = -4.701424222957011e+06;
915        let zref = 4.246579604632881e+06;
916        
917        let (x,y,z) = enu2ecef(e, n, u, lat0, lon0, alt0, Ellipsoid::default());
918
919        assert!((x-xref).abs()<1e-3);
920        assert!((y-yref).abs()<1e-3);
921        assert!((z-zref).abs()<1e-3);
922    }
923
924    #[test]
925    fn test_enu2geodetic() {
926        let lat0 = deg2rad(42.0);
927        let lon0 = deg2rad(-82.0);
928        let alt0 = 200.0;
929        let e = 0.0;
930        let n = 0.0;
931        let u = -1.0;
932
933        let latref = deg2rad(41.999999999999993);
934        let lonref = deg2rad(-82.0);
935        let altref = 1.990000000007368e+02;
936
937        let (lat,lon,alt) = enu2geodetic(e, n, u, lat0, lon0, alt0, Ellipsoid::default());
938
939        assert!((lat-latref).abs()<1e-8);
940        assert!((lon-lonref).abs()<1e-8);
941        assert!((alt-altref).abs()<1e-8);
942    }
943    
944    #[test]
945    fn test_ecef2geodetic() {
946        let latref = deg2rad(30.14988205);
947        let lonref = deg2rad(91.38733072);
948        let altref = 4031.0;
949
950        let (x,y,z) = geodetic2ecef(latref, lonref, altref, Ellipsoid::default());
951        let (lat,lon,alt) = ecef2geodetic(x, y, z, Ellipsoid::default());
952        
953        assert!((lat-latref).abs()<1e-8);
954        assert!((lon-lonref).abs()<1e-8);
955        assert!((alt-altref).abs()<1e-8);
956
957        let (x,y,z) = geodetic2ecef(latref, lonref, altref-5000.0, Ellipsoid::default());
958        let (lat,lon,alt) = ecef2geodetic(x, y, z, Ellipsoid::default());
959        
960        assert!((lat-latref).abs()<1e-8);
961        assert!((lon-lonref).abs()<1e-8);
962        assert!((alt-(altref-5000.0)).abs()<1e-8);
963    }
964
965    #[test]
966    fn test_ecef2enu() {
967        let lat0 = deg2rad(42.0);
968        let lon0 = deg2rad(-82.0);
969        let alt0 = 200.0;
970        let eref = 1.862775210000000e+02;
971        let nref = 2.868422200000000e+02;
972        let uref = 9.396926200000000e+02;
973        
974        let (x,y,z) = enu2ecef(eref, nref, uref, lat0, lon0, alt0, Ellipsoid::default());
975        let (e,n,u)= ecef2enu(x, y, z, lat0, lon0, alt0, Ellipsoid::default());
976
977        assert!((e-eref).abs()<1e-3);
978        assert!((n-nref).abs()<1e-3);
979        assert!((u-uref).abs()<1e-3);
980    }
981
982    #[test]
983    fn test_ecef2aer() {
984        let lat0 = deg2rad(42.0);
985        let lon0 = deg2rad(-82.0);
986        let alt0 = 200.0;
987
988        let azref = deg2rad(33.0);
989        let elref = deg2rad(70.0);
990        let rangeref = 1000.0;
991
992        let (x,y,z) = aer2ecef(azref, elref, rangeref, lat0, lon0, alt0, Ellipsoid::default());
993        let (az,el,range) = ecef2aer(x, y, z, lat0, lon0, alt0, Ellipsoid::default());
994
995        assert!((az-azref).abs()<1e-3);
996        assert!((el-elref).abs()<1e-3);
997        assert!((range-rangeref).abs()<1e-3);
998    }
999
1000    #[test]
1001    fn test_eci2aer() {
1002        let azref = deg2rad(162.55);
1003        let elref = deg2rad(55.12);
1004        let rangeref = 384013940.9;
1005        let gst = 4.501012562811752;
1006
1007        let lat0 = deg2rad(28.4);
1008        let lon0 = deg2rad(-80.5);
1009        let alt0 = 2.7;
1010
1011        let (x,y,z) = aer2eci(gst, azref, elref, rangeref, lat0, lon0, alt0, Ellipsoid::default());
1012        let (az,el,range) = eci2aer(gst, x, y, z, lat0, lon0, alt0, Ellipsoid::default());
1013
1014        assert!((az-azref).abs()<1e-3);
1015        assert!((el-elref).abs()<1e-3);
1016        assert!((range-rangeref).abs()<1e-3);
1017    }
1018
1019    #[test]
1020    fn test_ned2geodetic() {
1021        let lat0 = deg2rad(42.0);
1022        let lon0 = deg2rad(-82.0);
1023        let alt0 = 200.0;
1024        let e = 0.0;
1025        let n = 0.0;
1026        let d = 1.0;
1027
1028        let latref = deg2rad(41.999999999999993);
1029        let lonref = deg2rad(-82.0);
1030        let altref = 1.990000000007368e+02;
1031
1032        let (lat,lon,alt) = ned2geodetic(n, e, d, lat0, lon0, alt0, Ellipsoid::default());
1033
1034        assert!((lat-latref).abs()<1e-8);
1035        assert!((lon-lonref).abs()<1e-8);
1036        assert!((alt-altref).abs()<1e-8);
1037    }
1038
1039    #[test]
1040    fn test_geodetic2ned() {
1041        let lat = deg2rad(41.999999999999993);
1042        let lon = deg2rad(-82.0);
1043        let alt = 1.990000000007368e+02;
1044        let lat0 = deg2rad(42.0);
1045        let lon0 = deg2rad(-82.0);
1046        let alt0 = 200.0;
1047
1048        let eref = 0.0;
1049        let nref = 0.0;
1050        let dref = 1.0;
1051
1052        let (n,e,d) = geodetic2ned(lat, lon, alt, lat0, lon0, alt0, Ellipsoid::default());
1053    
1054        assert!((e-eref).abs()<1e-3);
1055        assert!((n-nref).abs()<1e-3);
1056        assert!((d-dref).abs()<1e-3);
1057    }
1058
1059    #[test]
1060    fn test_aer2ned() {
1061        let az = deg2rad(33.0);
1062        let el = deg2rad(70.0);
1063        let slant_range = 1000.0;
1064
1065        let eref = 1.862775208165935e+02;
1066        let nref = 2.868422278517140e+02;
1067        let dref = -9.396926207859083e+02;
1068
1069        let (n, e, d)= aer2ned(az, el, slant_range);
1070
1071        assert!((e-eref).abs()<1e-3);
1072        assert!((n-nref).abs()<1e-3);
1073        assert!((d-dref).abs()<1e-3);
1074    }
1075
1076    #[test]
1077    fn test_ned2aer() {
1078        let az_ref = deg2rad(33.0);
1079        let el_ref = deg2rad(70.0);
1080        let range_ref = 1000.0;
1081
1082        let e = 1.862775208165935e+02;
1083        let n = 2.868422278517140e+02;
1084        let d = -9.396926207859083e+02;
1085
1086        let (az, el, range) = ned2aer(n, e, d);
1087
1088        assert!((az-az_ref).abs()<1e-6);
1089        assert!((el-el_ref).abs()<1e-6);
1090        assert!((range-range_ref).abs()<1e-3);
1091    }
1092
1093    #[test]
1094    fn test_ned2ecef() {
1095        let lat0 = deg2rad(42.0);
1096        let lon0 = deg2rad(-82.0);
1097        let alt0 = 200.0;
1098        let e = 1.862775210000000e+02;
1099        let n = 2.868422200000000e+02;
1100        let d = -9.396926200000000e+02;
1101
1102        let xref = 6.609301927610815e+05;
1103        let yref = -4.701424222957011e+06;
1104        let zref = 4.246579604632881e+06;
1105        
1106        let (x,y,z) = ned2ecef(n, e, d, lat0, lon0, alt0, Ellipsoid::default());
1107
1108        assert!((x-xref).abs()<1e-3);
1109        assert!((y-yref).abs()<1e-3);
1110        assert!((z-zref).abs()<1e-3);
1111    }
1112
1113    #[test]
1114    fn test_ellipsoid_references() {
1115        let (a, b, f, e) = Ellipsoid::WGS84.parameters();
1116        assert!((a-6378137.0).abs() < 1E-6);
1117        assert!((b-6356752.314245).abs() < 1E-6);
1118        assert!((1.0/f-298.257223563).abs() < 1E-6);
1119        assert!((e-6.6943799E-3_f64).abs() < 1E-6);
1120        let (a, b, f, e) = Ellipsoid::WGS72.parameters();
1121        assert!((b-a*(1.0-f)).abs() < 1E-6);
1122        assert!((e-(f*(2.0-f))) < 1E-6);
1123        let (a, b, f, e) = Ellipsoid::WGS66.parameters();
1124        assert!((b-a*(1.0-f)).abs() < 1E-6);
1125        assert!((e-(f*(2.0-f))) < 1E-6);
1126        let (a, b, f, e) = Ellipsoid::WGS60.parameters();
1127        assert!((b-a*(1.0-f)).abs() < 1E-6);
1128        assert!((e-(f*(2.0-f))) < 1E-6);
1129        let (a, b, f, e) = Ellipsoid::PZ90.parameters();
1130        assert!((a-6378136.0).abs() < 1E-6);
1131        assert!((b-a*(1.0-f)).abs() < 1E-6);
1132        assert!((1.0/f-298.257839303).abs() < 1E-6);
1133        assert!((e-(f*(2.0-f))) < 1E-6);
1134        let (a, b, f, e) = Ellipsoid::GRS80.parameters();
1135        assert!((a-6378137.0).abs() < 1E-6);
1136        assert!((b-a*(1.0-f)).abs() < 1E-6);
1137        assert!((1.0/f-298.257222101).abs() < 1E-6);
1138        assert!((e-(f*(2.0-f))) < 1E-6);
1139        let (a, b, f, e) = Ellipsoid::BDC.parameters();
1140        assert!((a-6378137.0).abs() < 1E-6);
1141        assert!((b-a*(1.0-f)).abs() < 1E-6);
1142        assert!((1.0/f-298.257222101).abs() < 1E-6);
1143        assert!((e-(f*(2.0-f))) < 1E-6);
1144        let (a, b, f, e) = Ellipsoid::Bessel.parameters();
1145        assert!((b-a*(1.0-f)).abs() < 1E-6);
1146        assert!((e-(f*(2.0-f))) < 1E-6);
1147        let (a, b, f, e) = Ellipsoid::International.parameters();
1148        assert!((b-a*(1.0-f)).abs() < 1E-6);
1149        assert!((e-(f*(2.0-f))) < 1E-6);
1150        let (a, b, f, e) = Ellipsoid::Airy.parameters();
1151        assert!((b-a*(1.0-f)).abs() < 1E-6);
1152        assert!((e-(f*(2.0-f))) < 1E-6);
1153    }
1154
1155    #[test]
1156    fn test_ecef2ned() {
1157        let lat0 = deg2rad(42.0);
1158        let lon0 = deg2rad(-82.0);
1159        let alt0 = 200.0;
1160        let eref = 1.862775210000000e+02;
1161        let nref = 2.868422200000000e+02;
1162        let dref = -9.396926200000000e+02;
1163        
1164        let (x,y,z) = ned2ecef(nref, eref, dref, lat0, lon0, alt0, Ellipsoid::default());
1165        let (n,e,d)= ecef2ned(x, y, z, lat0, lon0, alt0, Ellipsoid::default());
1166
1167        assert!((e-eref).abs()<1e-3);
1168        assert!((n-nref).abs()<1e-3);
1169        assert!((d-dref).abs()<1e-3);
1170    }
1171
1172    #[test]
1173    fn test_distance_calculation() {
1174        let new_york = (40.730610,-73.935242);
1175        let paris = (48.856614, 2.3522219);
1176        let buenos_aires = (-34.603722, -58.381592);
1177        let sydney = (-33.867487, 151.20699);
1178        // TEST 1
1179        let expected_km = 5831.0_f64; 
1180        let d_km = distance(new_york, paris) / 1000.0_f64;
1181        assert!((expected_km - d_km).abs() < 1.0);
1182        // TEST2
1183        let expected_km = 8527.0_f64; 
1184        let d_km = distance(new_york, buenos_aires) / 1000.0_f64;
1185        assert!((expected_km - d_km).abs() < 1.0);
1186        // TEST3
1187        let expected_km = 15990.0_f64; 
1188        let d_km = distance(new_york, sydney) / 1000.0_f64;
1189        assert!((expected_km - d_km).abs() < 10.0);
1190        // TEST4
1191        let expected_km = 11050.0_f64; 
1192        let d_km = distance(buenos_aires, paris) / 1000.0_f64;
1193        assert!((expected_km - d_km).abs() < 10.0);
1194    }
1195}
map_3d/lib.rs

map_3d/
lib.rs
