use num_bigint::BigUint;
use num_rational::BigRational;
use num_traits::{Signed, ToPrimitive, Zero};
use crate::linalg::{Vec2, Vec3};
#[inline]
pub fn rat(v: f64) -> BigRational {
BigRational::from_float(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: &BigRational) -> f64 {
if r.is_zero() {
return 0.0;
}
let neg = r.is_negative();
let n: &BigUint = r.numer().magnitude();
let d: &BigUint = r.denom().magnitude();
let mut e = n.bits() as i64 - d.bits() 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 };
}
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 mut m = q;
let twice_rem = &rem << 1usize;
match twice_rem.cmp(&den) {
std::cmp::Ordering::Greater => m += 1u32,
std::cmp::Ordering::Equal => {
if m.bit(0) {
m += 1u32;
}
}
std::cmp::Ordering::Less => {}
}
if m.is_zero() {
return if neg { -0.0 } else { 0.0 };
}
if m.bits() as i64 - 1 + lsb > 1023 {
return if neg { f64::NEG_INFINITY } else { f64::INFINITY };
}
let val = m.to_u64().expect("mantissa fits in u64") as f64 * pow2(lsb);
if neg {
-val
} else {
val
}
}
#[derive(Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct R2 {
pub x: BigRational,
pub y: BigRational,
}
impl R2 {
#[inline]
pub fn new(x: BigRational, y: BigRational) -> 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: &BigRational) -> R2 {
R2::new(&self.x * s, &self.y * s)
}
pub fn dot(&self, o: &R2) -> BigRational {
&self.x * &o.x + &self.y * &o.y
}
pub fn cross(&self, o: &R2) -> BigRational {
&self.x * &o.y - &self.y * &o.x
}
pub fn is_zero(&self) -> bool {
self.x.is_zero() && self.y.is_zero()
}
}
#[derive(Clone, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)]
pub struct R3 {
pub x: BigRational,
pub y: BigRational,
pub z: BigRational,
}
impl R3 {
#[inline]
pub fn new(x: BigRational, y: BigRational, z: BigRational) -> 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: &BigRational) -> R3 {
R3::new(&self.x * s, &self.y * s, &self.z * s)
}
pub fn dot(&self, o: &R3) -> BigRational {
&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 {
self.x.is_zero() && self.y.is_zero() && self.z.is_zero()
}
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"),
}
}
}
#[inline]
fn rat_fields_eq(a: &BigRational, b: &BigRational) -> bool {
a.numer() == b.numer() && a.denom() == b.denom()
}
fn hash_rat<H: std::hash::Hasher>(r: &BigRational, state: &mut H) {
use std::hash::Hash;
(r.numer().sign() == num_bigint::Sign::Minus).hash(state);
for d in r.numer().iter_u64_digits() {
d.hash(state);
}
0xfeed_u64.hash(state); for d in r.denom().iter_u64_digits() {
d.hash(state);
}
}
#[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 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)
}