| use crate::{Error, Result}; |
| pub type DVec3 = [f64; 3]; |
| #[derive(Clone, Copy, Debug)] |
| pub struct Policy { |
| pub distance_epsilon: f64, |
| pub angular_epsilon: f64, |
| } |
| impl Default for Policy { |
| fn default() -> Self { |
| Self { |
| distance_epsilon: 1e-6, |
| angular_epsilon: 1e-10, |
| } |
| } |
| } |
| impl Policy { |
| pub fn validate(self) -> Result<Self> { |
| if !self.distance_epsilon.is_finite() |
| || self.distance_epsilon <= 0. |
| || !self.angular_epsilon.is_finite() |
| || self.angular_epsilon <= 0. |
| || self.angular_epsilon >= 1. |
| { |
| Err(Error::InvalidPolicy) |
| } else { |
| Ok(self) |
| } |
| } |
| } |
| pub fn add(a: DVec3, b: DVec3) -> DVec3 { |
| std::array::from_fn(|i| a[i] + b[i]) |
| } |
| pub fn sub(a: DVec3, b: DVec3) -> DVec3 { |
| std::array::from_fn(|i| a[i] - b[i]) |
| } |
| pub fn scale(a: DVec3, b: f64) -> DVec3 { |
| a.map(|x| x * b) |
| } |
| pub fn dot(a: DVec3, b: DVec3) -> f64 { |
| a[0] * b[0] + a[1] * b[1] + a[2] * b[2] |
| } |
| pub fn cross(a: DVec3, b: DVec3) -> DVec3 { |
| [ |
| a[1] * b[2] - a[2] * b[1], |
| a[2] * b[0] - a[0] * b[2], |
| a[0] * b[1] - a[1] * b[0], |
| ] |
| } |
| pub fn finite(v: DVec3) -> Result<DVec3> { |
| if v.iter().all(|x| x.is_finite()) { |
| Ok(v) |
| } else { |
| Err(Error::NonFinite) |
| } |
| } |
| pub fn length(v: DVec3) -> f64 { |
| v[0].hypot(v[1]).hypot(v[2]) |
| } |
| pub fn unit(v: DVec3) -> Result<DVec3> { |
| finite(v)?; |
| let n = length(v); |
| if !n.is_finite() { |
| return Err(Error::NonFinite); |
| } |
| if n == 0. { |
| return Err(Error::Degenerate); |
| } |
| finite(scale(v, 1. / n)) |
| } |
| pub fn from_f32(v: mb3_dat::Vec3) -> Result<DVec3> { |
| finite(v.map(f64::from)) |
| } |
| pub fn to_f32(v: DVec3) -> Result<mb3_dat::Vec3> { |
| finite(v)?; |
| let out = v.map(|n| n as f32); |
| if out.iter().all(|n| n.is_finite()) { |
| Ok(out) |
| } else { |
| Err(Error::FloatRange) |
| } |
| } |
| pub fn map_to_native(v: DVec3) -> DVec3 { |
| [v[0], v[2], -v[1]] |
| } |
| pub fn native_to_map(v: DVec3) -> DVec3 { |
| [v[0], -v[2], v[1]] |
| } |
| #[derive(Clone, Copy, Debug)] |
| pub struct Equation { |
| normal: DVec3, |
| d: f64, |
| } |
| impl Equation { |
| pub fn new(normal: DVec3, d: f64) -> Result<Self> { |
| let n = length(finite(normal)?); |
| if n == 0. { |
| return Err(Error::Degenerate); |
| } |
| if !n.is_finite() || !d.is_finite() { |
| return Err(Error::NonFinite); |
| } |
| let out = Self { |
| normal: scale(normal, 1. / n), |
| d: d / n, |
| }; |
| finite(out.normal)?; |
| if !out.d.is_finite() { |
| return Err(Error::NonFinite); |
| } |
| Ok(out) |
| } |
| pub fn from_basis(p: &mb3_dat::Plane, policy: Policy) -> Result<Self> { |
| let policy = policy.validate()?; |
| let origin = from_f32(p.p0)?; |
| let n = cross(unit(from_f32(p.u)?)?, unit(from_f32(p.v)?)?); |
| if length(n) <= policy.angular_epsilon { |
| return Err(Error::Degenerate); |
| } |
| let n = unit(n)?; |
| Self::new(n, dot(n, origin)) |
| } |
| pub fn normal(self) -> DVec3 { |
| self.normal |
| } |
| pub fn offset(self) -> f64 { |
| self.d |
| } |
| pub fn distance(self, p: DVec3) -> Result<f64> { |
| let d = dot(self.normal, finite(p)?) - self.d; |
| if d.is_finite() { |
| Ok(d) |
| } else { |
| Err(Error::NonFinite) |
| } |
| } |
| pub fn intersection(a: Self, b: Self, c: Self, policy: Policy) -> Result<Option<DVec3>> { |
| let policy = policy.validate()?; |
| let det = dot(a.normal, cross(b.normal, c.normal)); |
| if det.abs() <= policy.angular_epsilon { |
| return Ok(None); |
| } |
| finite(scale( |
| add( |
| add( |
| scale(cross(b.normal, c.normal), a.d), |
| scale(cross(c.normal, a.normal), b.d), |
| ), |
| scale(cross(a.normal, b.normal), c.d), |
| ), |
| 1. / det, |
| )) |
| .map(Some) |
| } |
| } |
| #[derive(Clone, Copy, Debug, PartialEq)] |
| pub struct Aabb { |
| pub min: DVec3, |
| pub max: DVec3, |
| } |
| impl Aabb { |
| pub fn new(min: DVec3, max: DVec3) -> Result<Self> { |
| finite(min)?; |
| finite(max)?; |
| if (0..3).any(|i| min[i] > max[i]) { |
| return Err(Error::InvalidDimensions); |
| } |
| Ok(Self { min, max }) |
| } |
| pub fn from_points(points: impl IntoIterator<Item = DVec3>) -> Result<Option<Self>> { |
| let mut out: Option<Self> = None; |
| for p in points { |
| finite(p)?; |
| if let Some(b) = &mut out { |
| for (i, value) in p.iter().enumerate() { |
| b.min[i] = b.min[i].min(*value); |
| b.max[i] = b.max[i].max(*value); |
| } |
| } else { |
| out = Some(Self { min: p, max: p }); |
| } |
| } |
| Ok(out) |
| } |
| pub fn contains(self, p: DVec3, policy: Policy) -> Result<bool> { |
| let e = policy.validate()?.distance_epsilon; |
| Self::new(self.min, self.max)?; |
| finite(p)?; |
| Ok((0..3).all(|i| p[i] >= self.min[i] - e && p[i] <= self.max[i] + e)) |
| } |
| } |
| #[cfg(test)] |
| mod tests { |
| use super::*; |
| #[test] |
| fn basis_orientation_and_intersections() { |
| let p = mb3_dat::Plane { |
| p0: [2., 0., 0.], |
| u: [0., 1., 0.], |
| v: [0., 0., 1.], |
| ..Default::default() |
| }; |
| let a = Equation::from_basis(&p, Policy::default()).unwrap(); |
| assert_eq!(a.normal(), [1., 0., 0.]); |
| assert_eq!(a.distance([3., 0., 0.]).unwrap(), 1.); |
| let b = Equation::new([0., 1., 0.], 3.).unwrap(); |
| let c = Equation::new([0., 0., 1.], 4.).unwrap(); |
| assert_eq!( |
| Equation::intersection(a, b, c, Policy::default()).unwrap(), |
| Some([2., 3., 4.]) |
| ); |
| assert!( |
| Equation::intersection(a, a, c, Policy::default()) |
| .unwrap() |
| .is_none() |
| ); |
| } |
| #[test] |
| fn boxes_axes_and_empty() { |
| assert_eq!(Aabb::from_points([]).unwrap(), None); |
| let b = Aabb::from_points([[-3., 2., 1.], [4., -1., 8.]]) |
| .unwrap() |
| .unwrap(); |
| assert_eq!(b.min, [-3., -1., 1.]); |
| assert!(b.contains([0., 0., 2.], Policy::default()).unwrap()); |
| let v = [1., 2., 3.]; |
| assert_eq!(native_to_map(map_to_native(v)), v); |
| } |
| #[test] |
| fn invalid_and_scaled_bases() { |
| assert!(unit([0.; 3]).is_err()); |
| assert!(Equation::new([f64::NAN, 0., 0.], 1.).is_err()); |
| assert!(to_f32([f64::MAX, 0., 0.]).is_err()); |
| assert!( |
| Policy { |
| distance_epsilon: 0., |
| ..Policy::default() |
| } |
| .validate() |
| .is_err() |
| ); |
| let p = mb3_dat::Plane { |
| u: [0., 1e-20, 0.], |
| v: [0., 0., 1e20], |
| ..Default::default() |
| }; |
| assert_eq!( |
| Equation::from_basis(&p, Policy::default()) |
| .unwrap() |
| .normal(), |
| [1., 0., 0.] |
| ); |
| } |
| } |
|
|
