#![no_std] use core::{ cmp::{Eq, Ord, PartialEq, PartialOrd}, fmt::{Debug, Display}, ops::{ Add, AddAssign, BitAnd, Div, DivAssign, Mul, MulAssign, Neg, Not, Rem, RemAssign, Shl, Shr, Sub, SubAssign, }, }; #[macro_export] macro_rules! num { ($value:literal) => {{ $crate::Num::new_from_parts(agb_macros::num!($value)) }}; } pub trait Number: Sized + Copy + PartialOrd + Ord + PartialEq + Eq + Add + Sub + Rem + Div + Mul { } impl Number for Num {} impl Number for I {} pub trait FixedWidthUnsignedInteger: Sized + Copy + PartialOrd + Ord + PartialEq + Eq + Shl + Shr + Add + Sub + Not + BitAnd + Rem + Div + Mul + From + Debug + Display { fn zero() -> Self; fn one() -> Self; fn ten() -> Self; fn from_as_i32(v: i32) -> Self; } pub trait FixedWidthSignedInteger: FixedWidthUnsignedInteger + Neg { #[must_use] fn fixed_abs(self) -> Self; } macro_rules! fixed_width_unsigned_integer_impl { ($T: ty) => { impl FixedWidthUnsignedInteger for $T { #[inline(always)] fn zero() -> Self { 0 } #[inline(always)] fn one() -> Self { 1 } #[inline(always)] fn ten() -> Self { 10 } #[inline(always)] fn from_as_i32(v: i32) -> Self { v as $T } } }; } macro_rules! fixed_width_signed_integer_impl { ($T: ty) => { impl FixedWidthSignedInteger for $T { #[inline(always)] fn fixed_abs(self) -> Self { self.abs() } } }; } fixed_width_unsigned_integer_impl!(i16); fixed_width_unsigned_integer_impl!(u16); fixed_width_unsigned_integer_impl!(i32); fixed_width_unsigned_integer_impl!(u32); fixed_width_unsigned_integer_impl!(usize); fixed_width_signed_integer_impl!(i16); fixed_width_signed_integer_impl!(i32); #[repr(C)] #[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord)] pub struct Num(I); pub type FixedNum = Num; pub type Integer = Num; impl From for Num { fn from(value: I) -> Self { Num(value << N) } } impl Default for Num where I: FixedWidthUnsignedInteger, { fn default() -> Self { Num(I::zero()) } } impl Add for Num where I: FixedWidthUnsignedInteger, T: Into>, { type Output = Self; fn add(self, rhs: T) -> Self::Output { Num(self.0 + rhs.into().0) } } impl AddAssign for Num where I: FixedWidthUnsignedInteger, T: Into>, { fn add_assign(&mut self, rhs: T) { self.0 = (*self + rhs.into()).0 } } impl Sub for Num where I: FixedWidthUnsignedInteger, T: Into>, { type Output = Self; fn sub(self, rhs: T) -> Self::Output { Num(self.0 - rhs.into().0) } } impl SubAssign for Num where I: FixedWidthUnsignedInteger, T: Into>, { fn sub_assign(&mut self, rhs: T) { self.0 = (*self - rhs.into()).0 } } impl Mul> for Num where I: FixedWidthUnsignedInteger, { type Output = Self; fn mul(self, rhs: Num) -> Self::Output { Num(((self.floor() * rhs.floor()) << N) + (self.floor() * rhs.frac() + rhs.floor() * self.frac()) + ((self.frac() * rhs.frac()) >> N)) } } impl Mul for Num where I: FixedWidthUnsignedInteger, { type Output = Self; fn mul(self, rhs: I) -> Self::Output { Num(self.0 * rhs) } } impl MulAssign for Num where I: FixedWidthUnsignedInteger, Num: Mul>, { fn mul_assign(&mut self, rhs: T) { self.0 = (*self * rhs).0 } } impl Div> for Num where I: FixedWidthUnsignedInteger, { type Output = Self; fn div(self, rhs: Num) -> Self::Output { Num((self.0 << N) / rhs.0) } } impl Div for Num where I: FixedWidthUnsignedInteger, { type Output = Self; fn div(self, rhs: I) -> Self::Output { Num(self.0 / rhs) } } impl DivAssign for Num where I: FixedWidthUnsignedInteger, Num: Div>, { fn div_assign(&mut self, rhs: T) { self.0 = (*self / rhs).0 } } impl Rem for Num where I: FixedWidthUnsignedInteger, T: Into>, { type Output = Self; fn rem(self, modulus: T) -> Self::Output { Num(self.0 % modulus.into().0) } } impl RemAssign for Num where I: FixedWidthUnsignedInteger, T: Into>, { fn rem_assign(&mut self, modulus: T) { self.0 = (*self % modulus).0 } } impl Neg for Num { type Output = Self; fn neg(self) -> Self::Output { Num(-self.0) } } impl Num { pub fn change_base, const M: usize>(self) -> Num { let n: J = self.0.into(); if N < M { Num(n << (M - N)) } else { Num(n >> (N - M)) } } pub fn from_raw(n: I) -> Self { Num(n) } pub fn to_raw(self) -> I { self.0 } pub fn trunc(self) -> I { self.0 / (I::one() << N) } #[must_use] pub fn rem_euclid(self, rhs: Self) -> Self { let r = self % rhs; if r < I::zero().into() { if rhs < I::zero().into() { r - rhs } else { r + rhs } } else { r } } pub fn floor(self) -> I { self.0 >> N } pub fn frac(self) -> I { self.0 & ((I::one() << N) - I::one()) } pub fn new(integral: I) -> Self { Self(integral << N) } pub fn new_from_parts(num: (i32, i32)) -> Self { Self(I::from_as_i32(((num.0) << N) + (num.1 >> (30 - N)))) } } impl Num { #[must_use] pub fn sqrt(self) -> Self { assert_eq!(N % 2, 0, "N must be even to be able to square root"); assert!(self.0 >= 0, "sqrt is only valid for positive numbers"); let mut d = 1 << 30; let mut x = self.0; let mut c = 0; while d > self.0 { d >>= 2; } while d != 0 { if x >= c + d { x -= c + d; c = (c >> 1) + d; } else { c >>= 1; } d >>= 2; } Self(c << (N / 2)) } } impl Num { #[must_use] pub fn abs(self) -> Self { Num(self.0.fixed_abs()) } /// domain of [0, 1]. /// see https://github.com/tarcieri/micromath/blob/24584465b48ff4e87cffb709c7848664db896b4f/src/float/cos.rs#L226 #[must_use] pub fn cos(self) -> Self { let one: Self = I::one().into(); let mut x = self; let four: I = 4.into(); let two: I = 2.into(); let sixteen: I = 16.into(); let nine: I = 9.into(); let forty: I = 40.into(); x -= one / four + (x + one / four).floor(); x *= (x.abs() - one / two) * sixteen; x += x * (x.abs() - one) * (nine / forty); x } #[must_use] pub fn sin(self) -> Self { let one: Self = I::one().into(); let four: I = 4.into(); (self + one / four).cos() } } impl Display for Num { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { let mut integral = self.0 >> N; let mask: I = (I::one() << N) - I::one(); let mut fractional = self.0 & mask; // Negative fix nums are awkward to print if they have non zero fractional part. // This is because you can think of them as `number + non negative fraction`. // // But if you think of a negative number, you'd like it to be `negative number - non negative fraction` // So we have to add 1 to the integral bit, and take 1 - fractional bit if fractional != I::zero() && integral < I::zero() { integral = integral + I::one(); fractional = (I::one() << N) - fractional; } write!(f, "{}", integral)?; if fractional != I::zero() { write!(f, ".")?; } while fractional & mask != I::zero() { fractional = fractional * I::ten(); write!(f, "{}", (fractional & !mask) >> N)?; fractional = fractional & mask; } Ok(()) } } impl Debug for Num { fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { use core::any::type_name; write!(f, "Num<{}, {}>({})", type_name::(), N, self) } } #[derive(Clone, Copy, PartialEq, Eq, Debug)] pub struct Vector2D { pub x: T, pub y: T, } impl Add> for Vector2D { type Output = Vector2D; fn add(self, rhs: Vector2D) -> Self::Output { Vector2D { x: self.x + rhs.x, y: self.y + rhs.y, } } } impl Mul for Vector2D where T: Mul, { type Output = Vector2D; fn mul(self, rhs: U) -> Self::Output { Vector2D { x: self.x * rhs, y: self.y * rhs, } } } impl MulAssign for Vector2D where T: Mul, { fn mul_assign(&mut self, rhs: U) { let result = *self * rhs; self.x = result.x; self.y = result.y; } } impl Div for Vector2D where T: Div, { type Output = Vector2D; fn div(self, rhs: U) -> Self::Output { Vector2D { x: self.x / rhs, y: self.y / rhs, } } } impl DivAssign for Vector2D where T: Div, { fn div_assign(&mut self, rhs: U) { let result = *self / rhs; self.x = result.x; self.y = result.y; } } impl AddAssign for Vector2D { fn add_assign(&mut self, rhs: Self) { *self = *self + rhs; } } impl Sub> for Vector2D { type Output = Vector2D; fn sub(self, rhs: Vector2D) -> Self::Output { Vector2D { x: self.x - rhs.x, y: self.y - rhs.y, } } } impl SubAssign for Vector2D { fn sub_assign(&mut self, rhs: Self) { *self = *self - rhs; } } impl Vector2D> { #[must_use] pub fn trunc(self) -> Vector2D { Vector2D { x: self.x.trunc(), y: self.y.trunc(), } } #[must_use] pub fn floor(self) -> Vector2D { Vector2D { x: self.x.floor(), y: self.y.floor(), } } } impl Vector2D> { #[must_use] pub fn magnitude_squared(self) -> Num { self.x * self.x + self.y * self.y } #[must_use] pub fn manhattan_distance(self) -> Num { self.x.abs() + self.y.abs() } #[must_use] pub fn magnitude(self) -> Num { self.magnitude_squared().sqrt() } // calculates the magnitude of a vector using the alpha max plus beta min // algorithm https://en.wikipedia.org/wiki/Alpha_max_plus_beta_min_algorithm // this has a maximum error of less than 4% of the true magnitude, probably // depending on the size of your fixed point approximation #[must_use] pub fn fast_magnitude(self) -> Num { let max = core::cmp::max(self.x, self.y); let min = core::cmp::min(self.x, self.y); max * num!(0.960433870103) + min * num!(0.397824734759) } #[must_use] pub fn normalise(self) -> Self { self / self.magnitude() } #[must_use] pub fn fast_normalise(self) -> Self { self / self.fast_magnitude() } } impl> From<(P, P)> for Vector2D { fn from(f: (P, P)) -> Self { Vector2D::new(f.0.into(), f.1.into()) } } impl Vector2D { pub fn change_base>(self) -> Vector2D { (self.x, self.y).into() } } impl Vector2D> { pub fn new_from_angle(angle: Num) -> Self { Vector2D { x: angle.cos(), y: angle.sin(), } } } impl From> for Vector2D> { fn from(n: Vector2D) -> Self { Vector2D { x: n.x.into(), y: n.y.into(), } } } #[derive(Debug, PartialEq, Eq, Clone)] pub struct Rect { pub position: Vector2D, pub size: Vector2D, } impl Rect { #[must_use] pub fn new(position: Vector2D, size: Vector2D) -> Self { Rect { position, size } } pub fn contains_point(&self, point: Vector2D) -> bool { point.x > self.position.x && point.x < self.position.x + self.size.x && point.y > self.position.y && point.y < self.position.y + self.size.y } pub fn touches(&self, other: Rect) -> bool { self.position.x < other.position.x + other.size.x && self.position.x + self.size.x > other.position.x && self.position.y < other.position.y + other.size.y && self.position.y + self.size.y > other.position.y } #[must_use] pub fn overlapping_rect(&self, other: Rect) -> Self { fn max(x: E, y: E) -> E { if x > y { x } else { y } } fn min(x: E, y: E) -> E { if x > y { y } else { x } } let top_left: Vector2D = ( max(self.position.x, other.position.x), max(self.position.y, other.position.y), ) .into(); let bottom_right: Vector2D = ( min( self.position.x + self.size.x, other.position.x + other.size.x, ), min( self.position.y + self.size.y, other.position.y + other.size.y, ), ) .into(); Rect::new(top_left, bottom_right - top_left) } } impl Rect { pub fn iter(self) -> impl Iterator { let mut x = self.position.x; let mut y = self.position.y; core::iter::from_fn(move || { if x >= self.position.x + self.size.x { x = self.position.x; y = y + T::one(); if y >= self.position.y + self.size.y { return None; } } let ret_x = x; x = x + T::one(); Some((ret_x, y)) }) } } impl Vector2D { pub fn new(x: T, y: T) -> Self { Vector2D { x, y } } pub fn get(self) -> (T, T) { (self.x, self.y) } #[must_use] pub fn hadamard(self, other: Self) -> Self { Self { x: self.x * other.x, y: self.y * other.y, } } #[must_use] pub fn swap(self) -> Self { Self { x: self.y, y: self.x, } } } #[cfg(test)] mod tests { extern crate alloc; use super::*; use alloc::format; #[test] fn formats_whole_numbers_correctly() { let a = Num::::new(-4i32); assert_eq!(format!("{}", a), "-4"); } #[test] fn formats_fractions_correctly() { let a = Num::::new(5); let two = Num::::new(4); let minus_one = Num::::new(-1); let b: Num = a / two; let c: Num = b * minus_one; assert_eq!(b + c, 0.into()); assert_eq!(format!("{}", b), "1.25"); assert_eq!(format!("{}", c), "-1.25"); } #[test] fn sqrt() { for x in 1..1024 { let n: Num = Num::new(x * x); assert_eq!(n.sqrt(), x.into()); } } #[test] fn test_macro_conversion() { fn test_positive() { let a: Num = num!(1.5); let one = A::one() << B; let b = Num::from_raw(one + (one >> 1)); assert_eq!(a, b); } fn test_negative() { let a: Num = num!(-1.5); let one = A::one() << B; let b = Num::from_raw(one + (one >> 1)); assert_eq!(a, -b); } fn test_base() { test_positive::(); test_positive::(); test_negative::(); if B < 16 { test_positive::(); test_positive::(); test_negative::(); } } // some nice powers of two test_base::<8>(); test_base::<4>(); test_base::<16>(); // not a power of two test_base::<10>(); // an odd number test_base::<9>(); // and a prime test_base::<11>(); } #[test] fn test_numbers() { // test addition let n: Num = 1.into(); assert_eq!(n + 2, 3.into(), "testing that 1 + 2 == 3"); // test multiplication let n: Num = 5.into(); assert_eq!(n * 3, 15.into(), "testing that 5 * 3 == 15"); // test division let n: Num = 30.into(); let p: Num = 3.into(); assert_eq!(n / 20, p / 2, "testing that 30 / 20 == 3 / 2"); assert_ne!(n, p, "testing that 30 != 3"); } #[test] fn test_division_by_one() { let one: Num = 1.into(); for i in -40..40 { let n: Num = i.into(); assert_eq!(n / one, n); } } #[test] fn test_division_and_multiplication_by_16() { let sixteen: Num = 16.into(); for i in -40..40 { let n: Num = i.into(); let m = n / sixteen; assert_eq!(m * sixteen, n); } } #[test] fn test_division_by_2_and_15() { let two: Num = 2.into(); let fifteen: Num = 15.into(); let thirty: Num = 30.into(); for i in -128..128 { let n: Num = i.into(); assert_eq!(n / two / fifteen, n / thirty); assert_eq!(n / fifteen / two, n / thirty); } } #[test] fn test_change_base() { let two: Num = 2.into(); let three: Num = 3.into(); assert_eq!(two + three.change_base(), 5.into()); assert_eq!(three + two.change_base(), 5.into()); } #[test] fn test_rem_returns_sensible_values_for_integers() { for i in -50..50 { for j in -50..50 { if j == 0 { continue; } let i_rem_j_normally = i % j; let i_fixnum: Num = i.into(); assert_eq!(i_fixnum % j, i_rem_j_normally.into()); } } } #[test] fn test_rem_returns_sensible_values_for_non_integers() { let one: Num = 1.into(); let third = one / 3; for i in -50..50 { for j in -50..50 { if j == 0 { continue; } // full calculation in the normal way let x: Num = third + i; let y: Num = j.into(); let truncated_division: Num = (x / y).trunc().into(); let remainder = x - truncated_division * y; assert_eq!(x % y, remainder); } } } #[test] fn test_rem_euclid_is_always_positive_and_sensible() { let one: Num = 1.into(); let third = one / 3; for i in -50..50 { for j in -50..50 { if j == 0 { continue; } let x: Num = third + i; let y: Num = j.into(); let rem_euclid = x.rem_euclid(y); assert!(rem_euclid > 0.into()); } } } #[test] fn test_vector_multiplication_and_division() { let a: Vector2D = (1, 2).into(); let b = a * 5; let c = b / 5; assert_eq!(b, (5, 10).into()); assert_eq!(a, c); } #[test] fn magnitude_accuracy() { let n: Vector2D> = (3, 4).into(); assert!((n.magnitude() - 5).abs() < num!(0.1)); let n: Vector2D> = (3, 4).into(); assert!((n.magnitude() - 5).abs() < num!(0.1)); } #[test] fn test_vector_changing() { let v1: Vector2D> = Vector2D::new(1.into(), 2.into()); let v2 = v1.trunc(); assert_eq!(v2.get(), (1, 2)); assert_eq!(v1 + v1, (v2 + v2).into()); } #[test] fn test_rect_iter() { let rect: Rect = Rect::new((5_i32, 5_i32).into(), (3_i32, 3_i32).into()); assert_eq!( rect.iter().collect::>(), &[ (5, 5), (6, 5), (7, 5), (5, 6), (6, 6), (7, 6), (5, 7), (6, 7), (7, 7), ] ); } }