use super::{DropAxis, ImplicitPoint, Sign};
macro_rules! fixed_impl {
($T:ty, $scale:expr) => {
use super::super::{assemble_sign, DropAxis, ImplicitPoint, Lpi, Sign, Tpi};
use num_traits::{CheckedAdd, CheckedMul, CheckedSub, FromPrimitive, One, Signed};
type I = $T;
type V3 = [I; 3];
#[inline]
fn gi(x: f64) -> Option<I> {
let scaled = x * $scale;
if !scaled.is_finite() || scaled.fract() != 0.0 || scaled.abs() >= 9.0e18 {
return None;
}
I::from_i64(scaled as i64)
}
#[inline]
fn vec(p: [f64; 3]) -> Option<V3> {
Some([gi(p[0])?, gi(p[1])?, gi(p[2])?])
}
#[inline]
fn mul(a: I, b: I) -> Option<I> {
CheckedMul::checked_mul(&a, &b)
}
#[inline]
fn sub(a: I, b: I) -> Option<I> {
CheckedSub::checked_sub(&a, &b)
}
#[inline]
fn add(a: I, b: I) -> Option<I> {
CheckedAdd::checked_add(&a, &b)
}
fn sub3(a: &V3, b: &V3) -> Option<V3> {
Some([sub(a[0], b[0])?, sub(a[1], b[1])?, sub(a[2], b[2])?])
}
fn cross(u: &V3, v: &V3) -> Option<V3> {
Some([
sub(mul(u[1], v[2])?, mul(u[2], v[1])?)?,
sub(mul(u[2], v[0])?, mul(u[0], v[2])?)?,
sub(mul(u[0], v[1])?, mul(u[1], v[0])?)?,
])
}
fn det3(u: &V3, v: &V3, w: &V3) -> Option<I> {
let m0 = sub(mul(v[1], w[2])?, mul(v[2], w[1])?)?;
let m1 = sub(mul(v[2], w[0])?, mul(v[0], w[2])?)?;
let m2 = sub(mul(v[0], w[1])?, mul(v[1], w[0])?)?;
add(add(mul(u[0], m0)?, mul(u[1], m1)?)?, mul(u[2], m2)?)
}
#[inline]
fn sign_of(x: &I) -> Sign {
if x.is_negative() {
Sign::Negative
} else if x.is_zero() {
Sign::Zero
} else {
Sign::Positive
}
}
#[inline]
fn axis_idx(axis: DropAxis) -> (usize, usize) {
match axis {
DropAxis::X => (1, 2),
DropAxis::Y => (0, 2),
DropAxis::Z => (0, 1),
}
}
fn lpi_lambda(l: &Lpi) -> Option<(V3, I)> {
let p = vec(l.p)?;
let q = vec(l.q)?;
let rr = vec(l.r)?;
let s = vec(l.s)?;
let t = vec(l.t)?;
let qp = sub3(&q, &p)?;
let sr = sub3(&s, &rr)?;
let tr = sub3(&t, &rr)?;
let pr = sub3(&p, &rr)?;
let d = det3(&qp, &sr, &tr)?;
let n = det3(&pr, &sr, &tr)?;
let lx = sub(mul(d, p[0])?, mul(n, qp[0])?)?;
let ly = sub(mul(d, p[1])?, mul(n, qp[1])?)?;
let lz = sub(mul(d, p[2])?, mul(n, qp[2])?)?;
Some(([lx, ly, lz], d))
}
fn tpi_lambda(t: &Tpi) -> Option<(V3, I)> {
let plane = |pl: &[[f64; 3]; 3]| -> Option<(V3, I)> {
let a = vec(pl[0])?;
let ba = sub3(&vec(pl[1])?, &a)?;
let ca = sub3(&vec(pl[2])?, &a)?;
let n = cross(&ba, &ca)?;
let off = add(add(mul(n[0], a[0])?, mul(n[1], a[1])?)?, mul(n[2], a[2])?)?;
Some((n, off))
};
let (n1, c1) = plane(&t.planes[0])?;
let (n2, c2) = plane(&t.planes[1])?;
let (n3, c3) = plane(&t.planes[2])?;
let d = det3(&n1, &n2, &n3)?;
let ns = [n1, n2, n3];
let cs = [c1, c2, c3];
let cramer = |k: usize| -> Option<I> {
let mut rows = [ns[0], ns[1], ns[2]];
for (row, &ci) in rows.iter_mut().zip(cs.iter()) {
row[k] = ci;
}
det3(&rows[0], &rows[1], &rows[2])
};
Some(([cramer(0)?, cramer(1)?, cramer(2)?], d))
}
pub fn lambda_of(p: &ImplicitPoint) -> Option<(V3, I)> {
match p {
ImplicitPoint::Lpi(l) => lpi_lambda(l),
ImplicitPoint::Tpi(t) => tpi_lambda(t),
ImplicitPoint::Explicit(e) => Some((vec(*e)?, I::one())),
}
}
pub fn orient2d_2i(a: &ImplicitPoint, b: &ImplicitPoint, c: [f64; 3], axis: DropAxis) -> Option<Sign> {
let (i, j) = axis_idx(axis);
let (lam1, d1) = lambda_of(a)?;
let (lam2, d2) = lambda_of(b)?;
let cr = vec(c)?;
let a_i = sub(lam1[i], mul(d1, cr[i])?)?;
let a_j = sub(lam1[j], mul(d1, cr[j])?)?;
let b_i = sub(lam2[i], mul(d2, cr[i])?)?;
let b_j = sub(lam2[j], mul(d2, cr[j])?)?;
let det = sub(mul(a_i, b_j)?, mul(a_j, b_i)?)?;
Some(assemble_sign(sign_of(&det), &[sign_of(&d1), sign_of(&d2)]))
}
pub fn orient2d_3i(a: &ImplicitPoint, b: &ImplicitPoint, c: &ImplicitPoint, axis: DropAxis) -> Option<Sign> {
let (i, j) = axis_idx(axis);
let (lam1, d1) = lambda_of(a)?;
let (lam2, d2) = lambda_of(b)?;
let (lam3, d3) = lambda_of(c)?;
let u_i = sub(mul(d1, lam2[i])?, mul(d2, lam1[i])?)?;
let u_j = sub(mul(d1, lam2[j])?, mul(d2, lam1[j])?)?;
let v_i = sub(mul(d1, lam3[i])?, mul(d3, lam1[i])?)?;
let v_j = sub(mul(d1, lam3[j])?, mul(d3, lam1[j])?)?;
let det = sub(mul(u_i, v_j)?, mul(u_j, v_i)?)?;
Some(assemble_sign(sign_of(&det), &[sign_of(&d2), sign_of(&d3)]))
}
pub fn indirect_orient2d(p: &ImplicitPoint, b: [f64; 3], c: [f64; 3], axis: DropAxis) -> Option<Sign> {
let (i, j) = axis_idx(axis);
let (lambda, d) = lambda_of(p)?;
let br = vec(b)?;
let cr = vec(c)?;
let li = sub(lambda[i], mul(d, cr[i])?)?;
let lj = sub(lambda[j], mul(d, cr[j])?)?;
let det = sub(mul(li, sub(br[j], cr[j])?)?, mul(lj, sub(br[i], cr[i])?)?)?;
Some(assemble_sign(sign_of(&det), &[sign_of(&d)]))
}
fn cmp_axis(a: &ImplicitPoint, b: &ImplicitPoint, k: usize) -> Option<Sign> {
use ImplicitPoint::Explicit;
match (a, b) {
(Explicit(ae), Explicit(be)) => Some(sign_of(&sub(gi(ae[k])?, gi(be[k])?)?)),
(_, Explicit(be)) => {
let (lam, d) = lambda_of(a)?;
let bk = gi(be[k])?;
Some(assemble_sign(sign_of(&sub(lam[k], mul(d, bk)?)?), &[sign_of(&d)]))
}
(Explicit(ae), _) => {
let (lam, d) = lambda_of(b)?;
let ak = gi(ae[k])?;
Some(assemble_sign(sign_of(&sub(mul(ak, d)?, lam[k])?), &[sign_of(&d)]))
}
(_, _) => {
let (la, da) = lambda_of(a)?;
let (lb, db) = lambda_of(b)?;
Some(assemble_sign(
sign_of(&sub(mul(la[k], db)?, mul(lb[k], da)?)?),
&[sign_of(&da), sign_of(&db)],
))
}
}
}
pub fn cmp_lex(a: &ImplicitPoint, b: &ImplicitPoint) -> Option<Sign> {
for k in 0..3 {
let s = cmp_axis(a, b, k)?;
if s != Sign::Zero {
return Some(s);
}
}
Some(Sign::Zero)
}
pub fn cmp_along(a: &ImplicitPoint, b: &ImplicitPoint, u: [f64; 3]) -> Option<Sign> {
let (la, da) = lambda_of(a)?;
let (lb, db) = lambda_of(b)?;
let ur = vec(u)?;
let dot_a = add(add(mul(la[0], ur[0])?, mul(la[1], ur[1])?)?, mul(la[2], ur[2])?)?;
let dot_b = add(add(mul(lb[0], ur[0])?, mul(lb[1], ur[1])?)?, mul(lb[2], ur[2])?)?;
let num = sub(mul(dot_a, db)?, mul(dot_b, da)?)?;
Some(assemble_sign(sign_of(&num), &[sign_of(&da), sign_of(&db)]))
}
pub fn indirect_orient3d(p: &ImplicitPoint, p2: [f64; 3], p3: [f64; 3], p4: [f64; 3]) -> Option<Sign> {
let (lambda, d) = lambda_of(p)?;
let p4r = vec(p4)?;
let row1 = [
sub(lambda[0], mul(d, p4r[0])?)?,
sub(lambda[1], mul(d, p4r[1])?)?,
sub(lambda[2], mul(d, p4r[2])?)?,
];
let row2 = sub3(&vec(p2)?, &p4r)?;
let row3 = sub3(&vec(p3)?, &p4r)?;
Some(assemble_sign(sign_of(&det3(&row1, &row2, &row3)?), &[sign_of(&d)]))
}
};
}
const COARSE: f64 = 65_536.0;
const FINE: f64 = 68_719_476_736.0;
const FINE_OVER_COARSE: i64 = 1 << 20;
mod w256 {
fixed_impl!(bnum::types::I256, super::COARSE);
}
mod w512 {
fixed_impl!(bnum::types::I512, super::COARSE);
}
mod w1024 {
fixed_impl!(bnum::types::I1024, super::COARSE);
}
mod f256 {
fixed_impl!(bnum::types::I256, super::FINE);
}
mod f512 {
fixed_impl!(bnum::types::I512, super::FINE);
}
mod f1024 {
fixed_impl!(bnum::types::I1024, super::FINE);
}
mod f2048 {
fixed_impl!(bnum::types::I2048, super::FINE);
}
macro_rules! cascade {
($name:ident ( $($arg:ident : $ty:ty),* )) => {
pub fn $name($($arg : $ty),*) -> Option<Sign> {
w256::$name($($arg),*)
.or_else(|| w512::$name($($arg),*))
.or_else(|| w1024::$name($($arg),*))
.or_else(|| f256::$name($($arg),*))
.or_else(|| f512::$name($($arg),*))
.or_else(|| f1024::$name($($arg),*))
.or_else(|| f2048::$name($($arg),*))
}
};
}
cascade!(orient2d_2i(a: &ImplicitPoint, b: &ImplicitPoint, c: [f64; 3], axis: DropAxis));
cascade!(orient2d_3i(a: &ImplicitPoint, b: &ImplicitPoint, c: &ImplicitPoint, axis: DropAxis));
cascade!(indirect_orient2d(p: &ImplicitPoint, b: [f64; 3], c: [f64; 3], axis: DropAxis));
cascade!(cmp_lex(a: &ImplicitPoint, b: &ImplicitPoint));
cascade!(cmp_along(a: &ImplicitPoint, b: &ImplicitPoint, u: [f64; 3]));
cascade!(indirect_orient3d(p: &ImplicitPoint, p2: [f64; 3], p3: [f64; 3], p4: [f64; 3]));
type Big = bnum::types::I512;
pub type Lam = ([Big; 3], Big);
#[inline]
fn bmul(a: Big, b: Big) -> Option<Big> {
num_traits::CheckedMul::checked_mul(&a, &b)
}
#[inline]
fn bsub(a: Big, b: Big) -> Option<Big> {
num_traits::CheckedSub::checked_sub(&a, &b)
}
#[inline]
fn bsign(x: &Big) -> Sign {
use num_traits::{Signed, Zero};
if x.is_negative() {
Sign::Negative
} else if x.is_zero() {
Sign::Zero
} else {
Sign::Positive
}
}
pub fn lambda1024(p: &ImplicitPoint) -> Option<Lam> {
use num_traits::{CheckedMul, FromPrimitive, One};
let (mut lam, mut d) = w512::lambda_of(p).or_else(|| {
let (lam, d) = f512::lambda_of(p)?;
let ratio = Big::from_i64(FINE_OVER_COARSE)?;
Some((lam, CheckedMul::checked_mul(&d, &ratio)?))
})?;
if d.is_negative() {
d = -d;
lam = [-lam[0], -lam[1], -lam[2]];
}
if d.is_zero() {
return None;
}
if !d.is_one()
&& (lam[0] % d).is_zero()
&& (lam[1] % d).is_zero()
&& (lam[2] % d).is_zero()
{
lam = [lam[0] / d, lam[1] / d, lam[2] / d];
d = One::one();
}
Some((lam, d))
}
#[inline]
fn axis_ij(axis: DropAxis) -> (usize, usize) {
match axis {
DropAxis::X => (1, 2),
DropAxis::Y => (0, 2),
DropAxis::Z => (0, 1),
}
}
pub fn orient2d_from_lam(a: &Lam, b: &Lam, c: &Lam, axis: DropAxis) -> Option<Sign> {
use num_traits::{One, ToPrimitive};
let (i, j) = axis_ij(axis);
let (lam1, d1) = a;
let (lam2, d2) = b;
let (lam3, d3) = c;
if d1.is_one() && d2.is_one() && d3.is_one() {
if let (Some(ai), Some(aj), Some(bi), Some(bj), Some(ci), Some(cj)) = (
lam1[i].to_i64(),
lam1[j].to_i64(),
lam2[i].to_i64(),
lam2[j].to_i64(),
lam3[i].to_i64(),
lam3[j].to_i64(),
) {
let (ai, aj) = (ai as i128, aj as i128);
let u_i = bi as i128 - ai;
let u_j = bj as i128 - aj;
let v_i = ci as i128 - ai;
let v_j = cj as i128 - aj;
if let (Some(p1), Some(p2)) = (u_i.checked_mul(v_j), u_j.checked_mul(v_i)) {
if let Some(det) = p1.checked_sub(p2) {
return Some(match det.cmp(&0) {
std::cmp::Ordering::Less => Sign::Negative,
std::cmp::Ordering::Greater => Sign::Positive,
std::cmp::Ordering::Equal => Sign::Zero,
});
}
}
}
}
let u_i = bsub(bmul(*d1, lam2[i])?, bmul(*d2, lam1[i])?)?;
let u_j = bsub(bmul(*d1, lam2[j])?, bmul(*d2, lam1[j])?)?;
let v_i = bsub(bmul(*d1, lam3[i])?, bmul(*d3, lam1[i])?)?;
let v_j = bsub(bmul(*d1, lam3[j])?, bmul(*d3, lam1[j])?)?;
let det = bsub(bmul(u_i, v_j)?, bmul(u_j, v_i)?)?;
Some(super::assemble_sign(bsign(&det), &[bsign(d2), bsign(d3)]))
}
pub fn cmp_lex_from_lam(a: &Lam, b: &Lam) -> Option<Sign> {
use num_traits::{One, ToPrimitive};
let (la, da) = a;
let (lb, db) = b;
if da.is_one() && db.is_one() {
if let (Some(a0), Some(a1), Some(a2), Some(b0), Some(b1), Some(b2)) = (
la[0].to_i64(),
la[1].to_i64(),
la[2].to_i64(),
lb[0].to_i64(),
lb[1].to_i64(),
lb[2].to_i64(),
) {
for (x, y) in [(a0, b0), (a1, b1), (a2, b2)] {
match x.cmp(&y) {
std::cmp::Ordering::Less => return Some(Sign::Negative),
std::cmp::Ordering::Greater => return Some(Sign::Positive),
std::cmp::Ordering::Equal => {}
}
}
return Some(Sign::Zero);
}
}
for k in 0..3 {
let s = bsub(bmul(la[k], *db)?, bmul(lb[k], *da)?)?;
let sg = super::assemble_sign(bsign(&s), &[bsign(da), bsign(db)]);
if sg != Sign::Zero {
return Some(sg);
}
}
Some(Sign::Zero)
}
pub fn point_to_f64(p: &ImplicitPoint) -> Option<[f64; 3]> {
use num_traits::ToPrimitive;
let (lambda, d, scale) = w1024::lambda_of(p)
.map(|(l, d)| (l, d, COARSE))
.or_else(|| f1024::lambda_of(p).map(|(l, d)| (l, d, FINE)))?;
let denom = d.to_f64()? * scale;
if denom == 0.0 || !denom.is_finite() {
return None;
}
let x = lambda[0].to_f64()? / denom;
let y = lambda[1].to_f64()? / denom;
let z = lambda[2].to_f64()? / denom;
if x.is_finite() && y.is_finite() && z.is_finite() {
Some([x, y, z])
} else {
None
}
}
#[cfg(test)]
mod tests {
use super::super::{interner::Interner, ImplicitPoint, Lpi, Tpi};
use super::lambda1024;
#[test]
fn degenerate_parallel_lpi_lambda_is_none_not_panic() {
let p = ImplicitPoint::Lpi(Lpi {
p: [0.0, 0.0, 1.0],
q: [1.0, 0.0, 1.0],
r: [0.0, 0.0, 0.0],
s: [1.0, 0.0, 0.0],
t: [0.0, 1.0, 0.0],
});
assert!(lambda1024(&p).is_none());
}
#[test]
fn degenerate_parallel_tpi_lambda_is_none_not_panic() {
let p = ImplicitPoint::Tpi(Tpi {
planes: [
[[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], [[0.0, 0.0, 1.0], [1.0, 0.0, 1.0], [0.0, 1.0, 1.0]], [[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], ],
});
assert!(lambda1024(&p).is_none());
}
#[test]
fn interner_survives_degenerate_point() {
let mut it = Interner::new();
it.intern(ImplicitPoint::Explicit([0.0, 0.0, 0.0]));
let _ = it.intern(ImplicitPoint::Lpi(Lpi {
p: [0.0, 0.0, 1.0],
q: [1.0, 0.0, 1.0],
r: [0.0, 0.0, 0.0],
s: [1.0, 0.0, 0.0],
t: [0.0, 1.0, 0.0],
}));
}
}