use super::expansion::{diff_expansion, expansion_sign, expansion_sum, product_of_expansions};
use super::sign::Sign;
use crate::primitives::Point3;
const INSPHERE_ERR_BOUND_FACTOR: f64 = 40.0 * f64::EPSILON / 2.0;
pub fn insphere(a: Point3, b: Point3, c: Point3, d: Point3, e: Point3) -> Sign {
let adx = a.x() - e.x();
let ady = a.y() - e.y();
let adz = a.z() - e.z();
let bdx = b.x() - e.x();
let bdy = b.y() - e.y();
let bdz = b.z() - e.z();
let cdx = c.x() - e.x();
let cdy = c.y() - e.y();
let cdz = c.z() - e.z();
let ddx = d.x() - e.x();
let ddy = d.y() - e.y();
let ddz = d.z() - e.z();
let adw = adx * adx + ady * ady + adz * adz;
let bdw = bdx * bdx + bdy * bdy + bdz * bdz;
let cdw = cdx * cdx + cdy * cdy + cdz * cdz;
let ddw = ddx * ddx + ddy * ddy + ddz * ddz;
let (m11, m11_bound) =
det3_with_precancel_bound((bdy, bdz, bdw), (cdy, cdz, cdw), (ddy, ddz, ddw));
let (m12, m12_bound) =
det3_with_precancel_bound((bdx, bdz, bdw), (cdx, cdz, cdw), (ddx, ddz, ddw));
let (m13, m13_bound) =
det3_with_precancel_bound((bdx, bdy, bdw), (cdx, cdy, cdw), (ddx, ddy, ddw));
let (m14, m14_bound) =
det3_with_precancel_bound((bdx, bdy, bdz), (cdx, cdy, cdz), (ddx, ddy, ddz));
let term_a = adx * m11;
let term_b = ady * m12;
let term_c = adz * m13;
let term_d = adw * m14;
let det = term_a - term_b + term_c - term_d;
let bound = INSPHERE_ERR_BOUND_FACTOR
* (adx.abs() * m11_bound
+ ady.abs() * m12_bound
+ adz.abs() * m13_bound
+ adw.abs() * m14_bound);
if bound > 0.0 && det.abs() > bound {
return Sign::of(det);
}
insphere_exact(a, b, c, d, e)
}
fn det3_with_precancel_bound(
p: (f64, f64, f64),
q: (f64, f64, f64),
r: (f64, f64, f64),
) -> (f64, f64) {
let qr_12 = q.1 * r.2;
let qr_21 = q.2 * r.1;
let qr_02 = q.0 * r.2;
let qr_20 = q.2 * r.0;
let qr_01 = q.0 * r.1;
let qr_10 = q.1 * r.0;
let value = p.0 * (qr_12 - qr_21) - p.1 * (qr_02 - qr_20) + p.2 * (qr_01 - qr_10);
let precancel_bound = p.0.abs() * (qr_12.abs() + qr_21.abs())
+ p.1.abs() * (qr_02.abs() + qr_20.abs())
+ p.2.abs() * (qr_01.abs() + qr_10.abs());
(value, precancel_bound)
}
fn insphere_exact(a: Point3, b: Point3, c: Point3, d: Point3, e: Point3) -> Sign {
let adx = diff_expansion(a.x(), e.x());
let ady = diff_expansion(a.y(), e.y());
let adz = diff_expansion(a.z(), e.z());
let bdx = diff_expansion(b.x(), e.x());
let bdy = diff_expansion(b.y(), e.y());
let bdz = diff_expansion(b.z(), e.z());
let cdx = diff_expansion(c.x(), e.x());
let cdy = diff_expansion(c.y(), e.y());
let cdz = diff_expansion(c.z(), e.z());
let ddx = diff_expansion(d.x(), e.x());
let ddy = diff_expansion(d.y(), e.y());
let ddz = diff_expansion(d.z(), e.z());
let square_sum = |x: &[f64], y: &[f64], z: &[f64]| -> Vec<f64> {
expansion_sum(
&expansion_sum(&product_of_expansions(x, x), &product_of_expansions(y, y)),
&product_of_expansions(z, z),
)
};
let adw = square_sum(&adx, &ady, &adz);
let bdw = square_sum(&bdx, &bdy, &bdz);
let cdw = square_sum(&cdx, &cdy, &cdz);
let ddw = square_sum(&ddx, &ddy, &ddz);
let m11 = det3_exact((&bdy, &bdz, &bdw), (&cdy, &cdz, &cdw), (&ddy, &ddz, &ddw));
let m12 = det3_exact((&bdx, &bdz, &bdw), (&cdx, &cdz, &cdw), (&ddx, &ddz, &ddw));
let m13 = det3_exact((&bdx, &bdy, &bdw), (&cdx, &cdy, &cdw), (&ddx, &ddy, &ddw));
let m14 = det3_exact((&bdx, &bdy, &bdz), (&cdx, &cdy, &cdz), (&ddx, &ddy, &ddz));
let term_a = product_of_expansions(&adx, &m11);
let term_b = product_of_expansions(&ady, &m12);
let term_c = product_of_expansions(&adz, &m13);
let term_d = product_of_expansions(&adw, &m14);
let det = expansion_sum(
&expansion_sum(&expansion_sum(&term_a, &negate(&term_b)), &term_c),
&negate(&term_d),
);
expansion_sign(&det)
}
fn det3_exact(
p: (&[f64], &[f64], &[f64]),
q: (&[f64], &[f64], &[f64]),
r: (&[f64], &[f64], &[f64]),
) -> Vec<f64> {
let qr_12 = product_of_expansions(q.1, r.2);
let qr_21 = product_of_expansions(q.2, r.1);
let qr_02 = product_of_expansions(q.0, r.2);
let qr_20 = product_of_expansions(q.2, r.0);
let qr_01 = product_of_expansions(q.0, r.1);
let qr_10 = product_of_expansions(q.1, r.0);
let term0 = product_of_expansions(p.0, &expansion_sum(&qr_12, &negate(&qr_21)));
let term1 = product_of_expansions(p.1, &expansion_sum(&qr_02, &negate(&qr_20)));
let term2 = product_of_expansions(p.2, &expansion_sum(&qr_01, &negate(&qr_10)));
expansion_sum(&expansion_sum(&term0, &negate(&term1)), &term2)
}
fn negate(e: &[f64]) -> Vec<f64> {
e.iter().map(|v| -v).collect()
}
#[cfg(test)]
mod tests {
use super::*;
fn pt(x: f64, y: f64, z: f64) -> Point3 {
Point3::new(x, y, z).unwrap()
}
fn regular_tetrahedron() -> (Point3, Point3, Point3, Point3) {
let h = 1.0 / 2.0_f64.sqrt();
(
pt(1.0, 0.0, -h),
pt(-1.0, 0.0, -h),
pt(0.0, 1.0, h),
pt(0.0, -1.0, h),
)
}
#[test]
fn center_is_inside() {
let (a, b, c, d) = regular_tetrahedron();
assert_eq!(insphere(a, b, c, d, pt(0.0, 0.0, 0.0)), Sign::Positive);
}
#[test]
fn far_point_is_outside() {
let (a, b, c, d) = regular_tetrahedron();
assert_eq!(
insphere(a, b, c, d, pt(100.0, 100.0, 100.0)),
Sign::Negative
);
}
#[test]
fn duplicate_point_is_zero() {
let p = pt(1.0, 2.0, 3.0);
assert_eq!(
insphere(
p,
p,
pt(4.0, 5.0, 6.0),
pt(7.0, 8.0, 9.0),
pt(1.0, 1.0, 1.0)
),
Sign::Zero
);
}
#[test]
fn coplanar_and_concyclic_abcd_is_zero() {
assert_eq!(
insphere(
pt(0.0, 0.0, 0.0),
pt(1.0, 0.0, 0.0),
pt(0.0, 1.0, 0.0),
pt(1.0, 1.0, 0.0),
pt(0.5, 0.5, 1.0)
),
Sign::Zero
);
}
#[test]
fn coplanar_but_not_concyclic_abcd_is_not_zero() {
assert_eq!(
insphere(
pt(0.0, 0.0, 0.0),
pt(1.0, 0.0, 0.0),
pt(0.0, 2.0, 0.0),
pt(3.0, 3.0, 0.0),
pt(0.5, 0.5, 1.0)
),
Sign::Negative
);
}
#[test]
fn exact_fallback_does_not_panic_on_near_cospherical() {
let (a, b, c, d) = regular_tetrahedron();
let e = pt(0.0, 0.0, 1e-9);
let _ = insphere(a, b, c, d, e);
}
}