From 8a56688f1eddc3cb4b0704109d58f092a91937d0 Mon Sep 17 00:00:00 2001
From: Taku Fukada <naninunenor@gmail.com>
Date: Tue, 5 Dec 2023 08:46:41 +0900
Subject: [PATCH] geographic-to-geocentric conversion

---
 Cargo.toml                          |  1 +
 nusamai-projection/Cargo.toml       |  4 ++
 nusamai-projection/src/cartesian.rs | 57 +++++++++++++++++++++++++++++
 nusamai-projection/src/ellipsoid.rs | 28 ++++++++++++++
 nusamai-projection/src/lib.rs       |  4 ++
 5 files changed, 94 insertions(+)
 create mode 100644 nusamai-projection/Cargo.toml
 create mode 100644 nusamai-projection/src/cartesian.rs
 create mode 100644 nusamai-projection/src/ellipsoid.rs
 create mode 100644 nusamai-projection/src/lib.rs

diff --git a/Cargo.toml b/Cargo.toml
index c052c7a8b..3f03ff491 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -6,5 +6,6 @@ members = [
     "nusamai-geojson",
     "nusamai-plateau",
     "nusamai-mvt",
+    "nusamai-projection"
 ]
 resolver = "2"
diff --git a/nusamai-projection/Cargo.toml b/nusamai-projection/Cargo.toml
new file mode 100644
index 000000000..fa53f3c7c
--- /dev/null
+++ b/nusamai-projection/Cargo.toml
@@ -0,0 +1,4 @@
+[package]
+name = "nusamai-projection"
+version = "0.1.0"
+edition = "2021"
diff --git a/nusamai-projection/src/cartesian.rs b/nusamai-projection/src/cartesian.rs
new file mode 100644
index 000000000..5d22610fb
--- /dev/null
+++ b/nusamai-projection/src/cartesian.rs
@@ -0,0 +1,57 @@
+//! Conversion between geographic and geocentric (cartesian) coordinate systems.
+
+use crate::ellipsoid::Ellipsoid;
+
+/// Convert from geographic to geocentric coordinate system.
+pub fn geographic_to_geocentric<E: Ellipsoid>(lng: f64, lat: f64, height: f64) -> (f64, f64, f64) {
+    let (lng_rad, lat_rad) = (lng.to_radians(), lat.to_radians());
+    let tan_psi = (1. - E::E_SQ) * lat_rad.tan();
+    let z = E::A * (1. / (1. / (tan_psi * tan_psi) + 1. / ((1. - E::F) * (1. - E::F)))).sqrt();
+    let r = E::A * (1. / (1. + (tan_psi * tan_psi) / ((1. - E::F) * (1. - E::F)))).sqrt();
+    let x = r * lng_rad.cos();
+    let y = r * lng_rad.sin();
+    let dhz = lat_rad.sin();
+    let dhx = lat_rad.cos() * lng_rad.cos();
+    let dhy = lat_rad.cos() * lng_rad.sin();
+    (x + dhx * height, y + dhy * height, z + dhz * height)
+}
+
+/// Convert from geocentric to geographic coordinate system.
+pub fn geocentric_to_geographic<E: Ellipsoid>(x: f64, y: f64, z: f64) -> (f64, f64, f64) {
+    println!("not implemented: {:?}", (x, y, z));
+    todo!();
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::ellipsoid::WGS84;
+
+    #[test]
+    fn fixtures() {
+        {
+            let (x, y, z) = geographic_to_geocentric::<WGS84>(140., 37., 50.);
+            assert!((x - -3906851.9770472576).abs() < 1e-9);
+            assert!((y - 3278238.0530045824).abs() < 1e-9);
+            assert!((z - 3817423.251099322).abs() < 1e-9);
+        }
+
+        // north pole
+        {
+            let height = 150.;
+            let (x, y, z) = geographic_to_geocentric::<WGS84>(123., 90., 150.);
+            assert!((x - 0.).abs() < 1e-9);
+            assert!((y - 0.).abs() < 1e-9);
+            assert!((z - (WGS84::B + height)).abs() < 1e-9);
+        }
+
+        // null island
+        {
+            let height = 100.;
+            let (x, y, z) = geographic_to_geocentric::<WGS84>(0., 0., height);
+            assert!((x - (WGS84::A + height)).abs() < 1e-9);
+            assert!((y - 0.).abs() < 1e-9);
+            assert!((z - 0.).abs() < 1e-9);
+        }
+    }
+}
diff --git a/nusamai-projection/src/ellipsoid.rs b/nusamai-projection/src/ellipsoid.rs
new file mode 100644
index 000000000..86b1420e4
--- /dev/null
+++ b/nusamai-projection/src/ellipsoid.rs
@@ -0,0 +1,28 @@
+/// Ellipsoid parameters
+pub trait Ellipsoid {
+    /// Semi-major axis
+    const A: f64;
+    /// Inverse flattening
+    const INV_F: f64;
+
+    /// Flattening
+    const F: f64 = 1. / Self::INV_F;
+    /// Semi-minor axis
+    const B: f64 = Self::A * (1. - Self::F);
+    /// Eccentricity squared
+    const E_SQ: f64 = Self::F * (2. - Self::F);
+}
+
+/// WGS84 Eliipsoid
+pub struct WGS84 {}
+impl Ellipsoid for WGS84 {
+    const A: f64 = 6378137.;
+    const INV_F: f64 = 298.257223563;
+}
+
+/// GRS80 Eliipsoid
+pub struct GRS80 {}
+impl Ellipsoid for GRS80 {
+    const A: f64 = 6378137.;
+    const INV_F: f64 = 298.257222101;
+}
diff --git a/nusamai-projection/src/lib.rs b/nusamai-projection/src/lib.rs
new file mode 100644
index 000000000..a88c56dbc
--- /dev/null
+++ b/nusamai-projection/src/lib.rs
@@ -0,0 +1,4 @@
+pub mod cartesian;
+pub mod ellipsoid;
+
+pub use ellipsoid::*;