cad_rs/src/math/mod.rs

pub mod eqn;

pub type Scalar = f64;

pub type Vec2 = nalgebra::Vector2<Scalar>;
pub type Point2 = nalgebra::Point2<Scalar>;

pub type Rot2 = nalgebra::UnitComplex<Scalar>;

pub trait Region<T> {
    fn full() -> Self;
    fn singleton(value: T) -> Self;

    fn nearest(&self, value: &T) -> Option<T>;
    fn contains(&self, value: &T) -> bool;
}

#[derive(Clone, Debug)]
pub enum Region1 {
    Empty,
    Singleton(Scalar),
    Range(Scalar, Scalar),
    Union(Box<Region1>, Box<Region1>),
    Full,
}

impl Region<Scalar> for Region1 {
    fn full() -> Self {
        Region1::Full
    }

    fn singleton(value: Scalar) -> Self {
        Region1::Singleton(value)
    }

    fn contains(&self, n: &Scalar) -> bool {
        use Region1::*;
        match self {
            Empty => false,
            Singleton(n1) => relative_eq!(n1, n),
            Range(l, u) => *l <= *n && *n <= *u,
            Union(r1, r2) => r1.contains(n) || r2.contains(n),
            Full => true,
        }
    }

    fn nearest(&self, s: &Scalar) -> Option<Scalar> {
        use Region1::*;
        match self {
            Empty => None,
            Full => Some(*s),
            Singleton(n) => Some(*n),
            Range(l, u) => match (l < s, s < u) {
                (true, true) => Some(*s),
                (true, false) => Some(*u),
                (false, true) => Some(*l),
                _ => None,
            },
            Union(r1, r2) => {
                let distance = |a: Scalar, b: Scalar| (a - b).abs();
                match (r1.nearest(s), r2.nearest(s)) {
                    (None, None) => None,
                    (Some(n), None) | (None, Some(n)) => Some(n),
                    (Some(n1), Some(n2)) => Some({
                        if distance(*s, n1) <= distance(*s, n2) {
                            n1
                        } else {
                            n2
                        }
                    }),
                }
            }
        }
    }
}

// line starting at start, point at angle dir, with range extent
// ie. start + (cos dir, sin dir) * t for t in extent
#[derive(Clone, Debug)]
pub struct Line2 {
    start: Point2,
    dir: Rot2,
    extent: Region1,
}

impl Line2 {
    pub fn new(start: Point2, dir: Rot2, extent: Region1) -> Self {
        Self { start, dir, extent }
    }

    pub fn evaluate(&self, t: Scalar) -> Point2 {
        self.start + self.dir * Vec2::new(t, 0.)
    }

    pub fn nearest(&self, p: &Point2) -> Point2 {
        // rotate angle 90 degrees
        let perp_dir = self.dir * Rot2::from_cos_sin_unchecked(0., 1.);
        let perp = Line2::new(*p, perp_dir, Region1::Full);
        if let Region2::Singleton(np) = self.intersect(&perp) {
            np
        } else {
            panic!("Line2::nearest not found!");
        }
    }

    pub fn intersect(&self, other: &Line2) -> Region2 {
        // if the two lines are parallel...
        let dirs = self.dir / other.dir;
        if relative_eq!(dirs.sin_angle(), 0.) {
            let starts = self.dir.to_rotation_matrix().inverse() * (other.start - self.start);
            return if relative_eq!(starts.y, 0.) {
                // and they are colinear
                Region2::Line(self.clone())
            } else {
                // they are parallel and never intersect
                Region2::Empty
            };
        }
        // TODO: respect extent
        let (a, b) = (self, other);
        let (a_0, a_v, b_0, b_v) = (a.start, a.dir, b.start, b.dir);
        let (a_c, a_s, b_c, b_s) = (
            a_v.cos_angle(),
            a_v.sin_angle(),
            b_v.cos_angle(),
            b_v.sin_angle(),
        );
        let t_b = (a_0.x * a_s - a_0.y * a_c + a_0.x * a_s + b_0.y * a_c) / (a_s * b_c - a_c * b_s);
        Region2::Singleton(b.evaluate(t_b))
    }
}

#[derive(Clone, Debug)]
pub enum Region2 {
    Empty,
    // single point at 0
    Singleton(Point2),
    Line(Line2),
    #[allow(dead_code)]
    Union(Box<Region2>, Box<Region2>),
    Full,
}

impl Region<Point2> for Region2 {
    fn full() -> Self {
        Region2::Full
    }

    fn singleton(value: Point2) -> Self {
        Region2::Singleton(value)
    }

    fn contains(&self, p: &Point2) -> bool {
        self.nearest(p).map_or(false, |n| relative_eq!(n, p))
    }

    fn nearest(&self, p: &Point2) -> Option<Point2> {
        use Region2::*;
        match self {
            Empty => None,
            Full => Some(*p),
            Singleton(n) => Some(*n),
            Line(line) => Some(line.nearest(p)),
            Union(r1, r2) => {
                use nalgebra::distance;
                match (r1.nearest(p), r2.nearest(p)) {
                    (None, None) => None,
                    (Some(n), None) | (None, Some(n)) => Some(n),
                    (Some(n1), Some(n2)) => Some({
                        if distance(p, &n1) <= distance(p, &n2) {
                            n1
                        } else {
                            n2
                        }
                    }),
                }
            }
        }
    }
}

impl Region2 {
    pub fn union(r1: Region2, r2: Region2) -> Region2 {
        use Region2::*;
        match (r1, r2) {
            (Empty, r) | (r, Empty) => r,
            (Full, _) | (_, Full) => Full,
            (r1, r2) => Union(Box::new(r1), Box::new(r2)),
        }
    }

    pub fn intersect(&self, other: &Region2) -> Region2 {
        use Region2::*;
        match (self, other) {
            (Empty, _) | (_, Empty) => Empty,
            (Full, r) | (r, Full) => r.clone(),
            (Singleton(n1), Singleton(n2)) => {
                if n1 == n2 {
                    Singleton(*n1)
                } else {
                    Empty
                }
            }
            (Singleton(n), o) | (o, Singleton(n)) => {
                if o.contains(n) {
                    Singleton(*n)
                } else {
                    Empty
                }
            }
            (Line(l1), Line(l2)) => l1.intersect(l2),
            (Union(un1, un2), o) | (o, Union(un1, un2)) => {
                Self::union(un1.intersect(o), un2.intersect(o))
            }
        }
    }
}