use super::backend::{
self, denom, int_mag, numer_mag, rat_from_f64, rat_is_negative, rat_is_zero, Int, Rational,
Signed, ToPrimitive, Uint,
};
use crate::linalg::{Vec2, Vec3};
#[inline]
pub fn rat(v: f64) -> Rational {
rat_from_f64(v).expect("robust engine: coordinate must be finite")
}
#[inline]
fn pow2(e: i64) -> f64 {
debug_assert!((-1074..=1023).contains(&e), "pow2 exponent out of range: {e}");
if e >= -1022 {
f64::from_bits(((e + 1023) as u64) << 52)
} else {
f64::from_bits(1u64 << (e + 1074))
}
}
pub fn rat_to_f64(r: &Rational) -> f64 {
if rat_is_zero(r) {
return 0.0;
}
let n_mag = numer_mag(r);
mag_ratio_to_f64(n_mag.as_ref(), denom(r), rat_is_negative(r)).0
}
pub fn int_ratio_to_f64(numer: &Int, denom: &Int) -> (f64, bool) {
debug_assert!(!denom.is_zero(), "int_ratio_to_f64: zero denominator");
if numer.is_zero() {
return (0.0, true);
}
let neg = numer.is_negative() != denom.is_negative();
mag_ratio_to_f64(&int_mag(numer), &int_mag(denom), neg)
}
fn mag_ratio_to_f64(n: &Uint, d: &Uint, neg: bool) -> (f64, bool) {
let mut e = backend::uint_bits(n) as i64 - backend::uint_bits(d) as i64;
let ge = if e >= 0 {
*n >= (d << e as usize)
} else {
(n << (-e) as usize) >= *d
};
if !ge {
e -= 1;
}
if e > 1023 {
return (if neg { f64::NEG_INFINITY } else { f64::INFINITY }, false);
}
let lsb = (e - 52).max(-1074);
let (num, den) = if lsb >= 0 {
(n.clone(), d << lsb as usize)
} else {
(n << (-lsb) as usize, d.clone())
};
let q = &num / &den;
let rem = &num - &q * &den;
let exact = rem.is_zero();
let mut m = q;
let twice_rem = &rem << 1usize;
match twice_rem.cmp(&den) {
std::cmp::Ordering::Greater => m += 1u32,
std::cmp::Ordering::Equal => {
if backend::uint_bit(&m, 0) {
m += 1u32;
}
}
std::cmp::Ordering::Less => {}
}
if m.is_zero() {
return (if neg { -0.0 } else { 0.0 }, false);
}
if backend::uint_bits(&m) as i64 - 1 + lsb > 1023 {
return (if neg { f64::NEG_INFINITY } else { f64::INFINITY }, false);
}
let val = m.to_u64().expect("mantissa fits in u64") as f64 * pow2(lsb);
(if neg { -val } else { val }, exact)
}
#[derive(Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct R2 {
pub x: Rational,
pub y: Rational,
}
impl R2 {
#[inline]
pub fn new(x: Rational, y: Rational) -> Self {
Self { x, y }
}
#[inline]
pub fn from_vec2(v: Vec2) -> Self {
Self::new(rat(v.x), rat(v.y))
}
pub fn to_vec2_rounded(&self) -> Vec2 {
Vec2::new(rat_to_f64(&self.x), rat_to_f64(&self.y))
}
pub fn sub(&self, o: &R2) -> R2 {
R2::new(&self.x - &o.x, &self.y - &o.y)
}
pub fn add(&self, o: &R2) -> R2 {
R2::new(&self.x + &o.x, &self.y + &o.y)
}
pub fn scale(&self, s: &Rational) -> R2 {
R2::new(&self.x * s, &self.y * s)
}
pub fn dot(&self, o: &R2) -> Rational {
&self.x * &o.x + &self.y * &o.y
}
pub fn cross(&self, o: &R2) -> Rational {
&self.x * &o.y - &self.y * &o.x
}
pub fn is_zero(&self) -> bool {
rat_is_zero(&self.x) && rat_is_zero(&self.y)
}
}
#[derive(Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct R3 {
pub x: Rational,
pub y: Rational,
pub z: Rational,
}
impl R3 {
#[inline]
pub fn new(x: Rational, y: Rational, z: Rational) -> Self {
Self { x, y, z }
}
#[inline]
pub fn from_vec3(v: Vec3) -> Self {
Self::new(rat(v.x), rat(v.y), rat(v.z))
}
pub fn to_vec3_rounded(&self) -> Vec3 {
Vec3::new(rat_to_f64(&self.x), rat_to_f64(&self.y), rat_to_f64(&self.z))
}
pub fn sub(&self, o: &R3) -> R3 {
R3::new(&self.x - &o.x, &self.y - &o.y, &self.z - &o.z)
}
pub fn add(&self, o: &R3) -> R3 {
R3::new(&self.x + &o.x, &self.y + &o.y, &self.z + &o.z)
}
pub fn scale(&self, s: &Rational) -> R3 {
R3::new(&self.x * s, &self.y * s, &self.z * s)
}
pub fn dot(&self, o: &R3) -> Rational {
&self.x * &o.x + &self.y * &o.y + &self.z * &o.z
}
pub fn cross(&self, o: &R3) -> R3 {
R3::new(
&self.y * &o.z - &self.z * &o.y,
&self.z * &o.x - &self.x * &o.z,
&self.x * &o.y - &self.y * &o.x,
)
}
pub fn is_zero(&self) -> bool {
rat_is_zero(&self.x) && rat_is_zero(&self.y) && rat_is_zero(&self.z)
}
pub fn project_drop(&self, axis: usize) -> R2 {
match axis {
0 => R2::new(self.y.clone(), self.z.clone()),
1 => R2::new(self.z.clone(), self.x.clone()),
2 => R2::new(self.x.clone(), self.y.clone()),
_ => unreachable!("axis must be 0, 1, or 2"),
}
}
}
use backend::{hash_rational as hash_rat, rat_eq as rat_fields_eq};
#[derive(Clone, Debug)]
pub struct R2Key(pub R2);
impl PartialEq for R2Key {
#[inline]
fn eq(&self, other: &Self) -> bool {
rat_fields_eq(&self.0.x, &other.0.x) && rat_fields_eq(&self.0.y, &other.0.y)
}
}
impl Eq for R2Key {}
impl std::hash::Hash for R2Key {
fn hash<H: std::hash::Hasher>(&self, state: &mut H) {
hash_rat(&self.0.x, state);
hash_rat(&self.0.y, state);
}
}
#[derive(Clone, Debug)]
pub struct R3Key(pub R3);
impl PartialEq for R3Key {
#[inline]
fn eq(&self, other: &Self) -> bool {
rat_fields_eq(&self.0.x, &other.0.x)
&& rat_fields_eq(&self.0.y, &other.0.y)
&& rat_fields_eq(&self.0.z, &other.0.z)
}
}
impl Eq for R3Key {}
impl std::hash::Hash for R3Key {
fn hash<H: std::hash::Hasher>(&self, state: &mut H) {
hash_rat(&self.0.x, state);
hash_rat(&self.0.y, state);
hash_rat(&self.0.z, state);
}
}
#[inline]
pub fn r2_eq(a: &R2, b: &R2) -> bool {
rat_fields_eq(&a.x, &b.x) && rat_fields_eq(&a.y, &b.y)
}
#[inline]
pub fn r3_eq(a: &R3, b: &R3) -> bool {
rat_fields_eq(&a.x, &b.x) && rat_fields_eq(&a.y, &b.y) && rat_fields_eq(&a.z, &b.z)
}