use crate::units::{Bearing, Distance};
pub const UNITS_PER_DEGREE: i64 = 342_833_400_000_000;
pub const UNITS_PER_MINUTE: i64 = UNITS_PER_DEGREE / 60;
pub const UNITS_PER_HUNDREDTH_MINUTE: i64 = UNITS_PER_MINUTE / 100;
pub(crate) const LAT_MAX: i64 = 90 * UNITS_PER_DEGREE;
pub(crate) const LON_MAX: i64 = 180 * UNITS_PER_DEGREE;
const UM_NUM: i128 = 926;
const UM_DEN: i128 = 2_856_945;
const COS_Q15_ONE: i64 = 32_767;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
#[non_exhaustive]
pub enum GeoError {
BadLatitude {
got: i64,
},
BadLongitude {
got: i64,
},
BadAmbiguity {
got: u8,
},
BadGridLength {
got: usize,
},
BadGridChar {
got: u8,
position: usize,
},
}
impl core::fmt::Display for GeoError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::BadLatitude { got } => {
write!(f, "latitude {got} (storage units) exceeds 90 degrees")
}
Self::BadLongitude { got } => {
write!(f, "longitude {got} (storage units) exceeds 180 degrees")
}
Self::BadAmbiguity { got } => {
write!(f, "position ambiguity {got} is outside 0..=4 digits")
}
Self::BadGridLength { got } => {
write!(f, "Maidenhead locator length {got} is not 4, 6 or 8")
}
Self::BadGridChar { got, position } => write!(
f,
"byte {got:#04x} at offset {position} is not valid in a Maidenhead locator"
),
}
}
}
impl core::error::Error for GeoError {}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum LatitudeHemisphere {
North,
South,
}
impl LatitudeHemisphere {
#[must_use]
pub const fn letter(self) -> u8 {
match self {
Self::North => b'N',
Self::South => b'S',
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum LongitudeHemisphere {
East,
West,
}
impl LongitudeHemisphere {
#[must_use]
pub const fn letter(self) -> u8 {
match self {
Self::East => b'E',
Self::West => b'W',
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct DegreesMinutes {
pub degrees: u16,
pub hundredths_of_minute: u16,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Coordinates {
pub latitude: Latitude,
pub longitude: Longitude,
pub ambiguity: Ambiguity,
}
impl Coordinates {
#[must_use]
pub const fn new(latitude: Latitude, longitude: Longitude) -> Self {
Self {
latitude,
longitude,
ambiguity: Ambiguity::EXACT,
}
}
#[must_use]
pub const fn with_ambiguity(self, ambiguity: Ambiguity) -> Self {
Self { ambiguity, ..self }
}
#[must_use]
pub const fn maidenhead(self) -> MaidenheadGrid {
self.maidenhead_with_precision(GridPrecision::Subsquare)
}
#[must_use]
pub const fn maidenhead_with_precision(self, precision: GridPrecision) -> MaidenheadGrid {
let len = precision.characters() as u8;
let mut lat = self.latitude.0 + LAT_MAX;
let mut lon = self.longitude.0 + LON_MAX;
if lon >= 2 * LON_MAX {
lon -= 2 * LON_MAX;
}
if lat >= 2 * LAT_MAX {
lat = 2 * LAT_MAX - 1;
}
let lon_field = 20 * UNITS_PER_DEGREE;
let lat_field = 10 * UNITS_PER_DEGREE;
let lon_square = 2 * UNITS_PER_DEGREE;
let lat_square = UNITS_PER_DEGREE;
let lon_sub = lon_square / 24;
let lat_sub = lat_square / 24;
let mut chars = [0u8; 8];
chars[0] = b'A' + (lon / lon_field) as u8;
chars[1] = b'A' + (lat / lat_field) as u8;
chars[2] = b'0' + (lon % lon_field / lon_square) as u8;
chars[3] = b'0' + (lat % lat_field / lat_square) as u8;
if len >= 6 {
chars[4] = b'a' + (lon % lon_square / lon_sub) as u8;
chars[5] = b'a' + (lat % lat_square / lat_sub) as u8;
}
if len >= 8 {
chars[6] = b'0' + (lon % lon_sub * 10 / lon_sub) as u8;
chars[7] = b'0' + (lat % lat_sub * 10 / lat_sub) as u8;
}
MaidenheadGrid { chars, len }
}
#[must_use]
pub const fn from_maidenhead(grid: MaidenheadGrid) -> Self {
let chars = grid.chars;
let lon_square = 2 * UNITS_PER_DEGREE;
let lat_square = UNITS_PER_DEGREE;
let lon_sub = lon_square / 24;
let lat_sub = lat_square / 24;
let mut lon = (chars[0] - b'A') as i64 * 20 * UNITS_PER_DEGREE
+ (chars[2] - b'0') as i64 * lon_square;
let mut lat = (chars[1] - b'A') as i64 * 10 * UNITS_PER_DEGREE
+ (chars[3] - b'0') as i64 * lat_square;
let (lon_cell, lat_cell) = if grid.len >= 6 {
lon += (chars[4] - b'a') as i64 * lon_sub;
lat += (chars[5] - b'a') as i64 * lat_sub;
if grid.len == 8 {
lon += (chars[6] - b'0') as i64 * (lon_sub / 10);
lat += (chars[7] - b'0') as i64 * (lat_sub / 10);
(lon_sub / 10, lat_sub / 10)
} else {
(lon_sub, lat_sub)
}
} else {
(lon_square, lat_square)
};
lon += lon_cell / 2;
lat += lat_cell / 2;
Self {
latitude: Latitude(lat - LAT_MAX),
longitude: Longitude(lon - LON_MAX),
ambiguity: Ambiguity::EXACT,
}
}
#[must_use]
pub fn distance_to(self, other: Self) -> Distance {
let (east, north) = self.displacement(other);
let (east, north) = (i128::from(east), i128::from(north));
#[allow(clippy::cast_sign_loss)] let sum_of_squares = (east * east + north * north) as u128;
#[allow(clippy::cast_possible_truncation)] let magnitude = sum_of_squares.isqrt() as i128;
#[allow(clippy::cast_possible_truncation)] let micrometers = ((magnitude * UM_NUM + UM_DEN / 2) / UM_DEN) as i64;
Distance::from_micrometers(micrometers)
}
#[must_use]
pub fn bearing_to(self, other: Self) -> Bearing {
let (east, north) = self.displacement(other);
if east == 0 && north == 0 {
return Bearing::NORTH;
}
let (east, north) = (i128::from(east), i128::from(north));
let mut best_degrees = 0u16;
let mut best_dot = i128::MIN;
for degrees in 0..360u16 {
let phase = (u64::from(degrees) << 32) / 360;
#[allow(clippy::cast_possible_truncation)] let phase = phase as u32;
let sin = i128::from(crate::types::sine_at_interpolated(phase));
let cos = i128::from(crate::types::sine_at_interpolated(
phase.wrapping_add(1 << 30),
));
let dot = east * sin + north * cos;
if dot > best_dot {
best_dot = dot;
best_degrees = degrees;
}
}
Bearing::new(best_degrees).unwrap_or(Bearing::NORTH)
}
fn displacement(self, other: Self) -> (i64, i64) {
let north = other.latitude.0 - self.latitude.0;
let mut east = other.longitude.0 - self.longitude.0;
let full_turn = 2 * LON_MAX;
if east > LON_MAX {
east -= full_turn;
} else if east < -LON_MAX {
east += full_turn;
}
let mean_latitude = (self.latitude.0 + other.latitude.0) / 2;
#[allow(clippy::cast_possible_truncation)]
let east = ((i128::from(east) * i128::from(cos_q15(mean_latitude)))
/ i128::from(COS_Q15_ONE)) as i64;
(east, north)
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
pub struct Latitude(pub(crate) i64);
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
pub struct Longitude(pub(crate) i64);
impl Latitude {
pub const fn new(units: i64) -> Result<Self, GeoError> {
if units < -LAT_MAX || units > LAT_MAX {
Err(GeoError::BadLatitude { got: units })
} else {
Ok(Self(units))
}
}
#[must_use]
pub const fn units(self) -> i64 {
self.0
}
#[must_use]
#[allow(clippy::cast_precision_loss)] pub fn to_degrees(self) -> f64 {
self.0 as f64 / UNITS_PER_DEGREE as f64
}
pub fn from_degrees(degrees: f64) -> Result<Self, GeoError> {
Self::new(round_scaled(degrees).ok_or(GeoError::BadLatitude { got: i64::MAX })?)
}
pub const fn from_degrees_minutes(
degrees: u16,
hundredths_of_minute: u16,
hemisphere: LatitudeHemisphere,
) -> Result<Self, GeoError> {
let magnitude = degrees as i64 * UNITS_PER_DEGREE
+ hundredths_of_minute as i64 * UNITS_PER_HUNDREDTH_MINUTE;
match hemisphere {
LatitudeHemisphere::North => Self::new(magnitude),
LatitudeHemisphere::South => Self::new(-magnitude),
}
}
#[must_use]
pub const fn hemisphere(self) -> LatitudeHemisphere {
if self.0 < 0 {
LatitudeHemisphere::South
} else {
LatitudeHemisphere::North
}
}
#[must_use]
pub const fn degrees_minutes(self) -> DegreesMinutes {
degrees_minutes(self.0)
}
}
impl Longitude {
pub const fn new(units: i64) -> Result<Self, GeoError> {
if units < -LON_MAX || units > LON_MAX {
Err(GeoError::BadLongitude { got: units })
} else {
Ok(Self(units))
}
}
#[must_use]
pub const fn units(self) -> i64 {
self.0
}
#[must_use]
#[allow(clippy::cast_precision_loss)] pub fn to_degrees(self) -> f64 {
self.0 as f64 / UNITS_PER_DEGREE as f64
}
pub fn from_degrees(degrees: f64) -> Result<Self, GeoError> {
Self::new(round_scaled(degrees).ok_or(GeoError::BadLongitude { got: i64::MAX })?)
}
pub const fn from_degrees_minutes(
degrees: u16,
hundredths_of_minute: u16,
hemisphere: LongitudeHemisphere,
) -> Result<Self, GeoError> {
let magnitude = degrees as i64 * UNITS_PER_DEGREE
+ hundredths_of_minute as i64 * UNITS_PER_HUNDREDTH_MINUTE;
match hemisphere {
LongitudeHemisphere::East => Self::new(magnitude),
LongitudeHemisphere::West => Self::new(-magnitude),
}
}
#[must_use]
pub const fn hemisphere(self) -> LongitudeHemisphere {
if self.0 < 0 {
LongitudeHemisphere::West
} else {
LongitudeHemisphere::East
}
}
#[must_use]
pub const fn degrees_minutes(self) -> DegreesMinutes {
degrees_minutes(self.0)
}
}
const fn degrees_minutes(units: i64) -> DegreesMinutes {
let magnitude = units.unsigned_abs();
let half = UNITS_PER_HUNDREDTH_MINUTE.unsigned_abs() / 2;
let total = (magnitude + half) / UNITS_PER_HUNDREDTH_MINUTE.unsigned_abs();
#[allow(clippy::cast_possible_truncation)] DegreesMinutes {
degrees: (total / 6000) as u16,
hundredths_of_minute: (total % 6000) as u16,
}
}
fn cos_q15(latitude_units: i64) -> i64 {
let turn = 360 * UNITS_PER_DEGREE.unsigned_abs();
#[allow(clippy::cast_possible_truncation)] let phase =
((u128::from(latitude_units.unsigned_abs() % turn) << 32) / u128::from(turn)) as u64;
let phase = (phase as u32).wrapping_add(1 << 30);
i64::from(crate::types::sine_at_interpolated(phase))
}
#[allow(clippy::cast_precision_loss)] fn round_scaled(degrees: f64) -> Option<i64> {
let scaled = degrees * UNITS_PER_DEGREE as f64;
let limit = 2.0 * LON_MAX as f64;
if !(-limit..=limit).contains(&scaled) {
return None;
}
let rounded = if scaled >= 0.0 {
scaled + 0.5
} else {
scaled - 0.5
};
#[allow(clippy::cast_possible_truncation)]
Some(rounded as i64)
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Default)]
pub struct Ambiguity(u8);
impl Ambiguity {
pub const EXACT: Self = Self(0);
pub const fn new(digits: u8) -> Result<Self, GeoError> {
if digits > 4 {
Err(GeoError::BadAmbiguity { got: digits })
} else {
Ok(Self(digits))
}
}
#[must_use]
pub const fn digits(self) -> u8 {
self.0
}
#[must_use]
pub const fn is_exact(self) -> bool {
self.0 == 0
}
#[must_use]
pub const fn step(self) -> i64 {
match self.0 {
1 => UNITS_PER_HUNDREDTH_MINUTE * 10,
2 => UNITS_PER_MINUTE,
3 => UNITS_PER_MINUTE * 10,
4 => UNITS_PER_DEGREE,
_ => 1,
}
}
#[must_use]
pub const fn mask(self, units: i64) -> i64 {
let step = self.step();
(units / step) * step
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Default, Hash)]
pub enum GridPrecision {
Square,
#[default]
Subsquare,
ExtendedSquare,
}
impl GridPrecision {
#[must_use]
pub const fn characters(self) -> usize {
match self {
Self::Square => 4,
Self::Subsquare => 6,
Self::ExtendedSquare => 8,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct MaidenheadGrid {
chars: [u8; 8],
len: u8,
}
impl MaidenheadGrid {
pub const fn new(text: &str) -> Result<Self, GeoError> {
Self::from_bytes(text.as_bytes())
}
pub const fn from_bytes(bytes: &[u8]) -> Result<Self, GeoError> {
let len = bytes.len();
if len != 4 && len != 6 && len != 8 {
return Err(GeoError::BadGridLength { got: len });
}
let mut chars = [0u8; 8];
let mut i = 0;
while i < len {
let byte = bytes[i];
let canonical = match i {
0 | 1 => match byte {
b'A'..=b'R' => byte,
b'a'..=b'r' => byte - 32,
_ => {
return Err(GeoError::BadGridChar {
got: byte,
position: i,
});
}
},
2 | 3 | 6 | 7 => match byte {
b'0'..=b'9' => byte,
_ => {
return Err(GeoError::BadGridChar {
got: byte,
position: i,
});
}
},
_ => match byte {
b'a'..=b'x' => byte,
b'A'..=b'X' => byte + 32,
_ => {
return Err(GeoError::BadGridChar {
got: byte,
position: i,
});
}
},
};
chars[i] = canonical;
i += 1;
}
#[allow(clippy::cast_possible_truncation)] Ok(Self {
chars,
len: len as u8,
})
}
#[must_use]
pub fn as_str(&self) -> &str {
core::str::from_utf8(self.as_bytes()).unwrap_or("")
}
#[must_use]
pub fn as_bytes(&self) -> &[u8] {
self.chars.get(..self.len as usize).unwrap_or(&[])
}
#[must_use]
pub const fn precision(&self) -> GridPrecision {
match self.len {
4 => GridPrecision::Square,
8 => GridPrecision::ExtendedSquare,
_ => GridPrecision::Subsquare,
}
}
#[must_use]
pub const fn center(self) -> Coordinates {
Coordinates::from_maidenhead(self)
}
#[must_use]
pub const fn to_precision(self, precision: GridPrecision) -> Self {
let wanted = precision.characters() as u8;
if wanted >= self.len {
return self;
}
let mut chars = self.chars;
let mut i = wanted as usize;
while i < chars.len() {
chars[i] = 0;
i += 1;
}
Self { chars, len: wanted }
}
}
impl core::fmt::Display for MaidenheadGrid {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.write_str(self.as_str())
}
}
#[cfg(test)]
mod tests {
use super::*;
#[cfg(feature = "std")]
#[test]
fn cos_q15_is_not_biased() {
let mut total_lsb = 0.0f64;
let mut worst_lsb = 0.0f64;
let mut samples = 0u32;
for tenth in 1..900u32 {
let degrees = f64::from(tenth) / 10.0;
#[allow(clippy::cast_possible_truncation)]
let units = (degrees * UNITS_PER_DEGREE as f64) as i64;
let got = cos_q15(units) as f64;
let want = (degrees * core::f64::consts::PI / 180.0).cos() * 32_767.0;
let error_lsb = got - want;
total_lsb += error_lsb;
worst_lsb = worst_lsb.max(error_lsb.abs());
samples += 1;
}
let mean_lsb = total_lsb / f64::from(samples);
assert!(
mean_lsb.abs() < 0.2,
"cos_q15 is biased: mean error {mean_lsb:.3} LSB over {samples} samples \
(worst {worst_lsb:.3} LSB); a rounding interpolation should centre this"
);
assert!(
worst_lsb < 1.2,
"cos_q15 worst-case error {worst_lsb:.3} LSB exceeds the table's own \
half-LSB plus one rounding step"
);
}
#[cfg(feature = "std")]
#[test]
fn bearing_to_returns_the_nearest_whole_degree() {
let origin = Coordinates::new(Latitude::new(0).unwrap(), Longitude::new(0).unwrap());
let radius = (UNITS_PER_DEGREE / 10) as f64;
let mut mismatches = 0u32;
let mut checked = 0u32;
for tenth in 0..3600u32 {
let angle = f64::from(tenth) / 10.0;
let radians = angle * core::f64::consts::PI / 180.0;
#[allow(clippy::cast_possible_truncation)]
let north = (radius * radians.cos()) as i64;
#[allow(clippy::cast_possible_truncation)]
let east = (radius * radians.sin()) as i64;
let target =
Coordinates::new(Latitude::new(north).unwrap(), Longitude::new(east).unwrap());
let (e, n) = origin.displacement(target);
let want = (e as f64).atan2(n as f64).to_degrees().rem_euclid(360.0);
let fraction = want - want.floor();
if (fraction - 0.5).abs() < 0.02 {
continue;
}
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
let nearest = (want.round() as i64).rem_euclid(360) as u16;
checked += 1;
if origin.bearing_to(target).degrees() != nearest {
mismatches += 1;
}
}
assert_eq!(
mismatches, 0,
"{mismatches} of {checked} directions returned the wrong whole degree"
);
}
#[test]
fn coordinate_range_checks() {
assert!(Latitude::new(LAT_MAX).is_ok());
assert_eq!(
Latitude::new(LAT_MAX + 1),
Err(GeoError::BadLatitude { got: LAT_MAX + 1 })
);
assert_eq!(
Latitude::new(-LAT_MAX - 1),
Err(GeoError::BadLatitude { got: -LAT_MAX - 1 })
);
assert!(Longitude::new(-LON_MAX).is_ok());
assert_eq!(
Longitude::new(LON_MAX + 1),
Err(GeoError::BadLongitude { got: LON_MAX + 1 })
);
}
#[test]
fn degree_conversions() {
let l = match Latitude::from_degrees(49.0 + 3.5 / 60.0) {
Ok(l) => l,
Err(e) => panic!("{e}"),
};
let expected = (49 * 6000 + 350) * UNITS_PER_HUNDREDTH_MINUTE;
assert!(
(l.units() - expected).abs() <= 4,
"{} vs {expected}",
l.units()
);
assert!((l.to_degrees() - (49.0 + 3.5 / 60.0)).abs() < 1e-9);
assert!(Latitude::from_degrees(90.001).is_err());
assert!(Latitude::from_degrees(f64::NAN).is_err());
assert!(Longitude::from_degrees(-180.001).is_err());
let g = match Longitude::from_degrees(-72.75) {
Ok(g) => g,
Err(e) => panic!("{e}"),
};
let expected = -(72 * 6000 + 45 * 100) * UNITS_PER_HUNDREDTH_MINUTE;
assert!(
(g.units() - expected).abs() <= 4,
"{} vs {expected}",
g.units()
);
}
#[test]
fn ambiguity_range() {
assert_eq!(Ambiguity::new(0).map(Ambiguity::digits), Ok(0));
assert_eq!(Ambiguity::new(4).map(Ambiguity::digits), Ok(4));
assert_eq!(Ambiguity::new(5), Err(GeoError::BadAmbiguity { got: 5 }));
assert!(Ambiguity::EXACT.is_exact());
}
#[test]
fn ambiguity_masks_to_the_chapter_6_levels() {
let minute = UNITS_PER_DEGREE / 60;
let hundredth = minute / 100;
let value = 49 * UNITS_PER_DEGREE + 3 * minute + 57 * hundredth;
let cases = [
(0u8, 3 * minute + 57 * hundredth),
(1, 3 * minute + 50 * hundredth),
(2, 3 * minute),
(3, 0),
(4, 0),
];
for (digits, offset) in cases {
let a = Ambiguity::new(digits).expect("0..=4 is in range");
assert_eq!(
a.mask(value),
49 * UNITS_PER_DEGREE + offset,
"north, {digits} masked digits"
);
assert_eq!(
a.mask(-value),
-(49 * UNITS_PER_DEGREE + offset),
"south, {digits} masked digits"
);
assert!(
a.mask(-value).abs() <= value,
"masking must never increase a magnitude"
);
}
}
#[test]
fn ambiguity_masking_is_idempotent_and_ordered() {
let step = UNITS_PER_DEGREE / 6000;
for units in [
0i64,
step,
12_345 * step,
-12_345 * step,
180 * UNITS_PER_DEGREE,
] {
let mut previous = units;
for digits in 0u8..=4 {
let a = Ambiguity::new(digits).expect("0..=4 is in range");
let once = a.mask(units);
assert_eq!(a.mask(once), once, "idempotence at {digits} for {units}");
assert!(
once.abs() <= previous.abs(),
"level {digits} reported a larger magnitude than level {}",
digits.saturating_sub(1)
);
assert_eq!(
once % a.step(),
0,
"a masked value must be a whole number of steps"
);
previous = once;
}
}
}
}