#![cfg_attr(not(test), no_std)]
#![allow(clippy::float_cmp)]
pub mod trig;
pub mod vector2d;
use core::cmp::{Ordering, PartialOrd};
use core::convert::From;
use core::ops::{Add, AddAssign, Neg, Sub, SubAssign};
use num_traits::{Float, float::FloatConst};
use serde::{Deserialize, Deserializer, Serialize, Serializer};
pub const ONE_HUNDRED_AND_EIGHTY: f64 = 180.0;
pub const THREE_HUNDRED_AND_SIXTY: f64 = 360.0;
#[derive(Clone, Copy, Debug, Eq, PartialEq, Serialize, Deserialize)]
#[repr(transparent)]
pub struct Degrees<T: Float>(pub T);
impl<T: Float> Degrees<T> {
#[must_use]
pub fn abs(self) -> Self {
Self(self.0.abs())
}
#[must_use]
pub fn half(self) -> Self {
let half = T::one() / (T::one() + T::one());
Self(half * self.0)
}
#[allow(clippy::missing_panics_doc)]
#[must_use]
pub fn opposite(self) -> Self {
let one_eighty =
T::from(ONE_HUNDRED_AND_EIGHTY).expect("Could not convert constant to Float");
Self(if self.0 > T::zero() {
self.0 - one_eighty
} else {
self.0 + one_eighty
})
}
}
impl<T: Float> Default for Degrees<T> {
fn default() -> Self {
Self(T::zero())
}
}
impl<T: Float> Neg for Degrees<T> {
type Output = Self;
fn neg(self) -> Self::Output {
Self(T::zero() - self.0)
}
}
impl<T: Float> Add for Degrees<T> {
type Output = Self;
fn add(self, other: Self) -> Self::Output {
let one_eighty =
T::from(ONE_HUNDRED_AND_EIGHTY).expect("Could not convert constant to Float");
let three_sixty =
T::from(THREE_HUNDRED_AND_SIXTY).expect("Could not convert constant to Float");
let (s, t) = two_sum(self.0, other.0);
Self(if s <= -one_eighty {
s + three_sixty + t
} else if s > one_eighty {
s - three_sixty + t
} else {
s
})
}
}
impl<T: Float> AddAssign for Degrees<T> {
fn add_assign(&mut self, other: Self) {
*self = *self + other;
}
}
impl<T: Float> Sub for Degrees<T> {
type Output = Self;
fn sub(self, other: Self) -> Self::Output {
self + -other
}
}
impl<T: Float> SubAssign for Degrees<T> {
fn sub_assign(&mut self, other: Self) {
*self = *self - other;
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq, PartialOrd)]
#[repr(transparent)]
pub struct Radians<T: Float + FloatConst>(pub T);
impl<T: Float + FloatConst> Radians<T> {
#[must_use]
pub fn abs(self) -> Self {
Self(self.0.abs())
}
#[must_use]
pub fn half(self) -> Self {
let half = T::one() / (T::one() + T::one());
Self(half * self.0)
}
#[must_use]
pub fn opposite(self) -> Self {
Self(if self.0 > T::zero() {
self.0 - T::PI()
} else {
self.0 + T::PI()
})
}
#[must_use]
pub fn clamp(self, max_value: Self) -> Self {
Self(self.0.clamp(T::zero(), max_value.0))
}
}
impl<T: Float + FloatConst> Default for Radians<T> {
fn default() -> Self {
Self(T::zero())
}
}
impl<T: Float + FloatConst> Neg for Radians<T> {
type Output = Self;
fn neg(self) -> Self {
Self(T::zero() - self.0)
}
}
impl<T: Float + FloatConst> Add for Radians<T> {
type Output = Self;
fn add(self, other: Self) -> Self::Output {
let (s, t) = two_sum(self.0, other.0);
Self(if s <= -T::PI() {
s + T::TAU() + t
} else if s > T::PI() {
s - T::TAU() + t
} else {
s
})
}
}
impl<T: Float + FloatConst> AddAssign for Radians<T> {
fn add_assign(&mut self, other: Self) {
*self = *self + other;
}
}
impl<T: Float + FloatConst> Sub for Radians<T> {
type Output = Self;
fn sub(self, other: Self) -> Self::Output {
self + -other
}
}
impl<T: Float + FloatConst> SubAssign for Radians<T> {
fn sub_assign(&mut self, other: Self) {
*self = *self - other;
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Angle<T: Float> {
sin: trig::UnitNegRange<T>,
cos: trig::UnitNegRange<T>,
}
impl<T: Float> Default for Angle<T> {
fn default() -> Self {
Self {
sin: trig::UnitNegRange(T::zero()),
cos: trig::UnitNegRange(T::one()),
}
}
}
impl<T: Float> Validate for Angle<T> {
fn is_valid(&self) -> bool {
self.sin.is_valid()
&& self.cos.is_valid()
&& is_within_tolerance(
T::one(),
(self.sin.0.powi(2) + self.cos.0.powi(2)).sqrt(),
T::epsilon(),
)
}
}
impl<T: Float> Angle<T> {
#[must_use]
pub const fn new(sin: trig::UnitNegRange<T>, cos: trig::UnitNegRange<T>) -> Self {
Self { sin, cos }
}
#[must_use]
pub fn from_y_x(y: T, x: T) -> Self {
let length = y.hypot(x);
if is_small(length, T::epsilon()) {
Self::default()
} else {
Self::new(
trig::UnitNegRange::clamp(y / length),
trig::UnitNegRange::clamp(x / length),
)
}
}
#[must_use]
pub const fn sin(self) -> trig::UnitNegRange<T> {
self.sin
}
#[must_use]
pub const fn cos(self) -> trig::UnitNegRange<T> {
self.cos
}
#[must_use]
pub fn tan(self) -> Option<T> {
trig::tan(self.sin, self.cos)
}
#[must_use]
pub fn csc(self) -> Option<T> {
trig::csc(self.sin)
}
#[must_use]
pub fn sec(self) -> Option<T> {
trig::sec(self.cos)
}
#[must_use]
pub fn cot(self) -> Option<T> {
trig::cot(self.sin, self.cos)
}
#[must_use]
pub fn abs(self) -> Self {
Self {
sin: self.sin.abs(),
cos: self.cos,
}
}
#[must_use]
pub fn opposite(self) -> Self {
Self {
sin: -self.sin,
cos: -self.cos,
}
}
#[must_use]
pub fn quarter_turn_cw(self) -> Self {
Self {
sin: self.cos,
cos: -self.sin,
}
}
#[must_use]
pub fn quarter_turn_ccw(self) -> Self {
Self {
sin: -self.cos,
cos: self.sin,
}
}
#[must_use]
pub fn negate_cos(self) -> Self {
Self {
sin: self.sin,
cos: -self.cos,
}
}
#[must_use]
pub fn double(self) -> Self {
Self {
sin: trig::UnitNegRange::clamp((self.sin.0 + self.sin.0) * self.cos.0),
cos: trig::sq_a_minus_sq_b(self.cos, self.sin),
}
}
#[must_use]
pub fn half(self) -> Self {
Self {
sin: trig::UnitNegRange((trig::sq_sine_half(self.cos).sqrt()).copysign(self.sin.0)),
cos: trig::UnitNegRange(trig::sq_cosine_half(self.cos).sqrt()),
}
}
}
impl<T: Float> Neg for Angle<T> {
type Output = Self;
fn neg(self) -> Self {
Self {
sin: -self.sin,
cos: self.cos,
}
}
}
impl<T: Float> Add for Angle<T> {
type Output = Self;
fn add(self, other: Self) -> Self::Output {
Self {
sin: trig::sine_sum(self.sin, self.cos, other.sin, other.cos),
cos: trig::cosine_sum(self.sin, self.cos, other.sin, other.cos),
}
}
}
impl<T: Float> AddAssign for Angle<T> {
fn add_assign(&mut self, other: Self) {
*self = *self + other;
}
}
impl<T: Float> Sub for Angle<T> {
type Output = Self;
fn sub(self, other: Self) -> Self::Output {
Self {
sin: trig::sine_diff(self.sin, self.cos, other.sin, other.cos),
cos: trig::cosine_diff(self.sin, self.cos, other.sin, other.cos),
}
}
}
impl<T: Float> SubAssign for Angle<T> {
fn sub_assign(&mut self, other: Self) {
*self = *self - other;
}
}
impl<T: Float> PartialOrd for Angle<T> {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
let delta = *other - *self;
trig::UnitNegRange(T::zero()).partial_cmp(&delta.sin)
}
}
impl<T> From<Degrees<T>> for Angle<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(a: Degrees<T>) -> Self {
let (sin, cos) = trig::sincosd(a);
Self { sin, cos }
}
}
impl<T> From<(Degrees<T>, Degrees<T>)> for Angle<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(params: (Degrees<T>, Degrees<T>)) -> Self {
let (sin, cos) = trig::sincosd_diff(params.0, params.1);
Self { sin, cos }
}
}
impl<T> From<Radians<T>> for Angle<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(a: Radians<T>) -> Self {
let (sin, cos) = trig::sincos(a);
Self { sin, cos }
}
}
impl<T> From<(Radians<T>, Radians<T>)> for Angle<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(params: (Radians<T>, Radians<T>)) -> Self {
let (sin, cos) = trig::sincos_diff(params.0, params.1);
Self { sin, cos }
}
}
impl<T> From<Angle<T>> for Radians<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(a: Angle<T>) -> Self {
trig::arctan2(a.sin, a.cos)
}
}
impl<T> From<Angle<T>> for Degrees<T>
where
T: Float + FloatConst,
f64: From<T>,
{
fn from(a: Angle<T>) -> Self {
trig::arctan2d(a.sin, a.cos)
}
}
impl<T> Serialize for Angle<T>
where
T: Float + FloatConst + Serialize,
f64: From<T>,
{
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: Serializer,
{
serializer.serialize_newtype_struct("Degrees", &Degrees::from(*self))
}
}
impl<'de, T> Deserialize<'de> for Angle<T>
where
T: Float + FloatConst + Deserialize<'de>,
f64: From<T>,
{
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: Deserializer<'de>,
{
Ok(Self::from(Degrees::<T>::deserialize(deserializer)?))
}
}
#[must_use]
pub fn two_sum<T>(a: T, b: T) -> (T, T)
where
T: Copy + Add<Output = T> + Sub<Output = T>,
{
let s = a + b;
let a_prime = s - b;
let b_prime = s - a_prime;
let delta_a = a - a_prime;
let delta_b = b - b_prime;
let t = delta_a + delta_b;
(s, t)
}
#[must_use]
pub fn min<T>(a: T, b: T) -> T
where
T: PartialOrd + Copy,
{
if b < a { b } else { a }
}
#[must_use]
pub fn max<T>(a: T, b: T) -> T
where
T: PartialOrd + Copy,
{
if b < a { a } else { b }
}
pub trait Validate {
fn is_valid(&self) -> bool;
}
#[must_use]
pub fn is_small<T>(value: T, tolerance: T) -> bool
where
T: PartialOrd + Copy,
{
value <= tolerance
}
#[must_use]
pub fn is_within_tolerance<T>(reference: T, value: T, tolerance: T) -> bool
where
T: PartialOrd + Copy + Sub<Output = T>,
{
let delta = max(reference, value) - min(reference, value);
is_small(delta, tolerance)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_degrees_traits() {
let zero = Degrees::default();
assert_eq!(Degrees(0.0), zero);
let one = Degrees(1.0);
let mut one_clone = one.clone();
assert!(one_clone == one);
let two = Degrees(2.0);
let m_one = Degrees(-1.0);
assert_eq!(m_one, -one);
assert_eq!(one, m_one.abs());
assert_eq!(one, two.half());
assert_eq!(m_one, one - two);
one_clone -= two;
assert_eq!(m_one, one_clone);
assert_eq!(one, m_one + two);
one_clone += two;
assert_eq!(one, one_clone);
let d_120 = Degrees(120.0);
let d_m120 = Degrees(-120.0);
assert_eq!(d_120, d_m120.abs());
assert_eq!(Degrees(30.0), Degrees(-155.0) - Degrees(175.0));
assert_eq!(d_m120, d_120 + d_120);
assert_eq!(d_120, d_m120 + d_m120);
assert_eq!(d_120, d_m120 - d_120);
assert_eq!(Degrees(-60.0), d_120.opposite());
assert_eq!(Degrees(60.0), d_m120.opposite());
let serialized = serde_json::to_string(&one).unwrap();
let deserialized: Degrees<f64> = serde_json::from_str(&serialized).unwrap();
assert_eq!(one, deserialized);
let bad_text = "junk";
let _serde_error = serde_json::from_str::<Degrees<f64>>(&bad_text).unwrap_err();
print!("Degrees: {:?}", one);
}
#[test]
fn test_radians_traits() {
let zero = Radians::default();
assert_eq!(Radians(0.0), zero);
let one = Radians(1.0);
let mut one_clone = one.clone();
assert!(one_clone == one);
let two = Radians(2.0);
let m_two = -two;
assert!(one < two);
let m_one = Radians(-1.0);
assert_eq!(m_one, -one);
assert_eq!(one, m_one.abs());
assert_eq!(one, two.half());
assert_eq!(m_one, one - two);
one_clone -= two;
assert_eq!(m_one, one_clone);
assert_eq!(one, m_one + two);
one_clone += two;
assert_eq!(one, one_clone);
let result_1 = m_two - two;
assert_eq!(core::f64::consts::TAU - 4.0, result_1.0);
assert_eq!(core::f64::consts::PI - 4.0, result_1.opposite().0);
let result_2 = two - m_two;
assert_eq!(4.0 - core::f64::consts::TAU, result_2.0);
assert_eq!(4.0 - core::f64::consts::PI, result_2.opposite().0);
let value = Radians(-f64::EPSILON);
assert_eq!(Radians(0.0), value.clamp(Radians(1.0)));
let value = Radians(0.0);
assert_eq!(Radians(0.0), value.clamp(Radians(1.0)));
let value = Radians(1.0);
assert_eq!(Radians(1.0), value.clamp(Radians(1.0)));
let value = Radians(1.0 + f64::EPSILON);
assert_eq!(Radians(1.0), value.clamp(Radians(1.0)));
print!("Radians: {:?}", one);
}
#[test]
fn test_angle_traits() {
let zero = Angle::<f64>::default();
assert_eq!(0.0, zero.sin().0);
assert_eq!(1.0, zero.cos().0);
assert_eq!(0.0, zero.tan().unwrap());
assert!(zero.csc().is_none());
assert_eq!(1.0, zero.sec().unwrap());
assert!(zero.cot().is_none());
assert!(zero.is_valid());
let zero_clone = zero.clone();
assert_eq!(zero, zero_clone);
let one = Angle::from_y_x(1.0, 0.0);
assert_eq!(1.0, one.sin().0);
assert_eq!(0.0, one.cos().0);
assert!(one.tan().is_none());
assert_eq!(1.0, one.csc().unwrap());
assert!(one.sec().is_none());
assert_eq!(0.0, one.cot().unwrap());
assert!(one.is_valid());
let angle_m45 = Angle::from_y_x(-f64::EPSILON, f64::EPSILON);
assert!(is_within_tolerance(
-core::f64::consts::FRAC_1_SQRT_2,
angle_m45.sin().0,
f64::EPSILON
));
assert!(is_within_tolerance(
core::f64::consts::FRAC_1_SQRT_2,
angle_m45.cos().0,
f64::EPSILON
));
assert!(angle_m45 < zero);
let serialized = serde_json::to_string(&zero).unwrap();
let deserialized: Angle<f64> = serde_json::from_str(&serialized).unwrap();
assert_eq!(zero, deserialized);
let bad_text = "junk";
let _serde_error = serde_json::from_str::<Angle<f64>>(&bad_text).unwrap_err();
print!("Angle: {:?}", angle_m45);
}
#[test]
fn test_angle_conversion() {
let zero = Angle::default();
let too_small = Angle::from_y_x(-f64::EPSILON / 2.0, f64::EPSILON / 2.0);
assert!(too_small.is_valid());
assert_eq!(zero, too_small);
let small = Angle::from(Radians(-trig::MAX_COS_ANGLE_IS_ONE));
assert!(small.is_valid());
assert_eq!(-trig::MAX_COS_ANGLE_IS_ONE, small.sin().0);
assert_eq!(1.0, small.cos().0);
assert_eq!(-trig::MAX_COS_ANGLE_IS_ONE, Radians::from(small).0);
let angle_30 = Angle::from((
Radians(core::f64::consts::FRAC_PI_3),
Radians(core::f64::consts::FRAC_PI_6),
));
assert!(angle_30.is_valid());
assert_eq!(0.5, angle_30.sin().0);
assert_eq!(3.0_f64.sqrt() / 2.0, angle_30.cos().0);
assert_eq!(30.0, Degrees::from(angle_30).0);
assert_eq!(core::f64::consts::FRAC_PI_6, Radians::from(angle_30).0);
let angle_45 = Angle::from(Radians(core::f64::consts::FRAC_PI_4));
assert!(angle_45.is_valid());
assert_eq!(core::f64::consts::FRAC_1_SQRT_2, angle_45.sin().0);
assert_eq!(core::f64::consts::FRAC_1_SQRT_2, angle_45.cos().0);
assert_eq!(45.0, Degrees::from(angle_45).0);
assert_eq!(core::f64::consts::FRAC_PI_4, Radians::from(angle_45).0);
let angle_m45 = Angle::from(Degrees(-45.0));
assert!(angle_m45.is_valid());
assert_eq!(-core::f64::consts::FRAC_1_SQRT_2, angle_m45.sin().0);
assert_eq!(core::f64::consts::FRAC_1_SQRT_2, angle_m45.cos().0);
assert_eq!(-45.0, Degrees::from(angle_m45).0);
assert_eq!(-core::f64::consts::FRAC_PI_4, Radians::from(angle_m45).0);
let angle_60 = Angle::from((Degrees(-140.0), Degrees(160.0)));
assert!(angle_60.is_valid());
assert_eq!(3.0_f64.sqrt() / 2.0, angle_60.sin().0);
assert_eq!(0.5, angle_60.cos().0);
assert_eq!(60.0, Degrees::from(angle_60).0);
assert!(is_within_tolerance(
core::f64::consts::FRAC_PI_3,
Radians::from(angle_60).0,
f64::EPSILON
));
let angle_30 = Angle::from((Degrees(-155.0), Degrees(175.0)));
assert_eq!(0.5, angle_30.sin().0);
assert_eq!(3.0_f64.sqrt() / 2.0, angle_30.cos().0);
assert_eq!(30.0, Degrees::from(angle_30).0);
assert_eq!(core::f64::consts::FRAC_PI_6, Radians::from(angle_30).0);
let angle_120 = Angle::from(Degrees(120.0));
assert!(angle_120.is_valid());
assert_eq!(3.0_f64.sqrt() / 2.0, angle_120.sin().0);
assert_eq!(-0.5, angle_120.cos().0);
assert_eq!(120.0, Degrees::from(angle_120).0);
assert_eq!(
2.0 * core::f64::consts::FRAC_PI_3,
Radians::from(angle_120).0
);
let angle_m120 = Angle::from(Degrees(-120.0));
assert!(angle_m120.is_valid());
assert_eq!(-3.0_f64.sqrt() / 2.0, angle_m120.sin().0);
assert_eq!(-0.5, angle_m120.cos().0);
assert_eq!(-120.0, Degrees::from(angle_m120).0);
assert_eq!(
-2.0 * core::f64::consts::FRAC_PI_3,
Radians::from(angle_m120).0
);
let angle_m140 = Angle::from(Degrees(-140.0));
assert!(angle_m140.is_valid());
assert!(is_within_tolerance(
-0.6427876096865393,
angle_m140.sin().0,
f64::EPSILON
));
assert!(is_within_tolerance(
-0.7660444431189781,
angle_m140.cos().0,
f64::EPSILON
));
assert_eq!(-140.0, Degrees::from(angle_m140).0);
let angle_180 = Angle::from(Degrees(180.0));
assert!(angle_180.is_valid());
assert_eq!(0.0, angle_180.sin().0);
assert_eq!(-1.0, angle_180.cos().0);
assert_eq!(180.0, Degrees::from(angle_180).0);
assert_eq!(core::f64::consts::PI, Radians::from(angle_180).0);
}
#[test]
fn test_angle_maths() {
let degrees_30 = Angle::from(Degrees(30.0));
let degrees_60 = Angle::from(Degrees(60.0));
let degrees_120 = Angle::from(Degrees(120.0));
let degrees_m120 = -degrees_120;
assert!(degrees_120 < degrees_m120);
assert_eq!(degrees_120, degrees_m120.abs());
assert_eq!(degrees_60, degrees_m120.opposite());
assert_eq!(degrees_120, degrees_30.quarter_turn_cw());
assert_eq!(degrees_30, degrees_120.quarter_turn_ccw());
assert_eq!(degrees_60, degrees_120.negate_cos());
let result = degrees_m120 - degrees_120;
assert_eq!(Degrees(120.0).0, Degrees::from(result).0);
let mut result = degrees_m120;
result -= degrees_120;
assert_eq!(Degrees(120.0).0, Degrees::from(result).0);
let result = degrees_120 + degrees_120;
assert_eq!(Degrees(-120.0).0, Degrees::from(result).0);
let mut result = degrees_120;
result += degrees_120;
assert_eq!(Degrees(-120.0).0, Degrees::from(result).0);
let result = degrees_60.double();
assert_eq!(Degrees(120.0).0, Degrees::from(result).0);
let result = degrees_120.double();
assert_eq!(Degrees(-120.0).0, Degrees::from(result).0);
assert_eq!(-degrees_60, degrees_m120.half());
}
#[test]
fn test_two_sum() {
let result = two_sum(1.0, 1.0);
assert_eq!(2.0, result.0);
assert_eq!(0.0, result.1);
let result = two_sum(1.0, 1e-53);
assert_eq!(1.0, result.0);
assert_eq!(1e-53, result.1);
let result = two_sum(1.0, -1e-53);
assert_eq!(1.0, result.0);
assert_eq!(-1e-53, result.1);
}
#[test]
fn test_min_and_max() {
assert_eq!(min(-1.0 + f64::EPSILON, -1.0), -1.0);
assert_eq!(min(1.0, 1.0 + f64::EPSILON), 1.0);
assert_eq!(max(-1.0, -1.0 - f64::EPSILON), -1.0);
assert_eq!(max(1.0 - f64::EPSILON, 1.0), 1.0);
}
#[test]
fn test_is_within_tolerance() {
assert_eq!(
false,
is_within_tolerance(1.0 - 2.0 * f64::EPSILON, 1.0, f64::EPSILON)
);
assert!(is_within_tolerance(1.0 - f64::EPSILON, 1.0, f64::EPSILON));
assert!(is_within_tolerance(1.0 + f64::EPSILON, 1.0, f64::EPSILON));
assert_eq!(
false,
is_within_tolerance(1.0 + 2.0 * f64::EPSILON, 1.0, f64::EPSILON)
);
}
}