1mod number;
8
9use core::fmt::Display;
10
11pub use number::Number;
12
13use crate::geop_error::{GeopError, GeopResult};
14
15#[inline]
25pub(crate) fn next_up(x: f64) -> f64 {
26 if x.is_nan() {
27 return x;
28 }
29 if x == f64::INFINITY {
30 return f64::INFINITY;
31 }
32 if x == 0.0 {
33 return f64::from_bits(1);
34 }
35 let bits = x.to_bits();
36 let bits = if x > 0.0 { bits + 1 } else { bits - 1 };
37 f64::from_bits(bits)
38}
39
40#[inline]
41pub(crate) fn next_down(x: f64) -> f64 {
42 if x.is_nan() {
43 return x;
44 }
45 if x == f64::NEG_INFINITY {
46 return f64::NEG_INFINITY;
47 }
48 if x == 0.0 {
49 return -f64::from_bits(1);
50 }
51 let bits = x.to_bits();
52 let bits = if x < 0.0 { bits + 1 } else { bits - 1 };
53 f64::from_bits(bits)
54}
55
56#[inline]
62fn contains_periodic(lo: f64, hi: f64, target: f64, period: f64) -> bool {
63 let k = ((lo - target) / period).ceil();
64 target + k * period <= hi
65}
66
67fn interval_trig(lo: f64, hi: f64, f: impl Fn(f64) -> f64, max_at: f64, min_at: f64) -> (f64, f64) {
74 use std::f64::consts::TAU;
75 if !lo.is_finite() || !hi.is_finite() || hi - lo >= TAU {
76 return (-1.0, 1.0);
77 }
78 let (a, b) = (f(lo), f(hi));
79 let mut out_lo = next_down(a.min(b));
80 let mut out_hi = next_up(a.max(b));
81 if contains_periodic(lo, hi, max_at, TAU) {
82 out_hi = 1.0;
83 }
84 if contains_periodic(lo, hi, min_at, TAU) {
85 out_lo = -1.0;
86 }
87 (out_lo.max(-1.0), out_hi.min(1.0))
88}
89
90#[derive(Copy, Clone, Debug, Default, PartialEq)]
97pub struct Scalar {
98 pub lo: f64,
99 pub hi: f64,
100}
101
102impl Display for Scalar {
103 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
104 write!(f, "{:.3}", self.to_f64())
105 }
106}
107
108#[allow(
109 clippy::should_implement_trait,
110 reason = "the arithmetic is fallible (`div`) and spelled as `Number` spells it, so a residual reads the same over either"
111)]
112impl Scalar {
113 pub const ZERO: Self = Scalar { lo: 0.0, hi: 0.0 };
114 pub const ONE: Self = Scalar { lo: 1.0, hi: 1.0 };
115 pub const TWO: Self = Scalar { lo: 2.0, hi: 2.0 };
116
117 pub const PI: Self = Scalar {
119 lo: std::f64::consts::PI, hi: 3.1415926535897936_f64, };
122 pub const INFINITY: Self = Scalar {
124 lo: f64::INFINITY,
125 hi: f64::INFINITY,
126 };
127 pub const ENTIRE: Self = Scalar {
134 lo: f64::NEG_INFINITY,
135 hi: f64::INFINITY,
136 };
137
138 #[inline]
142 pub fn new(lo: f64, hi: f64) -> Self {
143 debug_assert!(lo <= hi, "Scalar::new: lo ({lo}) > hi ({hi})");
144 Scalar { lo, hi }
145 }
146
147 #[inline]
149 pub fn degenerate(v: f64) -> Self {
150 Scalar { lo: v, hi: v }
151 }
152
153 #[inline]
154 pub fn from_i64(v: i64) -> Self {
155 Scalar::degenerate(v as f64)
156 }
157
158 #[inline]
159 pub fn from_f64(v: f64) -> Self {
160 Scalar::degenerate(v)
161 }
162
163 #[inline]
164 pub fn from_ratio(num: i64, den: i64) -> GeopResult<Self> {
165 if den == 0 {
166 return Err(GeopError::new("Scalar::from_ratio: denominator is zero"));
167 }
168 let exact = num as f64 / den as f64;
169 Ok(Scalar::new(next_down(exact), next_up(exact)))
170 }
171
172 #[inline]
175 pub fn to_f64(self) -> f64 {
176 (self.lo + self.hi) / 2.0
177 }
178
179 #[inline]
182 pub fn add(self, other: Self) -> Self {
183 Scalar::new(next_down(self.lo + other.lo), next_up(self.hi + other.hi))
184 }
185
186 #[inline]
187 pub fn sub(self, other: Self) -> Self {
188 Scalar::new(next_down(self.lo - other.hi), next_up(self.hi - other.lo))
189 }
190
191 #[inline]
192 pub fn mul(self, other: Self) -> Self {
193 let products = [
194 self.lo * other.lo,
195 self.lo * other.hi,
196 self.hi * other.lo,
197 self.hi * other.hi,
198 ];
199 let lo = products.iter().cloned().fold(f64::INFINITY, f64::min);
200 let hi = products.iter().cloned().fold(f64::NEG_INFINITY, f64::max);
201 Scalar::new(next_down(lo), next_up(hi))
202 }
203
204 #[inline]
205 pub fn neg(self) -> Self {
206 Scalar::new(-self.hi, -self.lo)
207 }
208
209 #[inline]
211 pub fn div(self, other: Self) -> GeopResult<Self> {
212 if other.lo <= 0.0 && other.hi >= 0.0 {
213 return Err(GeopError::new(
214 "Scalar::div: divisor interval contains zero",
215 ));
216 }
217 let inv_lo = next_down(1.0 / other.hi);
219 let inv_hi = next_up(1.0 / other.lo);
220 let inv = Scalar::new(inv_lo, inv_hi);
221 Ok(self.mul(inv))
222 }
223
224 #[inline]
227 pub fn abs(self) -> Self {
228 if self.lo >= 0.0 {
229 self
230 } else if self.hi <= 0.0 {
231 Scalar::new(-self.hi, -self.lo)
232 } else {
233 Scalar::new(0.0, self.lo.abs().max(self.hi.abs()))
234 }
235 }
236
237 #[inline]
238 pub fn sqrt(self) -> GeopResult<Self> {
239 if self.hi < 0.0 {
240 return Err(GeopError::new(
241 "Scalar::sqrt: interval is definitely negative",
242 ));
243 }
244 let lo_clamped = if self.lo < 0.0 { 0.0 } else { self.lo };
245 Ok(Scalar::new(
246 next_down(lo_clamped.sqrt()),
247 next_up(self.hi.sqrt()),
248 ))
249 }
250
251 #[inline]
255 pub fn sin(self) -> Self {
256 use std::f64::consts::FRAC_PI_2;
257 let (lo, hi) = interval_trig(self.lo, self.hi, f64::sin, FRAC_PI_2, -FRAC_PI_2);
258 Scalar::new(lo, hi)
259 }
260
261 #[inline]
263 pub fn cos(self) -> Self {
264 use std::f64::consts::PI;
265 let (lo, hi) = interval_trig(self.lo, self.hi, f64::cos, 0.0, PI);
266 Scalar::new(lo, hi)
267 }
268
269 #[inline]
273 pub fn acos(self) -> GeopResult<Self> {
274 if self.hi < -1.0 || self.lo > 1.0 {
275 return Err(GeopError::new(format!(
276 "Scalar::acos: {self:?} lies outside [-1, 1]"
277 )));
278 }
279 let (lo, hi) = (self.lo.max(-1.0), self.hi.min(1.0));
282 Ok(Scalar::new(
283 next_down(hi.acos()).max(0.0),
284 next_up(lo.acos()).min(next_up(std::f64::consts::PI)),
285 ))
286 }
287
288 pub fn degrees_to_radians(self) -> GeopResult<Self> {
290 self.mul(Self::PI).div(Self::from_i64(180))
291 }
292
293 pub fn atan2(self, x: Self) -> Self {
308 let y = self;
309 let whole = Self::PI.neg().union(Self::PI);
310 if (y.could_be_equal(Self::ZERO) && x.could_be_equal(Self::ZERO))
313 || (x.could_be_less(Self::ZERO)
314 && y.could_be_less(Self::ZERO)
315 && !y.definitely_less(Self::ZERO))
316 {
317 return whole;
318 }
319 let ys = [y.lower().to_f64() + 0.0, y.upper().to_f64() + 0.0];
322 let xs = [x.lower().to_f64(), x.upper().to_f64()];
323 let corners = ys.iter().flat_map(|&a| xs.iter().map(move |&b| a.atan2(b)));
324 let (low, high) = corners.fold((f64::INFINITY, f64::NEG_INFINITY), |(l, h), a| {
325 (l.min(a), h.max(a))
326 });
327 let holds = |b: f64, sign: f64| {
330 let b = Self::from_f64(b);
331 let side = b.cos().mul(y).sub(b.sin().mul(x)).mul(Self::from_f64(sign));
332 !side.could_be_less(Self::ZERO)
333 };
334 let prove = |start: f64, sign: f64, limit: f64| -> Option<f64> {
335 let mut b = start;
336 let mut step = f64::EPSILON * start.abs().max(1.0);
337 for _ in 0..64 {
338 if holds(b, sign) {
339 return Some(b);
340 }
341 b -= sign * step;
342 step *= 2.0;
343 if (b - limit) * sign < 0.0 {
344 return None;
345 }
346 }
347 None
348 };
349 let pi = std::f64::consts::PI;
350 match (prove(low, 1.0, -pi - 1.0), prove(high, -1.0, pi + 1.0)) {
351 (Some(lo), Some(hi)) => Self::from_f64(lo)
352 .union(Self::from_f64(hi))
353 .intersect(whole),
354 _ => whole,
355 }
356 }
357
358 #[inline]
361 pub fn could_be_equal(self, other: Self) -> bool {
362 self.lo <= other.hi && other.lo <= self.hi
363 }
364
365 #[inline]
366 pub fn definitely_not_equal(self, other: Self) -> bool {
367 self.hi < other.lo || self.lo > other.hi
368 }
369
370 #[inline]
371 pub fn could_be_greater(self, other: Self) -> bool {
372 self.hi > other.lo
373 }
374
375 #[inline]
376 pub fn definitely_greater(self, other: Self) -> bool {
377 self.lo > other.hi
378 }
379
380 #[inline]
381 pub fn could_be_less(self, other: Self) -> bool {
382 self.lo < other.hi
383 }
384
385 #[inline]
386 pub fn definitely_less(self, other: Self) -> bool {
387 self.hi < other.lo
388 }
389
390 pub fn is_zero(self) -> bool {
394 !self.could_be_less(Self::ZERO) && !self.could_be_greater(Self::ZERO)
395 }
396
397 pub fn definite_sign(self) -> Option<bool> {
401 if self.definitely_greater(Self::ZERO) {
402 Some(true)
403 } else if self.definitely_less(Self::ZERO) {
404 Some(false)
405 } else {
406 None
407 }
408 }
409
410 #[inline]
413 pub fn is_infinite(self) -> bool {
414 self.lo == f64::NEG_INFINITY || self.hi == f64::INFINITY
415 }
416
417 #[inline]
418 pub fn is_finite(self) -> bool {
419 self.lo.is_finite() && self.hi.is_finite()
420 }
421
422 #[inline]
425 pub fn midpoint(self) -> Self {
426 let m = (self.lo + self.hi) / 2.0;
427 Scalar::degenerate(m)
428 }
429
430 #[inline]
433 pub fn is_sharp(self) -> bool {
434 self.lo == self.hi
435 }
436
437 #[inline]
445 pub fn width(self) -> Self {
446 let w = (self.hi - self.lo).max(0.0);
448 Scalar::new(w, w)
449 }
450
451 #[inline]
457 pub fn lower(self) -> Self {
458 Self::degenerate(self.lo)
459 }
460
461 #[inline]
463 pub fn upper(self) -> Self {
464 Self::degenerate(self.hi)
465 }
466
467 pub fn sharpen(self) -> Self {
485 self.midpoint()
486 }
487
488 pub fn fraction(lo: Self, hi: Self, f: Self) -> Self {
492 lo.add(hi.sub(lo).mul(f)).sharpen()
493 }
494
495 pub fn interpolate(a: Self, b: Self, alpha: Self) -> Self {
509 if a.is_exactly(b) && alpha.lo >= 0.0 && alpha.hi <= 1.0 {
538 return a;
539 }
540 let via_delta = a.add(alpha.mul(b.sub(a)));
541 let via_weights = Self::ONE.sub(alpha).mul(a).add(alpha.mul(b));
542 via_delta.intersect(via_weights)
543 }
544
545 #[inline]
552 pub fn intersect(self, other: Self) -> Self {
553 let lo = self.lo.max(other.lo);
554 let hi = self.hi.min(other.hi);
555 if lo <= hi {
556 Scalar::new(lo, hi)
557 } else if self.hi - self.lo <= other.hi - other.lo {
558 self
559 } else {
560 other
561 }
562 }
563
564 #[inline]
567 pub fn union(self, other: Self) -> Self {
568 Self::new(self.lo.min(other.lo), self.hi.max(other.hi))
569 }
570
571 #[inline]
577 pub fn is_subset_of(self, other: Self) -> bool {
578 other.lo <= self.lo && self.hi <= other.hi
579 }
580
581 pub fn is_exactly(self, other: Self) -> bool {
586 self.is_subset_of(other) && other.is_subset_of(self)
587 }
588
589 pub fn max(self, other: Self) -> Self {
592 let pick = |a: Self, b: Self| if b.definitely_greater(a) { b } else { a };
593 pick(self.lower(), other.lower()).union(pick(self.upper(), other.upper()))
594 }
595
596 pub fn min(self, other: Self) -> Self {
598 self.neg().max(other.neg()).neg()
599 }
600
601 pub fn clamped(self, lo: Self, hi: Self) -> Self {
604 if self.definitely_less(lo) {
605 lo
606 } else if self.definitely_greater(hi) {
607 hi
608 } else {
609 self
610 }
611 }
612}
613
614pub mod as_f64 {
621 use super::Scalar;
622 use serde::{Deserialize, Deserializer, Serializer};
623
624 pub fn serialize<Ser: Serializer>(x: &Scalar, serializer: Ser) -> Result<Ser::Ok, Ser::Error> {
625 serializer.serialize_f64(x.to_f64())
626 }
627
628 pub fn deserialize<'de, D: Deserializer<'de>>(deserializer: D) -> Result<Scalar, D::Error> {
629 f64::deserialize(deserializer).map(Scalar::from_f64)
630 }
631
632 pub mod vec {
635 use super::super::Scalar;
636 use serde::{Deserialize, Deserializer, Serializer, ser::SerializeSeq};
637
638 pub fn serialize<Ser: Serializer>(
639 xs: &[Scalar],
640 serializer: Ser,
641 ) -> Result<Ser::Ok, Ser::Error> {
642 let mut seq = serializer.serialize_seq(Some(xs.len()))?;
643 for x in xs {
644 seq.serialize_element(&x.to_f64())?;
645 }
646 seq.end()
647 }
648
649 pub fn deserialize<'de, D: Deserializer<'de>>(
650 deserializer: D,
651 ) -> Result<Vec<Scalar>, D::Error> {
652 Vec::<f64>::deserialize(deserializer)
653 .map(|xs| xs.into_iter().map(Scalar::from_f64).collect())
654 }
655 }
656
657 pub mod option {
660 use super::super::Scalar;
661 use serde::{Deserialize, Deserializer, Serializer};
662
663 pub fn serialize<Ser: Serializer>(
664 x: &Option<Scalar>,
665 serializer: Ser,
666 ) -> Result<Ser::Ok, Ser::Error> {
667 match x {
668 Some(x) => serializer.serialize_some(&x.to_f64()),
669 None => serializer.serialize_none(),
670 }
671 }
672
673 pub fn deserialize<'de, D: Deserializer<'de>>(
674 deserializer: D,
675 ) -> Result<Option<Scalar>, D::Error> {
676 Option::<f64>::deserialize(deserializer).map(|x| x.map(Scalar::from_f64))
677 }
678 }
679}
680
681#[cfg(test)]
684mod tests {
685 use super::*;
686
687 fn pt(v: f64) -> Scalar {
688 Scalar::degenerate(v)
689 }
690
691 fn iv(lo: f64, hi: f64) -> Scalar {
692 Scalar::new(lo, hi)
693 }
694
695 #[test]
696 fn arithmetic_add() {
697 let r = pt(1.0).add(pt(2.0));
698 assert!(r.could_be_equal(pt(3.0)));
699 }
700
701 #[test]
702 fn arithmetic_sub() {
703 let r = pt(5.0).sub(pt(3.0));
704 assert!(r.could_be_equal(pt(2.0)));
705 }
706
707 #[test]
708 fn arithmetic_mul() {
709 let r = pt(3.0).mul(pt(4.0));
710 assert!(r.could_be_equal(pt(12.0)));
711 }
712
713 #[test]
714 fn arithmetic_div() {
715 let r = pt(10.0).div(pt(2.0)).unwrap();
716 assert!(r.could_be_equal(pt(5.0)));
717 }
718
719 #[test]
720 fn arithmetic_sqrt_4() {
721 let r = pt(4.0).sqrt().unwrap();
722 assert!(r.could_be_equal(pt(2.0)));
723 }
724
725 #[test]
726 fn arithmetic_sqrt_9() {
727 let r = pt(9.0).sqrt().unwrap();
728 assert!(r.could_be_equal(pt(3.0)));
729 }
730
731 #[test]
732 fn arithmetic_abs() {
733 assert!(pt(-3.0).abs().could_be_equal(pt(3.0)));
734 }
735
736 #[test]
737 fn arithmetic_neg() {
738 assert!(pt(-5.0).neg().could_be_equal(pt(5.0)));
739 }
740
741 #[test]
742 fn sin_at_a_point() {
743 assert!(pt(0.0).sin().could_be_equal(pt(0.0)));
744 assert!(
745 pt(std::f64::consts::FRAC_PI_2)
746 .sin()
747 .could_be_equal(pt(1.0))
748 );
749 }
750
751 #[test]
752 fn cos_at_a_point() {
753 assert!(pt(0.0).cos().could_be_equal(pt(1.0)));
754 }
755
756 #[test]
757 fn sin_over_a_peak_reaches_exactly_one() {
758 let r = iv(0.0, std::f64::consts::PI).sin();
762 assert!(r.hi == 1.0 && r.lo >= -1e-15);
763 }
764
765 #[test]
766 fn cos_over_a_full_turn_is_entire_range() {
767 let r = iv(0.0, std::f64::consts::TAU).cos();
768 assert!(r.lo == -1.0 && r.hi == 1.0);
769 }
770
771 #[test]
772 fn sin_narrow_interval_encloses_true_value() {
773 let r = iv(0.5 - 1e-9, 0.5 + 1e-9).sin();
775 let truth = 0.5_f64.sin();
776 assert!(r.lo <= truth && truth <= r.hi);
777 }
778
779 #[test]
780 fn err_div_by_zero() {
781 assert!(pt(1.0).div(Scalar::ZERO).is_err());
782 }
783
784 #[test]
785 fn err_sqrt_negative() {
786 assert!(pt(-1.0).sqrt().is_err());
787 }
788
789 #[test]
790 fn sqrt_straddles_zero_ok() {
791 let r = iv(-1.0, 4.0).sqrt().unwrap();
793 assert!(r.could_be_equal(pt(2.0)));
794 }
795
796 #[test]
797 fn acos_encloses_the_angle() {
798 let r = iv(-0.5, 0.5).acos().unwrap();
799 assert!(r.could_be_equal(pt(std::f64::consts::FRAC_PI_2)));
800 assert!(r.lo <= std::f64::consts::FRAC_PI_3 && r.hi >= 2.0 * std::f64::consts::FRAC_PI_3);
801 assert!(pt(-1.0).acos().unwrap().could_be_equal(Scalar::PI));
802 assert!(pt(1.5).acos().is_err());
803 }
804
805 #[test]
806 fn overflow_infinity() {
807 let big = pt(f64::MAX);
808 let result = big.mul(big);
809 assert!(result.is_infinite());
810 }
811
812 #[test]
813 fn cmp_definitely_greater() {
814 assert!(pt(3.0).definitely_greater(pt(2.0)));
815 }
816
817 #[test]
818 fn cmp_could_be_less_false() {
819 assert!(!pt(3.0).definitely_less(pt(2.0)));
820 }
821
822 #[test]
823 fn cmp_overlapping_intervals_could_be_equal() {
824 assert!(iv(1.0, 3.0).could_be_equal(iv(2.0, 4.0)));
825 }
826
827 #[test]
828 fn cmp_disjoint_intervals_definitely_not_equal() {
829 assert!(iv(1.0, 2.0).definitely_not_equal(iv(3.0, 4.0)));
830 }
831
832 #[test]
833 fn entire_could_be_equal_point() {
834 assert!(Scalar::ENTIRE.could_be_equal(pt(42.0)));
835 assert!(pt(-1e300).could_be_equal(Scalar::ENTIRE));
836 }
837
838 #[test]
839 fn entire_could_be_equal_interval() {
840 assert!(Scalar::ENTIRE.could_be_equal(iv(-5.0, 5.0)));
841 }
842
843 #[test]
844 fn entire_could_be_greater_and_less() {
845 assert!(Scalar::ENTIRE.could_be_greater(pt(1e300)));
846 assert!(Scalar::ENTIRE.could_be_less(pt(-1e300)));
847 }
848
849 #[test]
850 fn entire_never_definitely() {
851 assert!(!Scalar::ENTIRE.definitely_not_equal(pt(0.0)));
852 assert!(!Scalar::ENTIRE.definitely_greater(pt(1e300)));
853 assert!(!Scalar::ENTIRE.definitely_less(pt(-1e300)));
854 }
855
856 #[test]
857 fn fp_enclosure_01_plus_02() {
858 let a = iv(next_down(0.1), next_up(0.1));
860 let b = iv(next_down(0.2), next_up(0.2));
861 let c = iv(next_down(0.3), next_up(0.3));
862 let sum = a.add(b);
863 assert!(sum.could_be_equal(c));
864 }
865
866 #[test]
869 fn atan2_encloses_the_angle() {
870 let f = Scalar::from_f64;
871 for degrees in (-179..=180).step_by(7).chain([0, 90, 180, -90, 45]) {
872 let a = (degrees as f64).to_radians();
873 let angle = f(a.sin()).atan2(f(a.cos()));
874 assert!(angle.could_be_equal(f(a)), "{degrees}: {angle:?}");
875 assert!(angle.width().to_f64() < 1e-8, "{degrees}: {angle:?}");
876 }
877 let pi = Scalar::ZERO.atan2(f(-1.0));
879 assert!(pi.could_be_equal(Scalar::PI), "{pi:?}");
880 assert!(pi.width().to_f64() < 1e-8, "{pi:?}");
881 let across = f(-1e-3).union(f(1e-3)).atan2(f(-1.0));
883 assert!(across.could_be_equal(Scalar::PI) && across.could_be_equal(Scalar::PI.neg()));
884 let round = f(-1.0).union(f(1.0)).atan2(f(-1.0).union(f(1.0)));
885 assert!(round.could_be_equal(Scalar::PI.neg()) && round.could_be_equal(Scalar::PI));
886 let wide = f(1.0).union(f(2.0)).atan2(f(1.0).union(f(3.0)));
888 assert!(wide.could_be_equal(f(1.0f64.atan2(3.0))));
889 assert!(wide.could_be_equal(f(2.0f64.atan2(1.0))));
890 assert!(!wide.could_be_equal(f(0.3)) && !wide.could_be_equal(f(1.11)));
891 }
892}