lots of cool work

src/entity.rs

@@ -2,6 +2,7 @@ use std::cell::RefCell;
 use std::rc::Rc;
 
 use crate::math::{Point2, Region, Region1, Region2, Scalar};
+use std::fmt;
 
 #[derive(Clone, Copy, Debug)]
 pub struct Var<T: Clone, TRegion: Region<T>> {
@@ -9,6 +10,12 @@ pub struct Var<T: Clone, TRegion: Region<T>> {
     constraints: TRegion,
 }
 
+impl<T: Clone + fmt::Display, TRegion: Region<T> + fmt::Display> fmt::Display for Var<T, TRegion> {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "{{ {} ∈ {} }}", self.value, self.constraints)
+    }
+}
+
 impl<T: Clone, TRegion: Region<T>> Var<T, TRegion> {
     pub fn new(value: T, constraints: TRegion) -> Self {
         Self { value, constraints }
@@ -42,7 +49,7 @@ impl<T: Clone, TRegion: Region<T>> Var<T, TRegion> {
 }
 
 type ScalarVar = Var<Scalar, Region1>;
-type PointVar = Var<Point2, Region2>;
+type PointVar = Var<Point2<Scalar>, Region2>;
 
 #[derive(Debug)]
 pub struct Point {
@@ -58,6 +65,12 @@ impl Point {
     }
 }
 
+impl fmt::Display for Point {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "Point {{ pos: {} }}", self.pos)
+    }
+}
+
 struct Line {
     p1: PointRef,
     p2: PointRef,

src/main.rs

@@ -18,38 +18,38 @@ mod relation;
 
 fn main() {
     use entity::{Point, PointRef, Var};
-    use math::{Point2, eqn};
+    use math::{Point2, eqn, Region2};
     use relation::{Relation, ResolveResult};
 
     env_logger::init();
 
     println!("Hello, world!");
-    // let u1 = math::eqn::Unknown(1);
+    let u1 = math::eqn::Unknown(1);
-    // let u2 = math::eqn::Unknown(2);
+    let u2 = math::eqn::Unknown(2);
-    let u1 = eqn::Expr::from(1.);
+    // let u1 = eqn::Expr::from(1.);
-    let u2 = eqn::Expr::from(1.);
+    // let u2 = eqn::Expr::from(1.);
     let origin = Point::new_ref(Var::new_single(Point2::new((0.).into(), (0.).into())));
     // let p1 = Point::new_ref(Var::new_full(Point2::new((1.).into(), (1.).into())));
-    let p1 = Point::new_ref(Var::new_single(Point2::new((u1).into(), (u2).into())));
+    let p1 = Point::new_ref(Var::new(Point2::new(1.,1.), Region2::Singleton(Point2::new((u1).into(), (u2).into()))));
     let p2 = Point::new_ref(Var::new_full(Point2::new((4.).into(), (4.).into())));
     let p3 = Point::new_ref(Var::new_full(Point2::new((2.).into(), (2.).into())));
     let mut points: Vec<PointRef> = vec![origin.clone(), p1.clone(), p2.clone(), p3.clone()];
     let print_points = |points: &Vec<PointRef>| {
         println!(
-            "origin, p1, p2, p3:\n {:?}\n {:?}\n {:?}\n {:?}",
+            "origin, p1, p2, p3:\n {}\n {}\n {}\n {}",
-            points[0], points[1], points[2], points[3],
+            points[0].borrow(), points[1].borrow(), points[2].borrow(), points[3].borrow(),
         );
     };
     print_points(&points);
-    // let c1 = relation::Coincident {
+    let c1 = relation::Coincident {
-    //     p1: origin.clone(),
+        p1: origin.clone(),
-    //     p2: p1.clone(),
+        p2: p1.clone(),
-    // };
+    };
-    let c2 = relation::PointAngle::new_vertical(p1.clone(), p2.clone());
+    let c4 = relation::PointAngle::new_vertical(p1.clone(), p2.clone());
     let c3 = relation::PointAngle::new_horizontal(p2.clone(), p3.clone());
-    let c4 = relation::AlignedDistance::new_vertical(p1.clone(), p2.clone(), 12.);
+    let c2 = relation::AlignedDistance::new_vertical(p1.clone(), p2.clone(), 12.);
     let mut relations: Vec<Box<dyn Relation>> =
-        vec![/*Box::new(c1), */Box::new(c2), Box::new(c3), Box::new(c4)];
+        vec![/*Box::new(c1),*/ Box::new(c2), Box::new(c3), Box::new(c4)];
     let mut constrained: Vec<Box<dyn Relation>> = Vec::new();
     let mut any_underconstrained = true;
     let mut any_constrained = true;
@@ -60,17 +60,10 @@ fn main() {
         let newly_constrained = relations.drain_filter(|r| {
             let rr = r.resolve();
             println!("resolve result: {:?}", rr);
-            println!(
+            print_points(&points);
-                "origin, p1, p2, p3:\n {:?}\n {:?}\n {:?}\n {:?}",
-                origin, p1, p2, p3
-            );
             let mut pos = p2.borrow().pos.val().clone();
-            pos.x = pos.x.distribute().simplify();
-            pos.y = pos.y.distribute().simplify();
             println!("p2 pos: {}", pos);
             let mut pos = p3.borrow().pos.val().clone();
-            pos.x = pos.x.distribute().simplify();
-            pos.y = pos.y.distribute().simplify();
             println!("p3 pos: {}", pos);
             match rr {
                 ResolveResult::Underconstrained => {

src/math/eqn.rs

@@ -147,11 +147,11 @@ fn group_sum(es: Exprs) -> Exprs {
 }
 
 fn product_fold(l: Expr, r: Expr) -> Expr {
-    use itertools::Itertools;
     use Expr::*;
     match (l, r) {
         (Const(lc), Const(rc)) => Const(lc * rc),
         (Const(c), o) | (o, Const(c)) if relative_eq!(c, 1.) => o,
+        (Const(c), _) | (_, Const(c)) if relative_eq!(c, 0.) => Const(0.),
         (Div(num, den), mul) | (mul, Div(num, den)) => {
             if mul == *den {
                 *num
@@ -165,30 +165,34 @@ fn product_fold(l: Expr, r: Expr) -> Expr {
 
 fn group_product(es: Exprs) -> Exprs {
     use Expr::*;
-    let mut common: BTreeMap<UnknownSet, Expr> = BTreeMap::new();
+    // let mut common: BTreeMap<UnknownSet, Expr> = BTreeMap::new();
+    let mut common: Option<Expr> = None;
     for e in es {
         let unkns = e.unknowns();
-        match common.get_mut(&unkns) {
+        // match common.get_mut(&unkns) {
+        match &mut common {
             None => {
                 match e {
                     Const(c) if relative_eq!(c, 1.) => (),
                     e => {
-                        common.insert(unkns, e);
+                        // common.insert(unkns, e);
+                        common = Some(e);
                     }
                 };
             }
             Some(existing) => {
                 match existing {
-                    Sum(ref mut es) => {
+                    // Product(ref mut es) => {
                         // already failed at merging, so just add it to the list
-                        es.push(e);
+                        // es.push(e);
-                    }
+                    // }
                     other => *other = product_fold(other.clone(), e),
                 };
             }
         };
     }
-    common.into_iter().map(|(_, v)| v).collect()
+    // common.into_iter().map(|(_, v)| v).collect()
+    common.into_iter().collect()
 }
 
 fn distribute_product_sums(mut es: Exprs) -> Expr {
@@ -276,22 +280,55 @@ impl Expr {
         Expr::new_div(Expr::Const(1.), den)
     }
 
-    pub fn is_zero(&self) -> bool {
+    pub fn is_zero(self) -> bool {
         use Expr::*;
-        match self {
+        match self.simplify() {
-            Const(c) => relative_eq!(*c, 0.),
+            Const(c) => relative_eq!(c, 0.),
             _ => false,
         }
     }
 
-    pub fn is_one(&self) -> bool {
+    pub fn is_one(self) -> bool {
         use Expr::*;
-        match self {
+        match self.simplify() {
-            Const(c) => relative_eq!(*c, 1.),
+            Const(c) => relative_eq!(c, 1.),
             _ => false,
         }
     }
 
+    pub fn evaluate_with(self, eqns: &Eqns) -> Expr {
+        use Expr::*;
+        for eqn in &eqns.0 {
+            if self == eqn.0 {
+                return eqn.1.clone();
+            }
+        }
+        match self {
+            Sum(mut es) => {
+                for e in &mut es {
+                    *e = e.clone().evaluate_with(eqns);
+                }
+                Sum(es)
+            }
+            Product(mut es) => {
+                for e in &mut es {
+                    *e = e.clone().evaluate_with(eqns);
+                }
+                Product(es)
+            }
+            Neg(mut e) => {
+                *e = e.evaluate_with(eqns);
+                Neg(e)
+            }
+            Div(mut num, mut den) => {
+                *num = num.evaluate_with(eqns);
+                *den = den.evaluate_with(eqns);
+                Div(num, den)
+            }
+            other => other,
+        }
+    }
+
     pub fn simplify(self) -> Expr {
         use Expr::*;
         match self {

src/math/mod.rs

@@ -1,8 +1,12 @@
+use std::fmt;
+
 pub mod eqn;
 pub mod ops;
+pub mod vec;
 
-pub use eqn::Unknown;
+pub use eqn::{Expr, Unknown};
 pub use ops::*;
+pub use vec::*;
 
 pub type Scalar = f64;
 // #[derive(Clone, Copy, PartialEq, Debug)]
@@ -14,236 +18,110 @@ pub type Value = eqn::Expr;
 
 // pub type Vec2 = nalgebra::Vector2<Value>;
 // pub type Point2 = nalgebra::Point2<Value>;
-#[derive(Clone, PartialEq, Debug)]
-pub struct Vec2 {
-    pub x: Value,
-    pub y: Value,
-}
-
-impl Vec2 {
-    pub fn new(x: Value, y: Value) -> Self {
-        Self { x, y }
-    }
-}
-
-impl std::ops::Add<Vec2> for Vec2 {
-    type Output = Vec2;
-    fn add(self, rhs: Vec2) -> Vec2 {
-        Vec2 {
-            x: self.x + rhs.x,
-            y: self.y + rhs.y,
-        }
-    }
-}
 
-#[derive(Clone, PartialEq, Debug)]
+// pub type Rot2 = nalgebra::UnitComplex<Value>;
-pub struct Point2 {
-    pub x: Value,
-    pub y: Value,
-}
-
-impl Point2 {
-    pub fn new(x: Value, y: Value) -> Point2 {
-        Point2 { x, y }
-    }
-}
 
-impl std::ops::Add<Vec2> for Point2 {
+pub trait Region<T> {
-    type Output = Point2;
+    fn full() -> Self;
-    fn add(self, rhs: Vec2) -> Point2 {
+    fn singleton(value: T) -> Self;
-        Point2 {
+    // fn intersection(self, other: Self) -> Self;
-            x: self.x + rhs.x,
-            y: self.y + rhs.y,
-        }
-    }
-}
 
-impl std::ops::Sub<Vec2> for Point2 {
+    fn nearest(&self, value: &T) -> Option<T>;
-    type Output = Point2;
+    fn contains(&self, value: &T) -> Option<bool>;
-    fn sub(self, rhs: Vec2) -> Point2 {
-        Point2 {
-            x: self.x - rhs.x,
-            y: self.y - rhs.y,
-        }
-    }
 }
 
-impl std::ops::Sub<Point2> for Point2 {
+#[derive(Clone, Debug)]
-    type Output = Vec2;
+pub enum Region1 {
-    fn sub(self, rhs: Point2) -> Vec2 {
+    Empty,
-        Vec2 {
+    Singleton(Value),
-            x: self.x - rhs.x,
+    Range(Value, Value),
-            y: self.y - rhs.y,
+    Intersection(Box<Region1>, Box<Region1>),
-        }
+    // Union(Box<Region1>, Box<Region1>),
-    }
+    Full,
 }
 
-use std::fmt;
+impl fmt::Display for Region1 {
-
-impl fmt::Display for Point2 {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "({}, {})", self.x, self.y)
+        use Region1::*;
-    }
+        match self {
-}
+            Empty => write!(f, "Ø"),
-
+            Singleton(v) => write!(f, "{{ {} }}", v),
-#[derive(Clone, PartialEq, Debug)]
+            Range(l, u) => write!(f, "[ {}, {} ]", l, u),
-pub struct Rot2 {
+            Intersection(r1, r2) => write!(f, "{} ∩ {}", r1, r2),
-    cos: Value,
+            Full => write!(f, "ℝ")
-    sin: Value,
-}
-
-impl Rot2 {
-    pub fn from_cos_sin_unchecked(cos: Value, sin: Value) -> Self {
-        Self { cos, sin }
-    }
-
-    pub fn from_angle(angle: Scalar) -> Self {
-        Self {
-            cos: angle.cos().into(),
-            sin: angle.sin().into(),
-        }
-    }
-
-    pub fn cardinal(index: i64) -> Self {
-        match index % 4 {
-            0 => Rot2 {
-                cos: (1.).into(),
-                sin: (0.).into(),
-            },
-            1 => Rot2 {
-                cos: (0.).into(),
-                sin: (1.).into(),
-            },
-            2 => Rot2 {
-                cos: (-1.).into(),
-                sin: (0.).into(),
-            },
-            3 => Rot2 {
-                cos: (0.).into(),
-                sin: (-1.).into(),
-            },
-            _ => unreachable!(),
-        }
-    }
-
-    pub fn cos(&self) -> &Value {
-        &self.cos
-    }
-
-    pub fn sin(&self) -> &Value {
-        &self.sin
-    }
-
-    pub fn conj(self) -> Self {
-        Self {
-            cos: self.cos,
-            sin: -self.sin,
-        }
-    }
-}
-
-impl std::ops::Mul<Scalar> for Rot2 {
-    type Output = Vec2;
-    fn mul(self, rhs: Scalar) -> Vec2 {
-        Vec2 {
-            x: self.cos * rhs,
-            y: self.sin * rhs,
-        }
-    }
-}
-
-impl std::ops::Mul<Value> for Rot2 {
-    type Output = Vec2;
-    fn mul(self, rhs: Value) -> Vec2 {
-        Vec2 {
-            x: self.cos * rhs.clone(),
-            y: self.sin * rhs,
-        }
-    }
-}
-
-impl std::ops::Add<Rot2> for Rot2 {
-    type Output = Rot2;
-    fn add(self, rhs: Rot2) -> Rot2 {
-        Rot2 {
-            cos: self.cos.clone() * rhs.cos.clone() - self.sin.clone() * rhs.sin.clone(),
-            sin: self.cos * rhs.sin + self.sin * rhs.cos,
         }
     }
 }
 
-impl std::ops::Sub<Rot2> for Rot2 {
+impl Region1 {
-    type Output = Rot2;
+    pub fn intersection(self, other: Region1) -> Self {
-    fn sub(self, rhs: Rot2) -> Rot2 {
+        Region1::Intersection(Box::new(self), Box::new(other))
-        Rot2 {
-            cos: self.cos.clone() * rhs.cos.clone() + self.sin.clone() * rhs.sin.clone(),
-            sin: self.sin * rhs.cos - self.cos * rhs.sin,
-        }
     }
-}
 
-impl std::ops::Mul<Vec2> for Rot2 {
+    pub fn simplify(self) -> Self {
-    type Output = Vec2;
+        use Region1::*;
-    fn mul(self, rhs: Vec2) -> Vec2 {
+        match self {
-        Vec2 {
+            Singleton(n) => Singleton(n.simplify()),
-            x: self.cos.clone() * rhs.x.clone() - self.sin.clone() * rhs.y.clone(),
+            Range(l, u) => Range(l.simplify(), u.simplify()),
-            y: self.cos * rhs.y + self.sin * rhs.x,
+            Intersection(r1, r2) => r1.simplify().intersection(r2.simplify()),
+            other => other,
         }
     }
 }
 
-// pub type Rot2 = nalgebra::UnitComplex<Value>;
-
-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)
+        Region1::Singleton(value.into())
     }
 
-    fn contains(&self, n: &Scalar) -> bool {
+    fn contains(&self, n: &Scalar) -> Option<bool> {
+        use Expr::Const;
         use Region1::*;
         match self {
-            Empty => false,
+            Empty => Some(false),
-            Singleton(n1) => relative_eq!(n1, n),
+            Singleton(n1) => match n1 {
-            Range(l, u) => *l <= *n && *n <= *u,
+                Const(c) => Some(relative_eq!(c, n)),
-            Union(r1, r2) => r1.contains(n) || r2.contains(n),
+                _ => None,
-            Full => true,
+            },
+            Range(l, u) => match (l, u) {
+                (Const(cl), Const(cu)) => Some(*cl <= *n && *n <= *cu),
+                _ => None,
+            },
+            Intersection(r1, r2) => r1
+                .contains(n)
+                .and_then(|c1| r2.contains(n).map(|c2| c1 && c2)),
+            // Union(r1, r2) => r1.contains(n) || r2.contains(n),
+            Full => Some(true),
         }
     }
 
     fn nearest(&self, s: &Scalar) -> Option<Scalar> {
+        use Expr::Const;
         use Region1::*;
         match self {
             Empty => None,
             Full => Some(*s),
-            Singleton(n) => Some(*n),
+            Singleton(n) => match n {
-            Range(l, u) => match (l < s, s < u) {
+                Const(c) => Some(*c),
-                (true, true) => Some(*s),
-                (true, false) => Some(*u),
-                (false, true) => Some(*l),
                 _ => None,
             },
-            Union(r1, r2) => {
+            Range(l, u) => match (l, u) {
+                (Const(cl), Const(cu)) => match (cl < s, s < cu) {
+                    (true, true) => Some(*s),
+                    (true, false) => Some(*cu),
+                    (false, true) => Some(*cl),
+                    _ => None,
+                },
+                _ => None,
+            },
+            Intersection(r1, r2) => {
+                unimplemented!()
+            }
+            /*Union(r1, r2) => {
                 let distance = |a: Scalar, b: Scalar| (a - b).abs();
                 match (r1.nearest(s), r2.nearest(s)) {
                     (None, None) => None,
@@ -256,7 +134,7 @@ impl Region<Scalar> for Region1 {
                         }
                     }),
                 }
-            }
+            }*/
         }
     }
 }
@@ -265,54 +143,91 @@ impl Region<Scalar> for Region1 {
 // ie. start + (cos dir, sin dir) * t for t in extent
 #[derive(Clone, Debug)]
 pub struct Line2 {
-    start: Point2,
+    start: Point2<Value>,
     dir: Rot2,
     extent: Region1,
 }
 
+impl fmt::Display for Line2 {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "{{ (x, y) = {} + {} * {} }}", self.start, self.dir, self.extent)
+    }
+}
+
 impl Line2 {
-    pub fn new(start: Point2, dir: Rot2, extent: Region1) -> Self {
+    pub fn new(start: Point2<Value>, dir: Rot2, extent: Region1) -> Self {
         Self { start, dir, extent }
     }
 
-    pub fn evaluate(&self, t: Value) -> Point2 {
+    pub fn evaluate(&self, t: Value) -> Point2<Value> {
         self.start.clone() + self.dir.clone() * t
     }
 
-    pub fn nearest(&self, p: &Point2) -> Point2 {
+    pub fn evaluate_extent(&self) -> Option<Point2<Value>> {
+        match &self.extent {
+            Region1::Singleton(t) => Some(self.evaluate(t.clone())),
+            _ => None,
+        }
+    }
+    
+    pub fn with_extent(self, new_extent: Region1) -> Line2 {
+        Line2 { start: self.start, dir: self.dir, extent: new_extent }
+    }
+
+    pub fn nearest(&self, p: &Point2<Value>) -> Point2<Value> {
         // rotate angle 90 degrees
         let perp_dir = self.dir.clone() + Rot2::cardinal(1);
         let perp = Line2::new(p.clone(), perp_dir, Region1::Full);
-        if let Region2::Singleton(np) = self.intersect(&perp) {
+        match self.intersect(&perp) {
-            np
+            Region2::Singleton(np) => np,
-        } else {
+            Region2::Line(l) => l.evaluate_extent().expect("Line2::nearest not found"),
-            panic!("Line2::nearest not found!");
+            _ => panic!("Line2::nearest not found!")
         }
     }
 
     pub fn intersect(&self, other: &Line2) -> Region2 {
         // if the two lines are parallel...
         let dirs = self.dir.clone() - other.dir.clone();
-        /*
         if relative_eq!(dirs.sin(), 0.) {
-            let starts = self.dir.to_rotation_matrix().inverse() * (other.start - self.start);
+            let starts = self.dir.conj() * (other.start.clone() - self.start.clone());
-            return if relative_eq!(starts.y, 0.) {
+            return if starts.y.simplify().is_zero() {
                 // 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.clone(), a.dir.clone(), b.start.clone(), b.dir.clone());
+        let (a_0, a_v, b_0, b_v) = (
+            a.start.clone(),
+            a.dir.clone(),
+            b.start.clone(),
+            b.dir.clone(),
+        );
         let (a_c, a_s, b_c, b_s) = (a_v.cos(), a_v.sin(), b_v.cos(), b_v.sin());
         let t_b = (a_0.x.clone() * a_s.clone() - a_0.y.clone() * a_c.clone()
-            + a_0.x.clone() * a_s.clone()
+            - b_0.x.clone() * a_s.clone()
             + b_0.y.clone() * a_c.clone())
             / (a_s.clone() * b_c.clone() - a_c.clone() * b_s.clone());
-        Region2::Singleton(b.evaluate(t_b))
+        // Region2::Singleton(b.evaluate(t_b))
+        let res = Region2::Line(b.clone().with_extent(Region1::Singleton(t_b.simplify())));
+        trace!("intersect a: {}, b: {} = {}", a, b, res);
+        res
+    }
+
+    pub fn simplify(self) -> Region2 {
+        let new_l = Line2 {
+            start: self.start.simplify(),
+            dir: self.dir,
+            extent: self.extent.simplify(),
+        };
+        trace!("line {}: simplify evaluate extent: {:?}", new_l, new_l.evaluate_extent());
+        if let Some(p) = new_l.evaluate_extent() {
+            return Region2::Singleton(p.simplify());
+        }
+        Region2::Line(new_l)
     }
 }
 
@@ -320,53 +235,133 @@ impl Line2 {
 pub enum Region2 {
     Empty,
     // single point at 0
-    Singleton(Point2),
+    Singleton(Point2<Value>),
     Line(Line2),
-    #[allow(dead_code)]
+    // #[allow(dead_code)]
-    Union(Box<Region2>, Box<Region2>),
+    // Union(Box<Region2>, Box<Region2>),
+    Intersection(Box<Region2>, Box<Region2>),
     Full,
 }
 
-impl Region<Point2> for Region2 {
+impl fmt::Display for Region2 {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        use Region2::*;
+        match self {
+            Empty => write!(f, "ز"),
+            Singleton(v) => write!(f, "{{ {} }}", v),
+            Line(l) => l.fmt(f),
+            Intersection(r1, r2) => write!(f, "{} ∩ {}", r1, r2),
+            Full => write!(f, "ℝ²")
+        }
+    }
+}
+
+impl Region<Point2<Scalar>> for Region2 {
     fn full() -> Self {
         Region2::Full
     }
 
-    fn singleton(value: Point2) -> Self {
+    fn singleton(value: Point2<Scalar>) -> Self {
+        Region2::Singleton(value.into())
+    }
+
+    fn contains(&self, p: &Point2<Scalar>) -> Option<bool> {
+        self.nearest(p).map(|n| n == *p)
+    }
+
+    fn nearest(&self, p: &Point2<Scalar>) -> Option<Point2<Scalar>> {
+        use Expr::Const;
+        use Region2::*;
+        match self {
+            Empty => None,
+            Full => Some(p.clone()),
+            Singleton(n) => match (&n.x, &n.y) {
+                (Const(cx), Const(cy)) => Some(Point2::new(*cx, *cy)),
+                _ => None,
+            },
+            Line(line) => {
+                let pv: Point2<Value> = p.clone().into();
+                let n = line.nearest(&pv).simplify();
+                trace!("line {} nearest to {}: {}", line, pv, n);
+                match (n.x, n.y) {
+                    (Const(cx), Const(cy)) => Some(Point2::new(cx, cy)),
+                    _ => None,
+                }
+            } 
+            Intersection(r1, r2) => {
+                None
+                // r1.clone().intersect((**r2).clone()).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 Region<Point2<Value>> for Region2 {
+    fn full() -> Self {
+        Region2::Full
+    }
+
+    fn singleton(value: Point2<Value>) -> Self {
         Region2::Singleton(value)
     }
 
-    fn contains(&self, p: &Point2) -> bool {
+    fn contains(&self, p: &Point2<Value>) -> Option<bool> {
-        self.nearest(p)
+        self.nearest(p).map(|n| n.simplify() == p.clone().simplify())
-            .map_or(false, |n| true /*relative_eq!(n, p)*/)
     }
 
-    fn nearest(&self, p: &Point2) -> Option<Point2> {
+    fn nearest(&self, p: &Point2<Value>) -> Option<Point2<Value>> {
         use Region2::*;
         match self {
             Empty => None,
             Full => Some(p.clone()),
             Singleton(n) => Some(n.clone()),
             Line(line) => Some(line.nearest(p)),
-            Union(r1, r2) => {
+            Intersection(r1, r2) => {
+                r1.clone().intersect((**r2).clone()).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) {
+                        if distance(p, &n1) <= distance(p, &n2) {
-                        n1
+                            n1
-                        // } else {
+                        } else {
-                        // n2
+                            n2
-                        // }
+                        }
                     }),
                 }
-            }
+            }*/
         }
     }
 }
 
 impl Region2 {
+    pub fn intersection(self, other: Self) -> Self {
+        use Region2::*;
+        match (self, other) {
+            (Empty, _) | (_, Empty) => Empty,
+            (Full, r) | (r, Full) => r,
+            (r1, r2) => Intersection(Box::new(r1), Box::new(r2)),
+        }
+    }
+
+    /*
     pub fn union(r1: Region2, r2: Region2) -> Region2 {
         use Region2::*;
         match (r1, r2) {
@@ -375,30 +370,45 @@ impl Region2 {
             (r1, r2) => Union(Box::new(r1), Box::new(r2)),
         }
     }
+    */
 
-    pub fn intersect(&self, other: &Region2) -> Region2 {
+    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.clone())
+                    Singleton(n1)
                 } else {
-                    Empty
+                    Region2::intersection(Singleton(n1), Singleton(n2))
                 }
             }
             (Singleton(n), o) | (o, Singleton(n)) => {
-                if o.contains(n) {
+                if o.contains(&n).unwrap_or(false) {
-                    Singleton(n.clone())
+                    Singleton(n)
                 } else {
-                    Empty
+                    Region2::intersection(Singleton(n), o)
                 }
             }
-            (Line(l1), Line(l2)) => l1.intersect(l2),
+            (Intersection(r1, r2), o) | (o, Intersection(r1, r2)) => {
-            (Union(un1, un2), o) | (o, Union(un1, un2)) => {
+                r1.intersect(*r2).intersect(o)
-                Self::union(un1.intersect(o), un2.intersect(o))
             }
+            (Line(l1), Line(l2)) => l1.intersect(&l2).simplify(),
+            /*(Union(un1, un2), o) | (o, Union(un1, un2)) => {
+                Self::union(un1.intersect(o), un2.intersect(o))
+            }*/
+            (r1, r2) => Intersection(Box::new(r1), Box::new(r2)),
+        }
+    }
+
+    pub fn simplify(self) -> Region2 {
+        use Region2::*;
+        match self {
+            Singleton(n) => Singleton(n.simplify()),
+            Line(l) => l.simplify(),
+            Intersection(r1, r2) => r1.simplify().intersect(r2.simplify()),
+            other => other,
         }
     }
 }

src/math/vec.rs

@@ -0,0 +1,276 @@
+use super::{Scalar, Value};
+
+use std::ops;
+
+#[derive(Clone, PartialEq, Debug)]
+pub struct Vec2<T> {
+  pub x: T,
+  pub y: T,
+}
+
+impl<T> Vec2<T> {
+  pub fn new(x: T, y: T) -> Self {
+    Self { x, y }
+  }
+}
+
+impl Vec2<Scalar> {
+  pub fn normal2(self) -> Scalar {
+    self.x * self.x + self.y * self.y
+  }
+
+  pub fn normal(self) -> Scalar {
+    self.normal2().sqrt()
+  }
+
+  pub fn normalize(self) -> Vec2<Scalar> {
+    self.clone() / self.normal()
+  }
+}
+
+impl Vec2<Value> {
+  pub fn simplify(self) -> Self {
+    Self {
+      x: self.x.simplify(),
+      y: self.y.simplify(),
+    }
+  }
+}
+
+impl<T: ops::Add<U>, U> ops::Add<Vec2<U>> for Vec2<T> {
+  type Output = Vec2<T::Output>;
+  fn add(self, rhs: Vec2<U>) -> Self::Output {
+    Self::Output {
+      x: self.x + rhs.x,
+      y: self.y + rhs.y,
+    }
+  }
+}
+
+impl<T: ops::Sub<U>, U> ops::Sub<Vec2<U>> for Vec2<T> {
+  type Output = Vec2<T::Output>;
+  fn sub(self, rhs: Vec2<U>) -> Self::Output {
+    Self::Output {
+      x: self.x - rhs.x,
+      y: self.y - rhs.y,
+    }
+  }
+}
+
+impl<T: ops::Mul<U>, U: Clone> ops::Mul<U> for Vec2<T> {
+  type Output = Vec2<T::Output>;
+  fn mul(self, rhs: U) -> Self::Output {
+    Self::Output {
+      x: self.x * rhs.clone(),
+      y: self.y * rhs,
+    }
+  }
+}
+
+impl<T: ops::Div<U>, U: Clone> ops::Div<U> for Vec2<T> {
+  type Output = Vec2<T::Output>;
+  fn div(self, rhs: U) -> Self::Output {
+    Self::Output {
+      x: self.x / rhs.clone(),
+      y: self.y / rhs,
+    }
+  }
+}
+
+#[derive(Clone, PartialEq, Debug)]
+pub struct Point2<T> {
+  pub x: T,
+  pub y: T,
+}
+
+impl<T> Point2<T> {
+  pub fn new(x: T, y: T) -> Point2<T> {
+    Point2 { x, y }
+  }
+}
+
+impl Point2<Value> {
+  pub fn simplify(self) -> Self {
+    Self {
+      x: self.x.simplify(),
+      y: self.y.simplify(),
+    }
+  }
+}
+
+impl From<Point2<Scalar>> for Point2<Value> {
+  fn from(sp: Point2<Scalar>) -> Self {
+    Self { x: sp.x.into(), y: sp.y.into() }
+  }
+}
+
+impl<T: ops::Add<U>, U> ops::Add<Vec2<U>> for Point2<T> {
+  type Output = Point2<T::Output>;
+  fn add(self, rhs: Vec2<U>) -> Self::Output {
+    Point2 {
+      x: self.x + rhs.x,
+      y: self.y + rhs.y,
+    }
+  }
+}
+
+impl<T: ops::Sub<U>, U> ops::Sub<Vec2<U>> for Point2<T> {
+  type Output = Point2<T::Output>;
+  fn sub(self, rhs: Vec2<U>) -> Self::Output {
+    Point2 {
+      x: self.x - rhs.x,
+      y: self.y - rhs.y,
+    }
+  }
+}
+
+impl<T: ops::Sub<U>, U> ops::Sub<Point2<U>> for Point2<T> {
+  type Output = Vec2<T::Output>;
+  fn sub(self, rhs: Point2<U>) -> Self::Output {
+    Vec2 {
+      x: self.x - rhs.x,
+      y: self.y - rhs.y,
+    }
+  }
+}
+
+use std::fmt;
+
+impl<T: fmt::Display> fmt::Display for Point2<T> {
+  fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    write!(f, "({}, {})", self.x, self.y)
+  }
+}
+
+#[derive(Clone, Copy, PartialEq, Debug)]
+pub struct Rot2 {
+  cos: Scalar,
+  sin: Scalar,
+}
+
+impl fmt::Display for Rot2 {
+  fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+    write!(f, "({}, {})", self.cos, self.sin)
+  }
+}
+
+impl Rot2 {
+  pub fn from_cos_sin_unchecked(cos: Scalar, sin: Scalar) -> Self {
+    Self { cos, sin }
+  }
+
+  pub fn from_cos_sin(cos: Scalar, sin: Scalar) -> Self {
+    Vec2 { x: cos, y: sin }.into()
+  }
+
+  pub fn from_angle(angle: Scalar) -> Self {
+    Self {
+      cos: angle.cos().into(),
+      sin: angle.sin().into(),
+    }
+  }
+
+  pub fn cardinal(index: i64) -> Self {
+    match index % 4 {
+      0 => Rot2 {
+        cos: (1.).into(),
+        sin: (0.).into(),
+      },
+      1 => Rot2 {
+        cos: (0.).into(),
+        sin: (1.).into(),
+      },
+      2 => Rot2 {
+        cos: (-1.).into(),
+        sin: (0.).into(),
+      },
+      3 => Rot2 {
+        cos: (0.).into(),
+        sin: (-1.).into(),
+      },
+      _ => unreachable!(),
+    }
+  }
+
+  pub fn cos(&self) -> Scalar {
+    self.cos
+  }
+
+  pub fn sin(&self) -> Scalar {
+    self.sin
+  }
+
+  pub fn conj(self) -> Self {
+    Self {
+      cos: self.cos,
+      sin: -self.sin,
+    }
+  }
+}
+
+impl From<Vec2<Scalar>> for Rot2 {
+  fn from(v: Vec2<Scalar>) -> Rot2 {
+    let v = v.normalize();
+    Rot2 { cos: v.x, sin: v.y }
+  }
+}
+
+impl ops::Mul<Scalar> for Rot2 {
+  type Output = Vec2<Scalar>;
+  fn mul(self, rhs: Scalar) -> Vec2<Scalar> {
+    Vec2 {
+      x: self.cos * rhs,
+      y: self.sin * rhs,
+    }
+  }
+}
+
+impl ops::Mul<Value> for Rot2 {
+  type Output = Vec2<Value>;
+  fn mul(self, rhs: Value) -> Vec2<Value> {
+    Vec2 {
+      x: rhs.clone() * self.cos,
+      y: rhs * self.sin,
+    }
+  }
+}
+
+impl ops::Add<Rot2> for Rot2 {
+  type Output = Rot2;
+  fn add(self, rhs: Rot2) -> Rot2 {
+    Rot2 {
+      cos: self.cos.clone() * rhs.cos.clone() - self.sin.clone() * rhs.sin.clone(),
+      sin: self.cos * rhs.sin + self.sin * rhs.cos,
+    }
+  }
+}
+
+impl ops::Sub<Rot2> for Rot2 {
+  type Output = Rot2;
+  fn sub(self, rhs: Rot2) -> Rot2 {
+    Rot2 {
+      cos: self.cos.clone() * rhs.cos.clone() + self.sin.clone() * rhs.sin.clone(),
+      sin: self.sin * rhs.cos - self.cos * rhs.sin,
+    }
+  }
+}
+
+impl ops::Mul<Vec2<Scalar>> for Rot2 {
+  type Output = Vec2<Scalar>;
+  fn mul(self, rhs: Vec2<Scalar>) -> Vec2<Scalar> {
+    Vec2 {
+      x: self.cos * rhs.x - self.sin * rhs.y,
+      y: self.cos * rhs.y + self.sin * rhs.x,
+    }
+  }
+}
+
+impl ops::Mul<Vec2<Value>> for Rot2 {
+  type Output = Vec2<Value>;
+  fn mul(self, rhs: Vec2<Value>) -> Vec2<Value> {
+    Vec2 {
+      x: rhs.x.clone() * self.cos - rhs.y.clone() * self.sin,
+      y: rhs.y * self.cos + rhs.x * self.sin,
+    }
+  }
+}

src/relation.rs

@@ -1,5 +1,5 @@
 use crate::entity::{Point as PointEntity, PointRef};
-use crate::math::{Line2, Vec2, Point2, Region1, Region2, Rot2, Scalar};
+use crate::math::{Line2, Vec2, Point2, Region1, Region2, Rot2, Scalar, Value, Region};
 
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub enum ResolveResult {
@@ -31,7 +31,7 @@ pub struct Coincident {
 impl Relation for Coincident {
     fn resolve(&self) -> ResolveResult {
         let (mut p1, mut p2) = (self.p1.borrow_mut(), self.p2.borrow_mut());
-        let r = { p1.pos.constraints().intersect(p2.pos.constraints()) };
+        let r = { p1.pos.constraints().clone().intersect(p2.pos.constraints().clone()).simplify() };
         p1.pos.reconstrain(r.clone());
         p2.pos.reconstrain(r.clone());
         ResolveResult::from_r2(&r)
@@ -62,9 +62,9 @@ impl Relation for PointAngle {
     fn resolve(&self) -> ResolveResult {
         use Region2::*;
         let (mut p1, mut p2) = (self.p1.borrow_mut(), self.p2.borrow_mut());
-        let constrain_line = |p1: &Point2, p2: &mut PointEntity| {
+        let constrain_line = |p1: &Point2<Value>, p2: &mut PointEntity| {
             let line = Region2::Line(Line2::new(p1.clone(), self.angle.clone(), Region1::Full));
-            let new_constraint = p2.pos.constraints().intersect(&line);
+            let new_constraint = p2.pos.constraints().clone().intersection(line).simplify();
             p2.pos.reconstrain(new_constraint);
             ResolveResult::from_r2(p2.pos.constraints())
         };
@@ -123,17 +123,17 @@ impl Relation for AlignedDistance {
     fn resolve(&self) -> ResolveResult {
         use Region2::*;
         let (mut p1, mut p2) = (self.p1.borrow_mut(), self.p2.borrow_mut());
-        let constrain_line = |p1: Point2, p2: &mut PointEntity| {
+        let constrain_line = |p1: Point2<Value>, p2: &mut PointEntity| {
             let angle = match self.axis {
                 Axis::Horizontal => Rot2::from_cos_sin_unchecked((0.).into(), (1.).into()),
                 Axis::Vertical => Rot2::from_cos_sin_unchecked((1.).into(), (0.).into()),
             };
-            let line = Region2::Line(Line2::new(p1, angle, Region1::Full));
+            let line = Region2::Line(Line2::new(p1.clone(), angle, Region1::Full));
-            let new_constraint = p2.pos.constraints().intersect(&line);
+            let new_constraint = p2.pos.constraints().clone().intersection(line).simplify();
             p2.pos.reconstrain(new_constraint);
             ResolveResult::from_r2(p2.pos.constraints())
         };
-        let offset = match self.axis {
+        let offset: Vec2<Scalar> = match self.axis {
             Axis::Horizontal => Vec2::new(self.distance.into(), (0.).into()),
             Axis::Vertical => Vec2::new((0.).into(), self.distance.into()),
         };