1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
use ::ecef::ECEF;
use ::wgs84::{ECCENTRICITY_SQ, SEMI_MAJOR_AXIS, WGS84};
use na::Vector3;
use num_traits::Float;
use std::convert::From;
#[derive(PartialEq, Clone, Copy, Debug)]
pub struct NVector<N> {
vec: Vector3<N>,
alt: N,
}
impl<N> NVector<N> {
pub fn new(vec: Vector3<N>, altitude: N) -> NVector<N> {
NVector {
vec: vec,
alt: altitude,
}
}
}
impl<N: Copy> NVector<N> {
pub fn vector(&self) -> Vector3<N> {
self.vec
}
pub fn altitude(&self) -> N {
self.alt
}
}
impl<N: Float> From<WGS84<N>> for NVector<N> {
fn from(f: WGS84<N>) -> NVector<N> {
let vec = Vector3::new(f.longitude().cos() * f.latitude().cos(),
f.longitude().sin() * f.latitude().cos(),
f.latitude().sin());
NVector::new(vec, f.altitude())
}
}
impl<N: Float> From<ECEF<N>> for NVector<N> {
fn from(f: ECEF<N>) -> NVector<N> {
let a_sq = N::from(SEMI_MAJOR_AXIS).unwrap().powi(2);
let e_2 = N::from(ECCENTRICITY_SQ).unwrap();
let e_4 = e_2.powi(2);
let x = f.z();
let y = f.y();
let z = -f.x();
let p = (y.powi(2) + z.powi(2)) / a_sq;
let q = ((N::one() - e_2) / a_sq) * x.powi(2);
let r = (p + q - e_4) / N::from(6.0).unwrap();
let s = (e_4 * p * q) / (N::from(4.0).unwrap() * r.powi(3));
let t = (N::one() + s + (s * (N::from(2.0).unwrap() + s)).sqrt()).cbrt();
let u = r * (N::one() + t + t.recip());
let v = (u.powi(2) + e_4 * q).sqrt();
let w = e_2 * ((u + v - q) / (N::from(2.0).unwrap() * v));
let k = (u + v + w.powi(2)).sqrt() - w;
let d = (k * (y.powi(2) + z.powi(2)).sqrt()) / (k + e_2);
let altitude = ((k + e_2 - N::one()) / k) * (d.powi(2) + x.powi(2)).sqrt();
let denom = (d.powi(2) + x.powi(2)).sqrt().recip();
let mul = k / (k + e_2);
let vec = Vector3::new(-mul * z * denom, mul * y * denom, x * denom);
NVector::new(vec, altitude)
}
}
#[cfg(test)]
mod tests {
use ::ecef::ECEF;
use ::wgs84::WGS84;
use assert::close;
use super::*;
quickcheck! {
fn from_wgs84(wgs: WGS84<f64>) -> () {
let test = WGS84::from(NVector::from(wgs));
close(wgs.latitude(), test.latitude(), 0.000001);
close(wgs.longitude(), test.longitude(), 0.000001);
close(wgs.altitude(), test.altitude(), 0.000001);
}
fn from_ecef(wgs: WGS84<f64>) -> () {
let ans = NVector::from(wgs);
let test = NVector::from(ECEF::from(wgs));
close(ans.altitude(), test.altitude(), 0.000001);
close(ans.vector().as_ref(), test.vector().as_ref(), 0.000001)
}
}
}