#![no_std]
#[cfg(feature = "nav-types-conversion")]
use nav_types::WGS84;
#[cfg(test)]
#[macro_use(quickcheck)]
extern crate quickcheck_macros;
#[derive(Clone, Debug, PartialEq)]
pub struct Wgs84 {
pub longitude: f64,
pub latitude: f64,
pub altitude: f64,
}
#[cfg(feature = "nav-types-conversion")]
impl Into<WGS84<f64>> for Wgs84 {
fn into(self) -> WGS84<f64> {
WGS84::from_degrees_and_meters(self.latitude, self.longitude, self.altitude)
}
}
#[cfg(feature = "nav-types-conversion")]
impl From<WGS84<f64>> for Wgs84 {
fn from(p: WGS84<f64>) -> Self {
Wgs84 {
latitude: p.latitude_degrees(),
longitude: p.longitude_degrees(),
altitude: p.altitude(),
}
}
}
impl Wgs84 {
pub fn to_lv03(&self) -> Option<Lv03> {
let phi = (3600.0 * self.latitude - 169_028.66) / 10_000.0;
let phi_2 = phi * phi;
let phi_3 = phi * phi_2;
let lambda = (3600.0 * self.longitude - 26_782.5) / 10_000.0;
let lambda_2 = lambda * lambda;
let lambda_3 = lambda * lambda_2;
let e = 2_600_072.37 + 211_455.93 * lambda
- 10938.51 * lambda * phi
- 0.36 * lambda * phi_2
- 44.54 * lambda_3;
let n = 1_200_147.07 + 308_807.95 * phi + 3745.25 * lambda_2 + 76.63 * phi_2
- 194.56 * lambda_2 * phi
+ 119.79 * phi_3;
let y = e - 2_000_000.00;
let x = n - 1_000_000.00;
let altitude = self.altitude - 49.55 + 2.73 * lambda + 6.94 * phi;
Lv03::new(x, y, altitude)
}
}
#[derive(Clone, Debug, PartialEq)]
pub struct Lv95 {
pub north: f64,
pub east: f64,
altitude: f64,
}
#[derive(Clone, Debug, PartialEq)]
pub struct Lv03 {
pub north: f64,
pub east: f64,
pub altitude: f64,
}
impl Lv03 {
pub fn new(north: f64, east: f64, altitude: f64) -> Option<Self> {
let valid_north_range = 70_000.0..300_000.0;
let valid_east_range = 480_000.0..850_000.0;
#[allow(clippy::if_same_then_else)]
if !valid_north_range.contains(&north) || !valid_east_range.contains(&east) {
None
} else if north > east {
None
} else {
Some(Lv03 {
north,
east,
altitude,
})
}
}
pub fn to_wgs84(&self) -> Wgs84 {
let y = (self.east - 600_000.0) / 1_000_000.0;
let y_2 = y * y;
let y_3 = y * y_2;
let x = (self.north - 200_000.0) / 1_000_000.0;
let x_2 = x * x;
let x_3 = x * x_2;
let lambda = 2.6779094 + 4.728982 * y + 0.791484 * y * x + 0.1306 * y * x_2 - 0.0436 * y_3;
let phi = 16.9023892 + 3.238272 * x
- 0.270978 * y_2
- 0.002528 * x_2
- 0.0447 * y_2 * x
- 0.0140 * x_3;
let altitude = self.altitude + 49.55 - 12.6 * y - 22.64 * x;
let lambda = lambda * 100.0 / 36.0;
let phi = phi * 100.0 / 36.0;
Wgs84 {
longitude: lambda,
latitude: phi,
altitude,
}
}
pub fn distance_squared(&self, p: &Lv03) -> f64 {
let d_north = self.north - p.north;
let d_east = self.east - p.east;
let d_altitude = self.altitude - p.altitude;
d_north * d_north + d_east * d_east + d_altitude * d_altitude
}
}
impl Lv95 {
pub fn new(north: f64, east: f64, altitude: f64) -> Option<Self> {
let p = Lv03::new(north, east, altitude);
p.map(|p| p.into())
}
pub fn to_wgs84(&self) -> Wgs84 {
let p03: Lv03 = self.clone().into();
p03.to_wgs84()
}
}
impl From<Lv95> for Lv03 {
fn from(p: Lv95) -> Self {
Lv03 {
north: p.north - 1_000_000.0,
east: p.east - 2_000_000.0,
altitude: p.altitude,
}
}
}
impl From<Lv03> for Lv95 {
fn from(p: Lv03) -> Self {
Lv95 {
north: p.north + 1_000_000.0,
east: p.east + 2_000_000.0,
altitude: p.altitude,
}
}
}
impl From<Lv03> for Wgs84 {
fn from(p: Lv03) -> Self {
p.to_wgs84()
}
}
impl From<Lv95> for Wgs84 {
fn from(p: Lv95) -> Self {
p.to_wgs84()
}
}
#[cfg(test)]
mod tests {
use super::*;
use oorandom;
fn test_conversions(lv: &Lv03, wgs: &Wgs84) {
let wgs_converted = lv.to_wgs84();
assert!((wgs_converted.longitude - wgs.longitude).abs() < 0.001);
assert!((wgs_converted.latitude - wgs.latitude).abs() < 0.001);
assert!((wgs_converted.altitude - wgs.altitude).abs() < 5.0);
let lv_converted = wgs.to_lv03().unwrap();
assert!((lv_converted.east - lv.east).abs() < 2.0);
assert!((lv_converted.north - lv.north).abs() < 2.0);
assert!((lv_converted.altitude - lv.altitude).abs() < 5.0);
test_roundtrip_wgs(lv);
test_roundtrip_lv(lv);
}
fn test_roundtrip_lv(lv03: &Lv03) {
let lv95: Lv95 = lv03.clone().into();
let lv03_converted: Lv03 = lv95.into();
assert!(lv03.distance_squared(&lv03_converted) < 0.001);
}
fn test_roundtrip_wgs(lv: &Lv03) {
let wgs = lv.to_wgs84();
let lv03 = wgs.to_lv03().unwrap();
assert!(lv03.distance_squared(lv) < 1.0);
}
fn get_random_lv03(rng: &mut oorandom::Rand32) -> Lv03 {
let north = 70_000.0 + (300_000.0 - 70_000.0) * rng.rand_float() as f64;
let east = 480_000.0 + (850_000.0 - 480_000.0) * rng.rand_float() as f64;
let altitude = 400.0 + (5_000.0 - 400.0) * rng.rand_float() as f64;
Lv03 {
north,
east,
altitude,
}
}
#[test]
fn random_locations() {
let some_seed = 13;
let mut generator = oorandom::Rand32::new(some_seed);
for _ in 0..1000 {
let lv03 = get_random_lv03(&mut generator);
let wgs84: Wgs84 = lv03.clone().into();
let new_lv03: Lv03 = wgs84.to_lv03().unwrap();
assert!((lv03.east - new_lv03.east).abs() < 5.0);
assert!((lv03.north - new_lv03.north).abs() < 5.0);
assert!((lv03.altitude - new_lv03.altitude).abs() < 1.0);
assert!(lv03.distance_squared(&new_lv03) < 30.0);
}
}
#[test]
fn test_bundeshaus() {
let lv = Lv03::new(199_498.43, 600_421.43, 542.8).unwrap();
let wgs = Wgs84 {
longitude: 7.44417,
latitude: 46.94658,
altitude: 591.8,
};
test_conversions(&lv, &wgs);
}
#[test]
fn test_matterhorn() {
let lv = Lv03::new(91_673.72, 617_049.89, 4477.4).unwrap();
let wgs = Wgs84 {
longitude: 7.65861,
latitude: 45.97642,
altitude: 4532.9,
};
test_conversions(&lv, &wgs);
}
#[test]
fn test_700_100() {
let lv = Lv03::new(100_000.0, 700_000.0, 1000.0).unwrap();
let wgs = Wgs84 {
longitude: 8.730497076,
latitude: 46.044130339,
altitude: 1050.0,
};
test_conversions(&lv, &wgs);
}
#[test]
fn test_negative() {
assert!(Lv03::new(-1.0, 2.0, 5.0).is_none());
assert!(Lv03::new(1.0, -2.0, 5.0).is_none());
assert!(Lv03::new(250_000.0, 500_000.0, -5.0).is_some());
}
#[test]
fn test_coordinates_swapped() {
assert!(Lv03::new(600_000.0, 200_000.0, 500.0).is_none());
}
#[test]
fn test_distance() {
let p1 = Lv03::new(200_000.0, 600_000.0, 500.0).unwrap();
let p2 = Lv03::new(200_002.0, 600_000.0, 500.0).unwrap();
assert_eq!(4.0, p1.distance_squared(&p2));
}
#[test]
fn test_lv_conversion() {
let p1 = Lv03::new(200_000.0, 600_000.0, 500.0).unwrap();
let p2: Lv95 = p1.clone().into();
assert_eq!(p1, p2.clone().into());
assert_eq!(p1.east + 2_000_000.0, p2.east);
assert_eq!(p1.north + 1_000_000.0, p2.north);
}
#[test]
#[cfg(feature = "nav-types-conversion")]
fn test_nav_types_conversion() {
let wgs = Wgs84 {
longitude: 8.730497076,
latitude: 46.044130339,
altitude: 542.8,
};
let nav_type: WGS84<f64> = wgs.clone().into();
let back: Wgs84 = Wgs84::from(nav_type);
assert_eq!(wgs, back);
assert_eq!(wgs.longitude, nav_type.longitude_degrees());
assert_eq!(wgs.latitude, nav_type.latitude_degrees());
assert_eq!(wgs.altitude, nav_type.altitude());
}
#[quickcheck]
fn roundtrip_test(north: f64, east: f64, altitude: f64) -> () {
let lv03 = Lv03::new(north, east, altitude);
if let Some(lv03) = lv03 {
let wgs84 = lv03.to_wgs84();
let lv03 = wgs84.to_lv03();
lv03.unwrap();
}
}
}