#![cfg_attr(not(feature = "std"), no_std)]
use core::ops::{Add, Div, Mul, Neg, Rem, Shl, Shr, Sub};
use num_traits::{
Bounded, CheckedDiv, CheckedMul, CheckedRem, Euclid, Float, FloatConst, NumCast, PrimInt,
ToPrimitive, WrappingAdd, WrappingMul, WrappingNeg, WrappingSub, Zero,
};
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)]
pub struct Angle<T>(pub T);
impl<T: NumCast + ToPrimitive + Bounded + Zero> Angle<T> {
#[must_use]
pub fn from_radians<F: Float + FloatConst + Euclid>(radians: F) -> Self {
if !radians.is_finite() {
return Self(T::zero());
}
let scale = <F as NumCast>::from(T::max_value()).unwrap() + F::one();
let normalized = radians.rem_euclid(&F::TAU()) / F::TAU();
Self(NumCast::from(normalized * scale).unwrap_or_else(T::zero))
}
#[must_use]
pub fn to_radians<F: Float + FloatConst>(self) -> F {
let scale = <F as NumCast>::from(T::max_value()).unwrap() + F::one();
<F as NumCast>::from(self.0).unwrap() / scale * F::TAU()
}
#[must_use]
pub fn from_degrees<F: Float + Euclid>(degrees: F) -> Self {
if !degrees.is_finite() {
return Self(T::zero());
}
let scale = <F as NumCast>::from(T::max_value()).unwrap() + F::one();
let full = <F as NumCast>::from(360).unwrap();
let normalized = degrees.rem_euclid(&full) / full;
Self(NumCast::from(normalized * scale).unwrap_or_else(T::zero))
}
#[must_use]
pub fn to_degrees<F: Float>(self) -> F {
let scale = <F as NumCast>::from(T::max_value()).unwrap() + F::one();
let full = <F as NumCast>::from(360).unwrap();
<F as NumCast>::from(self.0).unwrap() / scale * full
}
#[must_use]
pub fn from_atan2<F: Float + FloatConst + Euclid>(y: F, x: F) -> Self {
Self::from_radians(y.atan2(x))
}
#[must_use]
pub fn sin<F: Float + FloatConst>(self) -> F {
self.to_radians::<F>().sin()
}
#[must_use]
pub fn cos<F: Float + FloatConst>(self) -> F {
self.to_radians::<F>().cos()
}
#[must_use]
pub fn tan<F: Float + FloatConst>(self) -> F {
self.to_radians::<F>().tan()
}
#[must_use]
pub fn sin_cos<F: Float + FloatConst>(self) -> (F, F) {
self.to_radians::<F>().sin_cos()
}
#[must_use]
pub fn scale<F: Float + Euclid>(self, factor: F) -> Self {
if !factor.is_finite() {
return Self(T::zero());
}
let full = <F as NumCast>::from(T::max_value()).unwrap() + F::one();
let wrapped = (<F as NumCast>::from(self.0).unwrap() * factor).rem_euclid(&full);
Self(NumCast::from(wrapped).unwrap_or_else(T::zero))
}
}
impl<T: ToPrimitive> Angle<T> {
#[must_use]
pub fn ratio<F: Float>(self, other: Self) -> F {
<F as NumCast>::from(self.0).unwrap() / <F as NumCast>::from(other.0).unwrap()
}
}
impl<T: PrimInt> Angle<T> {
#[must_use]
pub fn to_frac(self) -> (T, T) {
let one = T::one();
if self.0.is_zero() {
return (T::zero(), one);
}
let two = one + one;
let num = self.0;
let den = T::max_value() / two + one;
let shift = num.trailing_zeros().min(den.trailing_zeros()) as usize;
(num >> shift, den >> shift)
}
}
impl<T: PrimInt + WrappingMul + WrappingAdd> Angle<T> {
#[must_use]
pub fn from_frac(num: T, den: T) -> Self {
let two = T::one() + T::one();
let pi = T::max_value() / two + T::one();
let q = num / den;
let r = num % den;
Angle(q.wrapping_mul(&pi).wrapping_add(&(r * (pi / den))))
}
}
impl<T: ToPrimitive + Shr<usize, Output = T>> Angle<T> {
#[must_use]
pub fn cast<U: NumCast + Shl<usize, Output = U>>(self) -> Angle<U> {
let src_bits = core::mem::size_of::<T>() * 8;
let dst_bits = core::mem::size_of::<U>() * 8;
if src_bits >= dst_bits {
let shifted = self.0 >> (src_bits - dst_bits);
Angle(<U as NumCast>::from(shifted).unwrap())
} else {
let widened: U = <U as NumCast>::from(self.0).unwrap();
Angle(widened << (dst_bits - src_bits))
}
}
}
impl<T: WrappingAdd> Add for Angle<T> {
type Output = Self;
fn add(self, rhs: Self) -> Self {
Angle(self.0.wrapping_add(&rhs.0))
}
}
impl<T: Zero + WrappingAdd> Zero for Angle<T> {
fn zero() -> Self {
Angle(T::zero())
}
fn is_zero(&self) -> bool {
self.0.is_zero()
}
}
impl<T: WrappingSub> Sub for Angle<T> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
Angle(self.0.wrapping_sub(&rhs.0))
}
}
impl<T: WrappingNeg> Neg for Angle<T> {
type Output = Self;
fn neg(self) -> Self {
Angle(self.0.wrapping_neg())
}
}
impl<T: WrappingMul> Mul<T> for Angle<T> {
type Output = Self;
fn mul(self, rhs: T) -> Self {
Angle(self.0.wrapping_mul(&rhs))
}
}
impl<T: Div> Div<T> for Angle<T> {
type Output = Angle<T::Output>;
fn div(self, rhs: T) -> Self::Output {
Angle(self.0 / rhs)
}
}
impl<T: Rem> Rem<T> for Angle<T> {
type Output = Angle<T::Output>;
fn rem(self, rhs: T) -> Self::Output {
Angle(self.0 % rhs)
}
}
impl<T: Shl<usize, Output = T> + Zero> Shl<usize> for Angle<T> {
type Output = Self;
fn shl(self, rhs: usize) -> Self {
if rhs >= core::mem::size_of::<T>() * 8 {
Angle(T::zero())
} else {
Angle(self.0 << rhs)
}
}
}
impl<T: Shr<usize, Output = T> + Zero> Shr<usize> for Angle<T> {
type Output = Self;
fn shr(self, rhs: usize) -> Self {
if rhs >= core::mem::size_of::<T>() * 8 {
Angle(T::zero())
} else {
Angle(self.0 >> rhs)
}
}
}
impl<T: CheckedMul> Angle<T> {
#[must_use]
pub fn checked_mul(self, rhs: T) -> Option<Self> {
self.0.checked_mul(&rhs).map(Angle)
}
}
impl<T: CheckedDiv> Angle<T> {
#[must_use]
pub fn checked_div(self, rhs: T) -> Option<Self> {
self.0.checked_div(&rhs).map(Angle)
}
}
impl<T: CheckedRem> Angle<T> {
#[must_use]
pub fn checked_rem(self, rhs: T) -> Option<Self> {
self.0.checked_rem(&rhs).map(Angle)
}
}
macro_rules! impl_consts {
($t:ty) => {
impl Angle<$t> {
pub const ZERO: Self = Angle(0);
pub const TAU: Self = Angle(0);
pub const PI: Self = Angle(<$t>::MAX / 2 + 1);
pub const FRAC_PI_2: Self = Angle(<$t>::MAX / 4 + 1);
pub const FRAC_PI_3: Self = Angle(<$t>::MAX / 6);
pub const FRAC_PI_4: Self = Angle(<$t>::MAX / 8 + 1);
pub const FRAC_PI_6: Self = Angle(<$t>::MAX / 12);
pub const FRAC_PI_8: Self = Angle(<$t>::MAX / 16 + 1);
}
};
}
impl<T: PrimInt + core::fmt::Display> core::fmt::Display for Angle<T> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
let (num, den) = self.to_frac();
if num.is_zero() {
return f.write_str("0");
}
let one = T::one();
match (num == one, den == one) {
(true, true) => f.write_str("π"),
(_, true) => write!(f, "{}π", num),
(true, _) => write!(f, "π/{}", den),
_ => write!(f, "{}π/{}", num, den),
}
}
}
impl_consts!(u8);
impl_consts!(u16);
impl_consts!(u32);
impl_consts!(u64);
impl_consts!(u128);
impl_consts!(usize);
pub type Angle8 = Angle<u8>;
pub type Angle16 = Angle<u16>;
pub type Angle32 = Angle<u32>;
pub type Angle64 = Angle<u64>;
pub type Angle128 = Angle<u128>;
pub type AngleSize = Angle<usize>;
#[cfg(feature = "serde")]
pub mod serde {
use crate::Angle;
use num_traits::{Bounded, NumCast, ToPrimitive, Zero};
use serde::{Deserialize, Deserializer, Serialize, Serializer};
pub mod raw {
use super::{Angle, Deserialize, Deserializer, Serialize, Serializer};
pub fn serialize<T, S>(angle: &Angle<T>, serializer: S) -> Result<S::Ok, S::Error>
where
T: Serialize,
S: Serializer,
{
angle.0.serialize(serializer)
}
pub fn deserialize<'de, T, D>(deserializer: D) -> Result<Angle<T>, D::Error>
where
T: Deserialize<'de>,
D: Deserializer<'de>,
{
T::deserialize(deserializer).map(Angle)
}
}
pub mod radians {
use super::{
Angle, Bounded, Deserialize, Deserializer, NumCast, Serializer, ToPrimitive, Zero,
};
pub fn serialize<T, S>(angle: &Angle<T>, serializer: S) -> Result<S::Ok, S::Error>
where
T: Copy + NumCast + ToPrimitive + Bounded + Zero,
S: Serializer,
{
serializer.serialize_f64(angle.to_radians::<f64>())
}
pub fn deserialize<'de, T, D>(deserializer: D) -> Result<Angle<T>, D::Error>
where
T: Copy + NumCast + ToPrimitive + Bounded + Zero,
D: Deserializer<'de>,
{
f64::deserialize(deserializer).map(Angle::<T>::from_radians)
}
}
pub mod degrees {
use super::{
Angle, Bounded, Deserialize, Deserializer, NumCast, Serializer, ToPrimitive, Zero,
};
pub fn serialize<T, S>(angle: &Angle<T>, serializer: S) -> Result<S::Ok, S::Error>
where
T: Copy + NumCast + ToPrimitive + Bounded + Zero,
S: Serializer,
{
serializer.serialize_f64(angle.to_degrees::<f64>())
}
pub fn deserialize<'de, T, D>(deserializer: D) -> Result<Angle<T>, D::Error>
where
T: Copy + NumCast + ToPrimitive + Bounded + Zero,
D: Deserializer<'de>,
{
f64::deserialize(deserializer).map(Angle::<T>::from_degrees)
}
}
}
#[cfg(test)]
extern crate alloc;
#[cfg(test)]
mod tests {
use super::*;
use alloc::format;
use core::f64::consts::{PI, TAU};
#[test]
fn pi_is_half_circle_u8() {
assert_eq!(Angle8::from_radians(PI), Angle(0b1000_0000));
}
#[test]
fn zero_and_tau_wrap_to_same() {
assert_eq!(Angle16::from_radians(0.0_f64), Angle16::from_radians(TAU),);
}
#[test]
fn roundtrip_u32_f64() {
let a = Angle32::from_radians(1.2345_f64);
let r: f64 = a.to_radians();
assert!((r - 1.2345).abs() < 1e-8);
}
#[test]
fn roundtrip_u16_f32() {
let a = Angle16::from_radians(1.0_f32);
let r: f32 = a.to_radians();
assert!((r - 1.0).abs() < 1e-3);
}
#[test]
fn widen_angle8_to_angle16_preserves_pi() {
assert_eq!(Angle::<u8>(0x80).cast::<u16>(), Angle(0x8000_u16));
}
#[test]
fn narrow_angle16_to_angle8_keeps_top_bits() {
assert_eq!(Angle::<u16>(0x80FF).cast::<u8>(), Angle(0x80_u8));
}
#[test]
fn identity_same_width() {
assert_eq!(
Angle::<u32>(0xDEAD_BEEF).cast::<u32>(),
Angle(0xDEAD_BEEF_u32),
);
}
#[test]
fn degrees_180_is_half_circle_u8() {
assert_eq!(Angle8::from_degrees(180.0_f64), Angle(0x80));
}
#[test]
fn degrees_wrap_at_360() {
assert_eq!(
Angle16::from_degrees(0.0_f64),
Angle16::from_degrees(360.0_f64),
);
}
#[test]
fn roundtrip_degrees_u32() {
let a = Angle32::from_degrees(123.456_f64);
let d: f64 = a.to_degrees();
assert!((d - 123.456).abs() < 1e-6);
}
#[test]
fn add_wraps_past_tau() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi + pi, Angle(0));
}
#[test]
fn sub_wraps_below_zero() {
let zero: Angle8 = Angle(0);
let pi: Angle8 = Angle(0x80);
assert_eq!(zero - pi, pi);
}
#[test]
fn mul_three_pi_wraps_to_pi() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi * 3_u8, Angle(0x80));
}
#[test]
fn div_halves_pi() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi / 2_u8, Angle(0x40));
}
#[test]
fn checked_mul_detects_overflow() {
let a: Angle8 = Angle(200);
assert!(a.checked_mul(2).is_none());
assert_eq!(a.checked_mul(1), Some(a));
}
#[test]
fn checked_div_by_zero_is_none() {
let a: Angle8 = Angle(0x80);
assert!(a.checked_div(0).is_none());
assert_eq!(a.checked_div(2), Some(Angle(0x40)));
}
#[test]
fn scale_half_pi_gives_quarter_pi() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi.scale(0.5_f64), Angle(0x40));
}
#[test]
fn scale_two_wraps_to_zero() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi.scale(2.0_f64), Angle(0));
}
#[test]
fn constants_match_from_radians() {
assert_eq!(Angle8::PI, Angle8::from_radians(PI));
assert_eq!(Angle8::FRAC_PI_2, Angle8::from_radians(PI / 2.0));
assert_eq!(Angle8::FRAC_PI_4, Angle8::from_radians(PI / 4.0));
assert_eq!(Angle8::FRAC_PI_8, Angle8::from_radians(PI / 8.0));
assert_eq!(Angle8::FRAC_PI_3, Angle8::from_radians(PI / 3.0));
assert_eq!(Angle8::FRAC_PI_6, Angle8::from_radians(PI / 6.0));
assert_eq!(Angle8::TAU, Angle8::ZERO);
assert_eq!(Angle8::ZERO, Angle(0));
assert_eq!(Angle8::PI, Angle(0x80));
assert_eq!(Angle8::FRAC_PI_2, Angle(0x40));
}
#[test]
fn neg_pi_is_pi() {
let pi: Angle8 = Angle(0x80);
assert_eq!(-pi, pi);
}
#[test]
fn neg_frac_pi_2_is_three_quarter_turn() {
let quarter: Angle8 = Angle(0x40);
assert_eq!(-quarter, Angle(0xC0));
}
#[test]
fn from_atan2_north_is_frac_pi_2() {
let a = Angle64::from_atan2(1.0_f64, 0.0_f64);
assert_eq!(a, Angle64::FRAC_PI_2);
}
#[test]
fn tan_frac_pi_4_is_one() {
let t: f64 = Angle64::FRAC_PI_4.tan();
assert!((t - 1.0).abs() < 1e-9);
}
#[test]
fn sin_cos_matches_individual() {
let a: Angle32 = Angle::<u32>(0x1234_5678);
let (s, c): (f64, f64) = a.sin_cos();
assert!((s - a.sin::<f64>()).abs() < 1e-12);
assert!((c - a.cos::<f64>()).abs() < 1e-12);
}
#[test]
fn sin_pi_is_near_zero() {
let pi: Angle8 = Angle(0x80);
let s: f64 = pi.sin();
assert!(s.abs() < 1e-9);
}
#[test]
fn cos_zero_is_one() {
let zero: Angle16 = Angle(0);
let c: f64 = zero.cos();
assert!((c - 1.0).abs() < 1e-9);
}
#[test]
fn cos_pi_is_minus_one() {
let pi: Angle8 = Angle(0x80);
let c: f64 = pi.cos();
assert!((c + 1.0).abs() < 1e-9);
}
#[test]
fn from_radians_nan_is_zero() {
assert_eq!(Angle8::from_radians(f64::NAN), Angle(0));
assert_eq!(Angle8::from_radians(f64::INFINITY), Angle(0));
assert_eq!(Angle8::from_radians(f64::NEG_INFINITY), Angle(0));
}
#[test]
fn from_degrees_non_finite_is_zero() {
assert_eq!(Angle16::from_degrees(f32::NAN), Angle(0));
assert_eq!(Angle16::from_degrees(f32::INFINITY), Angle(0));
}
#[test]
fn scale_non_finite_is_zero() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi.scale(f64::NAN), Angle(0));
assert_eq!(pi.scale(f64::INFINITY), Angle(0));
}
#[test]
fn scale_negative_wraps() {
let half_pi: Angle8 = Angle(0x40);
assert_eq!(half_pi.scale(-1.0_f64), Angle(0xC0));
}
#[test]
fn ratio_pi_over_half_pi_is_two() {
let pi: Angle8 = Angle(0x80);
let half_pi: Angle8 = Angle(0x40);
let r: f64 = pi.ratio(half_pi);
assert!((r - 2.0).abs() < 1e-9);
}
#[test]
fn shl_doubles_angle() {
let eighth: Angle8 = Angle(0x20);
assert_eq!(eighth << 1, Angle(0x40));
}
#[test]
fn shr_halves_pi() {
let pi: Angle8 = Angle(0x80);
assert_eq!(pi >> 1, Angle(0x40));
}
#[test]
fn shl_past_width_saturates_to_zero() {
let a: Angle8 = Angle(0xFF);
assert_eq!(a << 8, Angle(0));
assert_eq!(a << 100, Angle(0));
}
#[test]
fn shr_past_width_saturates_to_zero() {
let a: Angle8 = Angle(0xFF);
assert_eq!(a >> 8, Angle(0));
assert_eq!(a >> 100, Angle(0));
}
#[test]
fn widen_then_narrow_is_lossless() {
let a: Angle8 = Angle(0b1010_1010);
assert_eq!(a.cast::<u128>().cast::<u8>(), a);
}
#[test]
fn display_three_pi_over_two() {
assert_eq!(format!("{}", Angle::<u8>(0b1100_0000)), "3π/2");
}
#[test]
fn display_zero() {
assert_eq!(format!("{}", Angle8::ZERO), "0");
}
#[test]
fn display_pi() {
assert_eq!(format!("{}", Angle8::PI), "π");
}
#[test]
fn display_frac_pi_2() {
assert_eq!(format!("{}", Angle8::FRAC_PI_2), "π/2");
}
#[test]
fn display_frac_pi_4() {
assert_eq!(format!("{}", Angle8::FRAC_PI_4), "π/4");
}
#[test]
fn display_three_pi_over_four() {
assert_eq!(format!("{}", Angle::<u8>(0b0110_0000)), "3π/4");
}
#[test]
fn display_angle16_small_numerator() {
assert_eq!(format!("{}", Angle::<u16>(1)), "π/32768");
}
#[test]
fn to_frac_three_pi_over_two() {
assert_eq!(Angle::<u8>(0b1100_0000).to_frac(), (3, 2));
}
#[test]
fn to_frac_zero() {
assert_eq!(Angle8::ZERO.to_frac(), (0, 1));
}
#[test]
fn to_frac_pi() {
assert_eq!(Angle8::PI.to_frac(), (1, 1));
}
#[test]
fn to_frac_odd_numerator_keeps_full_denominator() {
assert_eq!(Angle::<u16>(1).to_frac(), (1, 32768));
}
#[test]
fn from_frac_three_pi_over_two() {
assert_eq!(Angle8::from_frac(3, 2), Angle(0xC0));
}
#[test]
fn from_frac_pi_is_half_circle() {
assert_eq!(Angle8::from_frac(1, 1), Angle8::PI);
}
#[test]
fn from_frac_zero() {
assert_eq!(Angle8::from_frac(0, 1), Angle8::ZERO);
}
#[test]
fn from_frac_wraps_past_two_pi() {
assert_eq!(Angle8::from_frac(3, 1), Angle8::PI);
assert_eq!(Angle8::from_frac(5, 2), Angle8::FRAC_PI_2);
}
#[test]
fn from_frac_quarter_turn() {
assert_eq!(Angle8::from_frac(1, 2), Angle8::FRAC_PI_2);
}
#[test]
fn from_frac_roundtrips_to_frac() {
let cases: [Angle8; 6] = [
Angle8::ZERO,
Angle8::PI,
Angle8::FRAC_PI_2,
Angle8::FRAC_PI_4,
Angle8::FRAC_PI_8,
Angle(0b1010_0000),
];
for a in cases {
let (n, d) = a.to_frac();
assert_eq!(Angle8::from_frac(n, d), a);
}
}
}
#[cfg(all(test, feature = "serde"))]
mod serde_tests {
use super::{Angle, Angle8, Angle32};
use ::serde::{Deserialize, Serialize};
use core::f64::consts::PI;
#[derive(Serialize, Deserialize, Debug, PartialEq)]
#[serde(crate = "::serde")]
struct Raw {
#[serde(with = "crate::serde::raw")]
a: Angle8,
}
#[derive(Serialize, Deserialize, Debug, PartialEq)]
#[serde(crate = "::serde")]
struct Rads {
#[serde(with = "crate::serde::radians")]
a: Angle32,
}
#[derive(Serialize, Deserialize, Debug, PartialEq)]
#[serde(crate = "::serde")]
struct Degs {
#[serde(with = "crate::serde::degrees")]
a: Angle32,
}
#[test]
fn raw_serializes_as_inner_integer() {
let v = Raw { a: Angle8::PI };
assert_eq!(serde_json::to_string(&v).unwrap(), r#"{"a":128}"#);
}
#[test]
fn raw_roundtrip_is_exact() {
let v = Raw { a: Angle(0xA5) };
let s = serde_json::to_string(&v).unwrap();
assert_eq!(serde_json::from_str::<Raw>(&s).unwrap(), v);
}
#[test]
fn radians_roundtrip_within_precision() {
let v = Rads {
a: Angle32::from_radians(1.2345_f64),
};
let s = serde_json::to_string(&v).unwrap();
let r: Rads = serde_json::from_str(&s).unwrap();
let delta: f64 = (r.a - v.a).to_radians();
let delta = delta.min(core::f64::consts::TAU - delta);
assert!(delta < 1e-8, "delta = {delta}");
}
#[test]
fn radians_serializes_pi() {
let v = Rads {
a: Angle32::from_radians(PI),
};
let s = serde_json::to_string(&v).unwrap();
let parsed: serde_json::Value = serde_json::from_str(&s).unwrap();
let f = parsed["a"].as_f64().unwrap();
assert!((f - PI).abs() < 1e-9);
}
#[test]
fn degrees_roundtrip_within_precision() {
let v = Degs {
a: Angle32::from_degrees(123.456_f64),
};
let s = serde_json::to_string(&v).unwrap();
let r: Degs = serde_json::from_str(&s).unwrap();
let delta: f64 = (r.a - v.a).to_degrees();
let delta = delta.min(360.0 - delta);
assert!(delta < 1e-6, "delta = {delta}");
}
#[test]
fn degrees_serializes_180() {
let v = Degs {
a: Angle32::from_degrees(180.0_f64),
};
assert_eq!(serde_json::to_string(&v).unwrap(), r#"{"a":180.0}"#);
}
}