#[cfg(test)]
use core::sync::atomic::{AtomicUsize, Ordering};
#[cfg(test)]
pub(crate) static ORIENT3D_EXACT_COUNT: AtomicUsize = AtomicUsize::new(0);
use crate::counters::{self, Outcome, Predicate};
use crate::expansion::{
O3D_ERR_BOUND_A, O3D_ERR_BOUND_B, O3D_ERR_BOUND_C, RESULT_ERR_BOUND, expansion_sum,
scale_expansion, two_product,
};
use crate::range::{Prepared, U_N, U_S, Window, prepare};
use crate::util::{sign_of, two_diff_tail, two_two_diff};
use crate::{NonFinite, Sign};
#[inline]
fn orient3d_filter_raw(pa: [f64; 3], pb: [f64; 3], pc: [f64; 3], pd: [f64; 3]) -> Option<Sign> {
let adx = pa[0] - pd[0];
let ady = pa[1] - pd[1];
let adz = pa[2] - pd[2];
let bdx = pb[0] - pd[0];
let bdy = pb[1] - pd[1];
let bdz = pb[2] - pd[2];
let cdx = pc[0] - pd[0];
let cdy = pc[1] - pd[1];
let cdz = pc[2] - pd[2];
let bdxcdy = bdx * cdy;
let cdxbdy = cdx * bdy;
let cdxady = cdx * ady;
let adxcdy = adx * cdy;
let adxbdy = adx * bdy;
let bdxady = bdx * ady;
let det = adz * (bdxcdy - cdxbdy) + bdz * (cdxady - adxcdy) + cdz * (adxbdy - bdxady);
let row = |z: f64, p: f64, q: f64| z.abs() * ((p.abs() + U_N) + (q.abs() + U_N)) + U_N;
let m1 = row(adz, bdxcdy, cdxbdy);
let m2 = row(bdz, cdxady, adxcdy);
let m3 = row(cdz, adxbdy, bdxady);
let bound = O3D_ERR_BOUND_A * ((m1 + m2) + m3) + U_S;
if det > bound || -det > bound {
Some(sign_of(det))
} else {
None
}
}
#[must_use = "the orientation result is the return value; discarding it is almost certainly a bug"]
pub fn orient3d(pa: [f64; 3], pb: [f64; 3], pc: [f64; 3], pd: [f64; 3]) -> Result<Sign, NonFinite> {
if !pa
.iter()
.chain(&pb)
.chain(&pc)
.chain(&pd)
.all(|v| v.is_finite())
{
counters::record(Predicate::Orient3d, Outcome::NonFinite);
return Err(NonFinite::new());
}
if let Some(sign) = orient3d_filter_raw(pa, pb, pc, pd) {
counters::record(Predicate::Orient3d, Outcome::Fast);
return Ok(sign);
}
let coords = [
pa[0], pa[1], pa[2], pb[0], pb[1], pb[2], pc[0], pc[1], pc[2], pd[0], pd[1], pd[2],
];
let (sa, sb, sc, sd) = match prepare(&coords, Window::ORIENT3D) {
Prepared::AsIs => (pa, pb, pc, pd),
Prepared::Scaled(s) => {
let scale = |p: [f64; 3]| [p[0] * s, p[1] * s, p[2] * s];
(scale(pa), scale(pb), scale(pc), scale(pd))
}
Prepared::OutOfWindow => return Ok(orient3d_exact_integer(pa, pb, pc, pd)),
};
let result = orient3d_in_window(sa, sb, sc, sd);
Ok(if result == Sign::Zero {
orient3d_exact_integer(pa, pb, pc, pd)
} else {
counters::record(Predicate::Orient3d, Outcome::Fast);
result
})
}
fn orient3d_in_window(pa: [f64; 3], pb: [f64; 3], pc: [f64; 3], pd: [f64; 3]) -> Sign {
let adx = pa[0] - pd[0];
let ady = pa[1] - pd[1];
let adz = pa[2] - pd[2];
let bdx = pb[0] - pd[0];
let bdy = pb[1] - pd[1];
let bdz = pb[2] - pd[2];
let cdx = pc[0] - pd[0];
let cdy = pc[1] - pd[1];
let cdz = pc[2] - pd[2];
let bdxcdy = bdx * cdy;
let cdxbdy = cdx * bdy;
let cdxady = cdx * ady;
let adxcdy = adx * cdy;
let adxbdy = adx * bdy;
let bdxady = bdx * ady;
let det = adz * (bdxcdy - cdxbdy) + bdz * (cdxady - adxcdy) + cdz * (adxbdy - bdxady);
let permanent = ((bdxcdy.abs() + cdxbdy.abs()) * adz.abs())
+ ((cdxady.abs() + adxcdy.abs()) * bdz.abs())
+ ((adxbdy.abs() + bdxady.abs()) * cdz.abs());
let errbound = O3D_ERR_BOUND_A * permanent;
if det > errbound || -det > errbound {
return sign_of(det);
}
orient3d_adapt(pa, pb, pc, pd, permanent)
}
#[inline(always)]
fn fin_merge_o3d(
fin1: &mut [f64; 192],
fin2: &mut [f64; 192],
fin_len: usize,
active: usize,
correction: &[f64],
) -> usize {
if active == 0 {
expansion_sum(&fin1[..fin_len], correction, fin2)
} else {
expansion_sum(&fin2[..fin_len], correction, fin1)
}
}
#[allow(clippy::too_many_lines)]
fn orient3d_adapt(pa: [f64; 3], pb: [f64; 3], pc: [f64; 3], pd: [f64; 3], permanent: f64) -> Sign {
#[cfg(test)]
ORIENT3D_EXACT_COUNT.fetch_add(1, Ordering::Relaxed);
let adx = pa[0] - pd[0];
let ady = pa[1] - pd[1];
let adz = pa[2] - pd[2];
let bdx = pb[0] - pd[0];
let bdy = pb[1] - pd[1];
let bdz = pb[2] - pd[2];
let cdx = pc[0] - pd[0];
let cdy = pc[1] - pd[1];
let cdz = pc[2] - pd[2];
let (bdxcdy1, bdxcdy0) = two_product(bdx, cdy);
let (cdxbdy1, cdxbdy0) = two_product(cdx, bdy);
let bc = two_two_diff(bdxcdy1, bdxcdy0, cdxbdy1, cdxbdy0);
let (cdxady1, cdxady0) = two_product(cdx, ady);
let (adxcdy1, adxcdy0) = two_product(adx, cdy);
let ca = two_two_diff(cdxady1, cdxady0, adxcdy1, adxcdy0);
let (adxbdy1, adxbdy0) = two_product(adx, bdy);
let (bdxady1, bdxady0) = two_product(bdx, ady);
let ab = two_two_diff(adxbdy1, adxbdy0, bdxady1, bdxady0);
let mut adet = [0.0_f64; 8];
let adet_len = scale_expansion(&bc, adz, &mut adet);
let mut bdet = [0.0_f64; 8];
let bdet_len = scale_expansion(&ca, bdz, &mut bdet);
let mut cdet = [0.0_f64; 8];
let cdet_len = scale_expansion(&ab, cdz, &mut cdet);
let mut abdet = [0.0_f64; 16];
let abdet_len = expansion_sum(&adet[..adet_len], &bdet[..bdet_len], &mut abdet);
let mut fin1 = [0.0_f64; 192];
let mut fin2 = [0.0_f64; 192];
let mut fin_len = expansion_sum(&abdet[..abdet_len], &cdet[..cdet_len], &mut fin1);
let mut active: usize = 0;
let det: f64 = fin1[..fin_len].iter().sum();
if det.abs() >= O3D_ERR_BOUND_B * permanent {
return sign_of(det);
}
let adxtail = two_diff_tail(pa[0], pd[0], adx);
let adytail = two_diff_tail(pa[1], pd[1], ady);
let adztail = two_diff_tail(pa[2], pd[2], adz);
let bdxtail = two_diff_tail(pb[0], pd[0], bdx);
let bdytail = two_diff_tail(pb[1], pd[1], bdy);
let bdztail = two_diff_tail(pb[2], pd[2], bdz);
let cdxtail = two_diff_tail(pc[0], pd[0], cdx);
let cdytail = two_diff_tail(pc[1], pd[1], cdy);
let cdztail = two_diff_tail(pc[2], pd[2], cdz);
if adxtail == 0.0
&& adytail == 0.0
&& adztail == 0.0
&& bdxtail == 0.0
&& bdytail == 0.0
&& bdztail == 0.0
&& cdxtail == 0.0
&& cdytail == 0.0
&& cdztail == 0.0
{
return sign_of(det);
}
let errbound = O3D_ERR_BOUND_C * permanent + RESULT_ERR_BOUND * det.abs();
let det2 = det
+ (adz * ((bdx * cdytail + cdy * bdxtail) - (bdy * cdxtail + cdx * bdytail))
+ adztail * (bdx * cdy - bdy * cdx))
+ (bdz * ((cdx * adytail + ady * cdxtail) - (cdy * adxtail + adx * cdytail))
+ bdztail * (cdx * ady - cdy * adx))
+ (cdz * ((adx * bdytail + bdy * adxtail) - (ady * bdxtail + bdx * adytail))
+ cdztail * (adx * bdy - ady * bdx));
if det2 >= errbound || -det2 >= errbound {
return sign_of(det2);
}
let mut tmp8a = [0.0_f64; 8];
let mut tmp8b = [0.0_f64; 8];
macro_rules! add_scaled {
($comp:expr, $scale:expr, $buf:expr) => {{
let n = scale_expansion($comp, $scale, $buf);
if n > 0 {
fin_len = fin_merge_o3d(&mut fin1, &mut fin2, fin_len, active, &$buf[..n]);
active ^= 1;
}
}};
}
if adztail != 0.0 {
let n = scale_expansion(&bc, adztail, &mut tmp8a);
if n > 0 {
fin_len = fin_merge_o3d(&mut fin1, &mut fin2, fin_len, active, &tmp8a[..n]);
active ^= 1;
}
}
if bdxtail != 0.0 {
let (s1, s0) = two_product(bdxtail, cdy);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2, adztail, &mut tmp8b);
}
}
if cdytail != 0.0 {
let (s1, s0) = two_product(bdx, cdytail);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2, adztail, &mut tmp8b);
}
if bdxtail != 0.0 {
let (s1, s0) = two_product(bdxtail, cdytail);
let tmp2c = [s0, s1];
add_scaled!(&tmp2c, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2c, adztail, &mut tmp8b);
}
}
}
if bdytail != 0.0 {
let (s1, s0) = two_product(bdytail, cdx);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2, adztail, &mut tmp8b);
}
}
if cdxtail != 0.0 {
let (s1, s0) = two_product(bdy, cdxtail);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2, adztail, &mut tmp8b);
}
if bdytail != 0.0 {
let (s1, s0) = two_product(bdytail, cdxtail);
let tmp2c = [-s0, -s1];
add_scaled!(&tmp2c, adz, &mut tmp8a);
if adztail != 0.0 {
add_scaled!(&tmp2c, adztail, &mut tmp8b);
}
}
}
if bdztail != 0.0 {
let n = scale_expansion(&ca, bdztail, &mut tmp8a);
if n > 0 {
fin_len = fin_merge_o3d(&mut fin1, &mut fin2, fin_len, active, &tmp8a[..n]);
active ^= 1;
}
}
if cdxtail != 0.0 {
let (s1, s0) = two_product(cdxtail, ady);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2, bdztail, &mut tmp8b);
}
}
if adytail != 0.0 {
let (s1, s0) = two_product(cdx, adytail);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2, bdztail, &mut tmp8b);
}
if cdxtail != 0.0 {
let (s1, s0) = two_product(cdxtail, adytail);
let tmp2c = [s0, s1];
add_scaled!(&tmp2c, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2c, bdztail, &mut tmp8b);
}
}
}
if cdytail != 0.0 {
let (s1, s0) = two_product(cdytail, adx);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2, bdztail, &mut tmp8b);
}
}
if adxtail != 0.0 {
let (s1, s0) = two_product(cdy, adxtail);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2, bdztail, &mut tmp8b);
}
if cdytail != 0.0 {
let (s1, s0) = two_product(cdytail, adxtail);
let tmp2c = [-s0, -s1];
add_scaled!(&tmp2c, bdz, &mut tmp8a);
if bdztail != 0.0 {
add_scaled!(&tmp2c, bdztail, &mut tmp8b);
}
}
}
if cdztail != 0.0 {
let n = scale_expansion(&ab, cdztail, &mut tmp8a);
if n > 0 {
fin_len = fin_merge_o3d(&mut fin1, &mut fin2, fin_len, active, &tmp8a[..n]);
active ^= 1;
}
}
if adxtail != 0.0 {
let (s1, s0) = two_product(adxtail, bdy);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2, cdztail, &mut tmp8b);
}
}
if bdytail != 0.0 {
let (s1, s0) = two_product(adx, bdytail);
let tmp2 = [s0, s1];
add_scaled!(&tmp2, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2, cdztail, &mut tmp8b);
}
if adxtail != 0.0 {
let (s1, s0) = two_product(adxtail, bdytail);
let tmp2c = [s0, s1];
add_scaled!(&tmp2c, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2c, cdztail, &mut tmp8b);
}
}
}
if adytail != 0.0 {
let (s1, s0) = two_product(adytail, bdx);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2, cdztail, &mut tmp8b);
}
}
if bdxtail != 0.0 {
let (s1, s0) = two_product(ady, bdxtail);
let tmp2 = [-s0, -s1];
add_scaled!(&tmp2, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2, cdztail, &mut tmp8b);
}
if adytail != 0.0 {
let (s1, s0) = two_product(adytail, bdxtail);
let tmp2c = [-s0, -s1];
add_scaled!(&tmp2c, cdz, &mut tmp8a);
if cdztail != 0.0 {
add_scaled!(&tmp2c, cdztail, &mut tmp8b);
}
}
}
let fin_slice = if active == 0 {
&fin1[..fin_len]
} else {
&fin2[..fin_len]
};
if fin_len == 0 {
return Sign::Zero;
}
sign_of(fin_slice[fin_len - 1])
}
fn orient3d_exact_integer(pa: [f64; 3], pb: [f64; 3], pc: [f64; 3], pd: [f64; 3]) -> Sign {
counters::record(Predicate::Orient3d, Outcome::Exact);
use crate::exact_int::f64_to_orient3d_int;
let ax = f64_to_orient3d_int(pa[0]);
let ay = f64_to_orient3d_int(pa[1]);
let az = f64_to_orient3d_int(pa[2]);
let bx = f64_to_orient3d_int(pb[0]);
let by_ = f64_to_orient3d_int(pb[1]);
let bz = f64_to_orient3d_int(pb[2]);
let cx = f64_to_orient3d_int(pc[0]);
let cy = f64_to_orient3d_int(pc[1]);
let cz = f64_to_orient3d_int(pc[2]);
let dx = f64_to_orient3d_int(pd[0]);
let dy = f64_to_orient3d_int(pd[1]);
let dz = f64_to_orient3d_int(pd[2]);
let adx = ax - dx;
let ady = ay - dy;
let adz = az - dz;
let bdx = bx - dx;
let bdy = by_ - dy;
let bdz = bz - dz;
let cdx = cx - dx;
let cdy = cy - dy;
let cdz = cz - dz;
let bc = bdx * cdy - bdy * cdx;
let ca = cdx * ady - cdy * adx;
let ab = adx * bdy - ady * bdx;
let det = adz * bc + bdz * ca + cdz * ab;
if det.is_positive() {
Sign::Positive
} else if det.is_negative() {
Sign::Negative
} else {
counters::record(Predicate::Orient3d, Outcome::ExactZero);
Sign::Zero
}
}
#[cfg(test)]
#[allow(
clippy::float_cmp,
clippy::cast_possible_truncation,
clippy::cast_precision_loss,
clippy::cast_possible_wrap,
clippy::range_plus_one,
clippy::no_effect_underscore_binding
)]
mod tests {
use super::*;
use core::sync::atomic::Ordering;
#[test]
fn orient3d_clearly_positive() {
let pa = [0.0_f64, 0.0, 0.0];
let pb = [1.0_f64, 0.0, 0.0];
let pc = [0.0_f64, 1.0, 0.0];
let pd = [0.0_f64, 0.0, -1.0];
assert_eq!(orient3d(pa, pb, pc, pd), Ok(Sign::Positive));
}
#[test]
fn orient3d_clearly_negative() {
let pa = [0.0_f64, 0.0, 0.0];
let pb = [1.0_f64, 0.0, 0.0];
let pc = [0.0_f64, 1.0, 0.0];
let pd = [0.0_f64, 0.0, 1.0];
assert_eq!(orient3d(pa, pb, pc, pd), Ok(Sign::Negative));
}
#[test]
fn orient3d_exactly_coplanar() {
let pa = [0.0_f64, 0.0, 0.0];
let pb = [1.0_f64, 0.0, 0.0];
let pc = [0.5_f64, 1.0, 0.0];
let pd = [0.25_f64, 0.5, 0.0];
assert_eq!(orient3d(pa, pb, pc, pd), Ok(Sign::Zero));
}
#[test]
fn orient3d_exact_count_increments_on_near_coplanar() {
let pa = [0.0_f64, 0.0, 1.0];
let pb = [1.0_f64, 0.0, 2.0];
let pc = [0.0_f64, 1.0, 3.0];
let pd = [1.0_f64, 1.0, 4.0 + 8.0 * f64::EPSILON];
let before = ORIENT3D_EXACT_COUNT.load(Ordering::Relaxed);
let result = orient3d(pa, pb, pc, pd);
let after = ORIENT3D_EXACT_COUNT.load(Ordering::Relaxed);
assert_eq!(result, Ok(Sign::Negative), "pd is above the plane, det < 0");
assert!(
after > before,
"Stage 3 counter should have been incremented for near-coplanar input"
);
}
#[test]
fn orient3d_non_finite_inputs_return_err() {
let inf = f64::INFINITY;
let nan = f64::NAN;
assert_eq!(
orient3d(
[inf, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]
),
Err(NonFinite::new()),
"a +inf coordinate must be rejected"
);
assert_eq!(
orient3d(
[nan, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]
),
Err(NonFinite::new()),
"a NaN coordinate must be rejected"
);
assert_eq!(
orient3d(
[f64::NEG_INFINITY, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0],
),
Err(NonFinite::new()),
"a -inf coordinate must be rejected"
);
}
#[test]
fn orient3d_finite_overflow_is_not_zero() {
let m = f64::MAX;
let pa = [m, 0.0, 0.0];
let pb = [0.0, m, 0.0];
let pc = [0.0, 0.0, m];
let pd = [-m, -m, -m];
assert!(
pa[0] - pd[0] == f64::INFINITY,
"precondition: difference overflows"
);
assert_eq!(
orient3d(pa, pb, pc, pd),
Ok(Sign::Positive),
"finite non-degenerate input must not collapse to Zero on overflow"
);
}
}