1use super::{DropAxis, ImplicitPoint, Sign};
34
35macro_rules! fixed_impl {
38 ($T:ty, $scale:expr) => {
39 use super::super::{assemble_sign, DropAxis, ImplicitPoint, Lpi, Sign, Tpi};
40 use num_traits::{CheckedAdd, CheckedMul, CheckedSub, FromPrimitive, One, Signed};
41
42 type I = $T;
43 type V3 = [I; 3];
44
45 #[inline]
46 fn gi(x: f64) -> Option<I> {
47 let scaled = x * $scale;
48 if !scaled.is_finite() || scaled.fract() != 0.0 || scaled.abs() >= 9.0e18 {
49 return None;
50 }
51 I::from_i64(scaled as i64)
52 }
53 #[inline]
54 fn vec(p: [f64; 3]) -> Option<V3> {
55 Some([gi(p[0])?, gi(p[1])?, gi(p[2])?])
56 }
57 #[inline]
58 fn mul(a: I, b: I) -> Option<I> {
59 CheckedMul::checked_mul(&a, &b)
60 }
61 #[inline]
62 fn sub(a: I, b: I) -> Option<I> {
63 CheckedSub::checked_sub(&a, &b)
64 }
65 #[inline]
66 fn add(a: I, b: I) -> Option<I> {
67 CheckedAdd::checked_add(&a, &b)
68 }
69 fn sub3(a: &V3, b: &V3) -> Option<V3> {
70 Some([sub(a[0], b[0])?, sub(a[1], b[1])?, sub(a[2], b[2])?])
71 }
72 fn cross(u: &V3, v: &V3) -> Option<V3> {
73 Some([
74 sub(mul(u[1], v[2])?, mul(u[2], v[1])?)?,
75 sub(mul(u[2], v[0])?, mul(u[0], v[2])?)?,
76 sub(mul(u[0], v[1])?, mul(u[1], v[0])?)?,
77 ])
78 }
79 fn det3(u: &V3, v: &V3, w: &V3) -> Option<I> {
80 let m0 = sub(mul(v[1], w[2])?, mul(v[2], w[1])?)?;
81 let m1 = sub(mul(v[2], w[0])?, mul(v[0], w[2])?)?;
82 let m2 = sub(mul(v[0], w[1])?, mul(v[1], w[0])?)?;
83 add(add(mul(u[0], m0)?, mul(u[1], m1)?)?, mul(u[2], m2)?)
84 }
85 #[inline]
86 fn sign_of(x: &I) -> Sign {
87 if x.is_negative() {
89 Sign::Negative
90 } else if x.is_zero() {
91 Sign::Zero
92 } else {
93 Sign::Positive
94 }
95 }
96 #[inline]
97 fn axis_idx(axis: DropAxis) -> (usize, usize) {
98 match axis {
99 DropAxis::X => (1, 2),
100 DropAxis::Y => (0, 2),
101 DropAxis::Z => (0, 1),
102 }
103 }
104 fn lpi_lambda(l: &Lpi) -> Option<(V3, I)> {
105 let p = vec(l.p)?;
106 let q = vec(l.q)?;
107 let rr = vec(l.r)?;
108 let s = vec(l.s)?;
109 let t = vec(l.t)?;
110 let qp = sub3(&q, &p)?;
111 let sr = sub3(&s, &rr)?;
112 let tr = sub3(&t, &rr)?;
113 let pr = sub3(&p, &rr)?;
114 let d = det3(&qp, &sr, &tr)?;
115 let n = det3(&pr, &sr, &tr)?;
116 let lx = sub(mul(d, p[0])?, mul(n, qp[0])?)?;
117 let ly = sub(mul(d, p[1])?, mul(n, qp[1])?)?;
118 let lz = sub(mul(d, p[2])?, mul(n, qp[2])?)?;
119 Some(([lx, ly, lz], d))
120 }
121 fn tpi_lambda(t: &Tpi) -> Option<(V3, I)> {
122 let plane = |pl: &[[f64; 3]; 3]| -> Option<(V3, I)> {
123 let a = vec(pl[0])?;
124 let ba = sub3(&vec(pl[1])?, &a)?;
125 let ca = sub3(&vec(pl[2])?, &a)?;
126 let n = cross(&ba, &ca)?;
127 let off = add(add(mul(n[0], a[0])?, mul(n[1], a[1])?)?, mul(n[2], a[2])?)?;
128 Some((n, off))
129 };
130 let (n1, c1) = plane(&t.planes[0])?;
131 let (n2, c2) = plane(&t.planes[1])?;
132 let (n3, c3) = plane(&t.planes[2])?;
133 let d = det3(&n1, &n2, &n3)?;
134 let ns = [n1, n2, n3];
135 let cs = [c1, c2, c3];
136 let cramer = |k: usize| -> Option<I> {
137 let mut rows = [ns[0], ns[1], ns[2]];
138 for (row, &ci) in rows.iter_mut().zip(cs.iter()) {
139 row[k] = ci;
140 }
141 det3(&rows[0], &rows[1], &rows[2])
142 };
143 Some(([cramer(0)?, cramer(1)?, cramer(2)?], d))
144 }
145 pub fn lambda_of(p: &ImplicitPoint) -> Option<(V3, I)> {
146 match p {
147 ImplicitPoint::Lpi(l) => lpi_lambda(l),
148 ImplicitPoint::Tpi(t) => tpi_lambda(t),
149 ImplicitPoint::Explicit(e) => Some((vec(*e)?, I::one())),
150 }
151 }
152 pub fn orient2d_2i(a: &ImplicitPoint, b: &ImplicitPoint, c: [f64; 3], axis: DropAxis) -> Option<Sign> {
153 let (i, j) = axis_idx(axis);
154 let (lam1, d1) = lambda_of(a)?;
155 let (lam2, d2) = lambda_of(b)?;
156 let cr = vec(c)?;
157 let a_i = sub(lam1[i], mul(d1, cr[i])?)?;
158 let a_j = sub(lam1[j], mul(d1, cr[j])?)?;
159 let b_i = sub(lam2[i], mul(d2, cr[i])?)?;
160 let b_j = sub(lam2[j], mul(d2, cr[j])?)?;
161 let det = sub(mul(a_i, b_j)?, mul(a_j, b_i)?)?;
162 Some(assemble_sign(sign_of(&det), &[sign_of(&d1), sign_of(&d2)]))
163 }
164 pub fn orient2d_3i(a: &ImplicitPoint, b: &ImplicitPoint, c: &ImplicitPoint, axis: DropAxis) -> Option<Sign> {
165 let (i, j) = axis_idx(axis);
166 let (lam1, d1) = lambda_of(a)?;
167 let (lam2, d2) = lambda_of(b)?;
168 let (lam3, d3) = lambda_of(c)?;
169 let u_i = sub(mul(d1, lam2[i])?, mul(d2, lam1[i])?)?;
170 let u_j = sub(mul(d1, lam2[j])?, mul(d2, lam1[j])?)?;
171 let v_i = sub(mul(d1, lam3[i])?, mul(d3, lam1[i])?)?;
172 let v_j = sub(mul(d1, lam3[j])?, mul(d3, lam1[j])?)?;
173 let det = sub(mul(u_i, v_j)?, mul(u_j, v_i)?)?;
174 Some(assemble_sign(sign_of(&det), &[sign_of(&d2), sign_of(&d3)]))
175 }
176 pub fn indirect_orient2d(p: &ImplicitPoint, b: [f64; 3], c: [f64; 3], axis: DropAxis) -> Option<Sign> {
177 let (i, j) = axis_idx(axis);
178 let (lambda, d) = lambda_of(p)?;
179 let br = vec(b)?;
180 let cr = vec(c)?;
181 let li = sub(lambda[i], mul(d, cr[i])?)?;
182 let lj = sub(lambda[j], mul(d, cr[j])?)?;
183 let det = sub(mul(li, sub(br[j], cr[j])?)?, mul(lj, sub(br[i], cr[i])?)?)?;
184 Some(assemble_sign(sign_of(&det), &[sign_of(&d)]))
185 }
186 fn cmp_axis(a: &ImplicitPoint, b: &ImplicitPoint, k: usize) -> Option<Sign> {
187 use ImplicitPoint::Explicit;
188 match (a, b) {
189 (Explicit(ae), Explicit(be)) => Some(sign_of(&sub(gi(ae[k])?, gi(be[k])?)?)),
190 (_, Explicit(be)) => {
191 let (lam, d) = lambda_of(a)?;
192 let bk = gi(be[k])?;
193 Some(assemble_sign(sign_of(&sub(lam[k], mul(d, bk)?)?), &[sign_of(&d)]))
194 }
195 (Explicit(ae), _) => {
196 let (lam, d) = lambda_of(b)?;
197 let ak = gi(ae[k])?;
198 Some(assemble_sign(sign_of(&sub(mul(ak, d)?, lam[k])?), &[sign_of(&d)]))
199 }
200 (_, _) => {
201 let (la, da) = lambda_of(a)?;
202 let (lb, db) = lambda_of(b)?;
203 Some(assemble_sign(
204 sign_of(&sub(mul(la[k], db)?, mul(lb[k], da)?)?),
205 &[sign_of(&da), sign_of(&db)],
206 ))
207 }
208 }
209 }
210 pub fn cmp_lex(a: &ImplicitPoint, b: &ImplicitPoint) -> Option<Sign> {
211 for k in 0..3 {
212 let s = cmp_axis(a, b, k)?;
213 if s != Sign::Zero {
214 return Some(s);
215 }
216 }
217 Some(Sign::Zero)
218 }
219 pub fn cmp_along(a: &ImplicitPoint, b: &ImplicitPoint, u: [f64; 3]) -> Option<Sign> {
220 let (la, da) = lambda_of(a)?;
221 let (lb, db) = lambda_of(b)?;
222 let ur = vec(u)?;
223 let dot_a = add(add(mul(la[0], ur[0])?, mul(la[1], ur[1])?)?, mul(la[2], ur[2])?)?;
224 let dot_b = add(add(mul(lb[0], ur[0])?, mul(lb[1], ur[1])?)?, mul(lb[2], ur[2])?)?;
225 let num = sub(mul(dot_a, db)?, mul(dot_b, da)?)?;
226 Some(assemble_sign(sign_of(&num), &[sign_of(&da), sign_of(&db)]))
227 }
228 pub fn indirect_orient3d(p: &ImplicitPoint, p2: [f64; 3], p3: [f64; 3], p4: [f64; 3]) -> Option<Sign> {
229 let (lambda, d) = lambda_of(p)?;
230 let p4r = vec(p4)?;
231 let row1 = [
232 sub(lambda[0], mul(d, p4r[0])?)?,
233 sub(lambda[1], mul(d, p4r[1])?)?,
234 sub(lambda[2], mul(d, p4r[2])?)?,
235 ];
236 let row2 = sub3(&vec(p2)?, &p4r)?;
237 let row3 = sub3(&vec(p3)?, &p4r)?;
238 Some(assemble_sign(sign_of(&det3(&row1, &row2, &row3)?), &[sign_of(&d)]))
239 }
240 };
241}
242
243const COARSE: f64 = 65_536.0;
245const FINE: f64 = 68_719_476_736.0;
247const FINE_OVER_COARSE: i64 = 1 << 20;
250
251mod w256 {
252 fixed_impl!(bnum::types::I256, super::COARSE);
253}
254mod w512 {
255 fixed_impl!(bnum::types::I512, super::COARSE);
256}
257mod w1024 {
258 fixed_impl!(bnum::types::I1024, super::COARSE);
259}
260mod f256 {
261 fixed_impl!(bnum::types::I256, super::FINE);
262}
263mod f512 {
264 fixed_impl!(bnum::types::I512, super::FINE);
265}
266mod f1024 {
267 fixed_impl!(bnum::types::I1024, super::FINE);
268}
269mod f2048 {
270 fixed_impl!(bnum::types::I2048, super::FINE);
271}
272
273macro_rules! cascade {
279 ($name:ident ( $($arg:ident : $ty:ty),* )) => {
280 pub fn $name($($arg : $ty),*) -> Option<Sign> {
281 w256::$name($($arg),*)
282 .or_else(|| w512::$name($($arg),*))
283 .or_else(|| w1024::$name($($arg),*))
284 .or_else(|| f256::$name($($arg),*))
285 .or_else(|| f512::$name($($arg),*))
286 .or_else(|| f1024::$name($($arg),*))
287 .or_else(|| f2048::$name($($arg),*))
288 }
289 };
290}
291cascade!(orient2d_2i(a: &ImplicitPoint, b: &ImplicitPoint, c: [f64; 3], axis: DropAxis));
292cascade!(orient2d_3i(a: &ImplicitPoint, b: &ImplicitPoint, c: &ImplicitPoint, axis: DropAxis));
293cascade!(indirect_orient2d(p: &ImplicitPoint, b: [f64; 3], c: [f64; 3], axis: DropAxis));
294cascade!(cmp_lex(a: &ImplicitPoint, b: &ImplicitPoint));
295cascade!(cmp_along(a: &ImplicitPoint, b: &ImplicitPoint, u: [f64; 3]));
296cascade!(indirect_orient3d(p: &ImplicitPoint, p2: [f64; 3], p3: [f64; 3], p4: [f64; 3]));
297
298type Big = bnum::types::I512;
315pub type Lam = ([Big; 3], Big);
316
317#[inline]
318fn bmul(a: Big, b: Big) -> Option<Big> {
319 num_traits::CheckedMul::checked_mul(&a, &b)
320}
321#[inline]
322fn bsub(a: Big, b: Big) -> Option<Big> {
323 num_traits::CheckedSub::checked_sub(&a, &b)
324}
325#[inline]
326fn bsign(x: &Big) -> Sign {
327 use num_traits::{Signed, Zero};
328 if x.is_negative() {
329 Sign::Negative
330 } else if x.is_zero() {
331 Sign::Zero
332 } else {
333 Sign::Positive
334 }
335}
336
337pub fn lambda1024(p: &ImplicitPoint) -> Option<Lam> {
345 use num_traits::{CheckedMul, FromPrimitive, One};
346 let (mut lam, mut d) = w512::lambda_of(p).or_else(|| {
347 let (lam, d) = f512::lambda_of(p)?;
348 let ratio = Big::from_i64(FINE_OVER_COARSE)?;
349 Some((lam, CheckedMul::checked_mul(&d, &ratio)?))
350 })?;
351 if d.is_negative() {
353 d = -d;
354 lam = [-lam[0], -lam[1], -lam[2]];
355 }
356 if d.is_zero() {
364 return None;
365 }
366 if !d.is_one()
373 && (lam[0] % d).is_zero()
374 && (lam[1] % d).is_zero()
375 && (lam[2] % d).is_zero()
376 {
377 lam = [lam[0] / d, lam[1] / d, lam[2] / d];
378 d = One::one();
379 }
380 Some((lam, d))
381}
382
383#[inline]
384fn axis_ij(axis: DropAxis) -> (usize, usize) {
385 match axis {
386 DropAxis::X => (1, 2),
387 DropAxis::Y => (0, 2),
388 DropAxis::Z => (0, 1),
389 }
390}
391
392pub fn orient2d_from_lam(a: &Lam, b: &Lam, c: &Lam, axis: DropAxis) -> Option<Sign> {
394 use num_traits::{One, ToPrimitive};
395 let (i, j) = axis_ij(axis);
396 let (lam1, d1) = a;
397 let (lam2, d2) = b;
398 let (lam3, d3) = c;
399 if d1.is_one() && d2.is_one() && d3.is_one() {
404 if let (Some(ai), Some(aj), Some(bi), Some(bj), Some(ci), Some(cj)) = (
405 lam1[i].to_i64(),
406 lam1[j].to_i64(),
407 lam2[i].to_i64(),
408 lam2[j].to_i64(),
409 lam3[i].to_i64(),
410 lam3[j].to_i64(),
411 ) {
412 let (ai, aj) = (ai as i128, aj as i128);
413 let u_i = bi as i128 - ai;
414 let u_j = bj as i128 - aj;
415 let v_i = ci as i128 - ai;
416 let v_j = cj as i128 - aj;
417 if let (Some(p1), Some(p2)) = (u_i.checked_mul(v_j), u_j.checked_mul(v_i)) {
418 if let Some(det) = p1.checked_sub(p2) {
419 return Some(match det.cmp(&0) {
420 std::cmp::Ordering::Less => Sign::Negative,
421 std::cmp::Ordering::Greater => Sign::Positive,
422 std::cmp::Ordering::Equal => Sign::Zero,
423 });
424 }
425 }
426 }
427 }
428 let u_i = bsub(bmul(*d1, lam2[i])?, bmul(*d2, lam1[i])?)?;
429 let u_j = bsub(bmul(*d1, lam2[j])?, bmul(*d2, lam1[j])?)?;
430 let v_i = bsub(bmul(*d1, lam3[i])?, bmul(*d3, lam1[i])?)?;
431 let v_j = bsub(bmul(*d1, lam3[j])?, bmul(*d3, lam1[j])?)?;
432 let det = bsub(bmul(u_i, v_j)?, bmul(u_j, v_i)?)?;
433 Some(super::assemble_sign(bsign(&det), &[bsign(d2), bsign(d3)]))
434}
435
436pub fn cmp_lex_from_lam(a: &Lam, b: &Lam) -> Option<Sign> {
438 use num_traits::{One, ToPrimitive};
439 let (la, da) = a;
440 let (lb, db) = b;
441 if da.is_one() && db.is_one() {
444 if let (Some(a0), Some(a1), Some(a2), Some(b0), Some(b1), Some(b2)) = (
445 la[0].to_i64(),
446 la[1].to_i64(),
447 la[2].to_i64(),
448 lb[0].to_i64(),
449 lb[1].to_i64(),
450 lb[2].to_i64(),
451 ) {
452 for (x, y) in [(a0, b0), (a1, b1), (a2, b2)] {
453 match x.cmp(&y) {
454 std::cmp::Ordering::Less => return Some(Sign::Negative),
455 std::cmp::Ordering::Greater => return Some(Sign::Positive),
456 std::cmp::Ordering::Equal => {}
457 }
458 }
459 return Some(Sign::Zero);
460 }
461 }
462 for k in 0..3 {
463 let s = bsub(bmul(la[k], *db)?, bmul(lb[k], *da)?)?;
464 let sg = super::assemble_sign(bsign(&s), &[bsign(da), bsign(db)]);
465 if sg != Sign::Zero {
466 return Some(sg);
467 }
468 }
469 Some(Sign::Zero)
470}
471
472pub fn point_to_f64(p: &ImplicitPoint) -> Option<[f64; 3]> {
479 use num_traits::ToPrimitive;
480 let (lambda, d, scale) = w1024::lambda_of(p)
483 .map(|(l, d)| (l, d, COARSE))
484 .or_else(|| f1024::lambda_of(p).map(|(l, d)| (l, d, FINE)))?;
485 let denom = d.to_f64()? * scale;
486 if denom == 0.0 || !denom.is_finite() {
487 return None;
488 }
489 let x = lambda[0].to_f64()? / denom;
490 let y = lambda[1].to_f64()? / denom;
491 let z = lambda[2].to_f64()? / denom;
492 if x.is_finite() && y.is_finite() && z.is_finite() {
493 Some([x, y, z])
494 } else {
495 None
496 }
497}
498
499#[cfg(test)]
500mod tests {
501 use super::super::{interner::Interner, ImplicitPoint, Lpi, Tpi};
502 use super::lambda1024;
503
504 #[test]
508 fn degenerate_parallel_lpi_lambda_is_none_not_panic() {
509 let p = ImplicitPoint::Lpi(Lpi {
512 p: [0.0, 0.0, 1.0],
513 q: [1.0, 0.0, 1.0],
514 r: [0.0, 0.0, 0.0],
515 s: [1.0, 0.0, 0.0],
516 t: [0.0, 1.0, 0.0],
517 });
518 assert!(lambda1024(&p).is_none());
519 }
520
521 #[test]
523 fn degenerate_parallel_tpi_lambda_is_none_not_panic() {
524 let p = ImplicitPoint::Tpi(Tpi {
525 planes: [
526 [[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]], ],
530 });
531 assert!(lambda1024(&p).is_none());
532 }
533
534 #[test]
538 fn interner_survives_degenerate_point() {
539 let mut it = Interner::new();
540 it.intern(ImplicitPoint::Explicit([0.0, 0.0, 0.0]));
541 let _ = it.intern(ImplicitPoint::Lpi(Lpi {
542 p: [0.0, 0.0, 1.0],
543 q: [1.0, 0.0, 1.0],
544 r: [0.0, 0.0, 0.0],
545 s: [1.0, 0.0, 0.0],
546 t: [0.0, 1.0, 0.0],
547 }));
548 }
549}