Lots of work to make equation solving work better

src/math/eqn.rs

@@ -96,19 +96,22 @@ fn remove_term(terms: &mut Vec<Expr>, term: &Expr) -> Option<Expr> {
 }
 
 fn sum_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, 0.) => o,
         (Product(mut l), Product(mut r)) => {
             let comm = remove_common_terms(&mut l, &mut r);
-            Expr::new_product(Sum(comm), Expr::new_sum(Product(l), Product(r))).simplify()
+            if comm.is_empty() {
+                Expr::new_sum(Product(l), Product(r))
+            } else {
+                Expr::new_product(Product(comm), Expr::new_sum(Product(l), Product(r)))
+            }
         }
         (Product(mut l), r) | (r, Product(mut l)) => {
             let comm = remove_term(&mut l, &r);
             match comm {
-                Some(_) => Expr::new_product(r, Expr::new_sum(Product(l), Const(1.))).simplify(),
+                Some(_) => Expr::new_product(r, Expr::new_sum(Product(l), Const(1.))),
                 None => Expr::new_sum(Product(l), r),
             }
         }
@@ -143,6 +146,9 @@ fn group_sum(es: Exprs) -> Exprs {
             }
         };
     }
+    for c in common.values() {
+        trace!("group sum value: {}", c);
+    }
     common.into_iter().map(|(_, v)| v).collect()
 }
 
@@ -159,44 +165,49 @@ fn product_fold(l: Expr, r: Expr) -> Expr {
                 Expr::Div(Box::new(Expr::Product(vec![*num, mul])), den).simplify()
             }
         }
+        (Product(mut ls), Product(mut rs)) => {
+            ls.append(&mut rs);
+            Product(ls)
+        },
+        (Product(mut ps), o) | (o, Product(mut ps)) => {
+            ps.push(o);
+            Product(ps)
+        },
         (l, r) => Expr::new_product(l, r),
     }
 }
 
 fn group_product(es: Exprs) -> Exprs {
     use Expr::*;
-    // let mut common: BTreeMap<UnknownSet, Expr> = BTreeMap::new();
-    let mut common: Option<Expr> = None;
+    let es2 = es.clone();
+    let mut consts: Option<Scalar> = None;
+    let mut other = Exprs::new();
     for e in es {
         let unkns = e.unknowns();
-        // match common.get_mut(&unkns) {
-        match &mut common {
-            None => {
-                match e {
-                    Const(c) if relative_eq!(c, 1.) => (),
-                    e => {
-                        // common.insert(unkns, e);
-                        common = Some(e);
-                    }
-                };
+        match e { 
+            Const(c) => match consts {
+                None => consts = Some(c),
+                Some(cs) => consts = Some(c * cs),
             }
-            Some(existing) => {
-                match existing {
-                    // Product(ref mut es) => {
-                        // already failed at merging, so just add it to the list
-                        // es.push(e);
-                    // }
-                    other => *other = product_fold(other.clone(), e),
-                };
+            e => {
+                other.push(e)
             }
-        };
+        }
     }
-    // common.into_iter().map(|(_, v)| v).collect()
-    common.into_iter().collect()
+    if let Some(cs) = consts { 
+        if relative_eq!(cs, 0.0) {
+            other.clear();
+            other.push(Const(0.0))
+        } else if relative_ne!(cs, 1.0) {
+            other.push(Const(cs)) 
+        }
+    };
+    trace!("group product: {:?} => {:?}", es2, other);
+    other
 }
 
 fn distribute_product_sums(mut es: Exprs) -> Expr {
-    trace!("distribute_product_sums: {}", Product(es.clone()));
+    let es_pre = es.clone();
     use itertools::Itertools;
     use Expr::*;
     for e in &mut es {
@@ -209,14 +220,14 @@ fn distribute_product_sums(mut es: Exprs) -> Expr {
         })
         .map(|e| {
             trace!("sum in product: {}", e);
-            match e {
+            match e.simplify() {
                 Sum(es) => es,
-                _ => unreachable!(),
+                o => vec![o],
             }
         });
     let products: Vec<_> = sums.multi_cartesian_product().collect();
     if products.is_empty() {
-        trace!("no sums to distribute");
+        trace!("distribute_product_sums: no sums to distribute");
         return Product(es);
     }
     let sums = products
@@ -227,7 +238,9 @@ fn distribute_product_sums(mut es: Exprs) -> Expr {
             Product(prod)
         })
         .collect();
-    Sum(sums)
+    let res = Sum(sums);
+    trace!("distribute_product_sums: {} => {}", Product(es_pre), res);
+    res
 }
 
 impl Unknowns for Expr {
@@ -336,6 +349,7 @@ impl Expr {
         use Expr::*;
         match self {
             Sum(es) => {
+                let pre_new_es = es.clone();
                 let mut new_es: Vec<_> = es
                     .into_iter()
                     .map(|e| e.simplify())
@@ -344,7 +358,6 @@ impl Expr {
                         other => vec![other],
                     })
                     .collect();
-                let pre_new_es = new_es.clone();
                 new_es = group_sum(new_es);
                 trace!(
                     "simplify sum {} => {}",
@@ -359,6 +372,7 @@ impl Expr {
                 }
             }
             Product(es) => {
+                let pre_new_es = es.clone();
                 let new_es: Vec<_> = es
                     .into_iter()
                     .map(|e| e.simplify())
@@ -367,7 +381,6 @@ impl Expr {
                         other => vec![other],
                     })
                     .collect();
-                let pre_new_es = new_es.clone();
                 let new_es = group_product(new_es);
                 trace!(
                     "simplify product {} => {}",
@@ -388,7 +401,7 @@ impl Expr {
                     box Neg(v) => *v,
                     box Product(mut es) => {
                         es.push(Const(-1.));
-                        Product(es)
+                        Product(es).simplify()
                     }
                     e => Product(vec![Const(-1.), *e]),
                 }
@@ -428,13 +441,15 @@ impl Expr {
 
     pub fn distribute(self) -> Expr {
         use Expr::*;
-        trace!("distribute {}", self);
         match self {
             Sum(mut es) => {
+                let es_pre = es.clone();
                 for e in &mut es {
                     *e = e.clone().distribute();
                 }
-                Sum(es)
+                let res = Sum(es);
+                trace!("distribute sum {} => {}", Sum(es_pre), res);
+                res
             }
             Product(es) => distribute_product_sums(es),
             Div(mut num, mut den) => {

src/math/mod.rs

@@ -165,7 +165,7 @@ pub struct Line2 {
 
 impl fmt::Display for Line2 {
     fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-        write!(f, "{{ (x, y) = {} + {} * {} }}", self.start, self.dir, self.extent)
+        write!(f, "{{ <x, y> = {} + {} * {} }}", self.start, self.dir, self.extent)
     }
 }
 

src/math/vec.rs

@@ -146,7 +146,7 @@ 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)
+    write!(f, "<{}, {}>", self.x, self.y)
   }
 }
 
@@ -158,7 +158,7 @@ pub struct Rot2 {
 
 impl fmt::Display for Rot2 {
   fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
-    write!(f, "({}, {})", self.cos, self.sin)
+    write!(f, "<{}, {}>", self.cos, self.sin)
   }
 }
 
@@ -167,6 +167,14 @@ impl Rot2 {
     Self { cos, sin }
   }
 
+  pub fn up() -> Self {
+    Self { cos: 0., sin: 1. }
+  }
+
+  pub fn right() -> Self {
+    Self { cos: 1., sin: 0. }
+  }
+
   pub fn from_cos_sin(cos: Scalar, sin: Scalar) -> Self {
     Vec2 { x: cos, y: sin }.into()
   }
@@ -218,6 +226,10 @@ impl Rot2 {
       sin: -self.sin,
     }
   }
+
+  pub fn dot(self, v: Vec2<Value>) -> Value {
+    v.x * self.cos + v.y * self.sin
+  }
 }
 
 impl From<Vec2<Scalar>> for Rot2 {

src/relation.rs

@@ -64,7 +64,9 @@ impl Relation for PointAngle {
         let (mut p1, mut p2) = (self.p1.borrow_mut(), self.p2.borrow_mut());
         let constrain_line = |p1: &Point2<Value>, p2: &mut PointEntity| {
             let line = Region2::Line(Line2::new(p1.clone(), self.angle.clone(), Region1::Full));
+            trace!("PointAngle line: {}, p2 constraint: {}", line, p2.pos.constraints());
             let new_constraint = p2.pos.constraints().clone().intersection(line).simplify();
+            trace!("PointAngle new_constraint: {}", new_constraint);
             p2.pos.reconstrain(new_constraint);
             ResolveResult::from_r2(p2.pos.constraints())
         };
@@ -74,7 +76,7 @@ impl Relation for PointAngle {
                 // if the angle p1 and p2 form is parallel to self.angle, the result
                 // will have a y component of 0
                 let r = self.angle.clone().conj() * (p2.clone() - p1.clone());
-                println!("angle.cos: {}", r.x);
+                trace!("angle.cos: {}", r.x);
                 // if relative_eq!(r.y, 0.) {
                     ResolveResult::Constrained
                 // } else {
@@ -88,34 +90,29 @@ impl Relation for PointAngle {
     }
 }
 
-pub enum Axis {
-    Vertical,
-    Horizontal,
-}
-
 pub struct AlignedDistance {
     pub p1: PointRef,
     pub p2: PointRef,
-    pub axis: Axis,
+    pub angle: Rot2,
     pub distance: Scalar,
 }
 
 impl AlignedDistance {
-    pub fn new(p1: PointRef, p2: PointRef, axis: Axis, distance: Scalar) -> Self {
+    pub fn new(p1: PointRef, p2: PointRef, angle: Rot2, distance: Scalar) -> Self {
         Self {
             p1,
             p2,
-            axis,
+            angle,
             distance,
         }
     }
 
     pub fn new_vertical(p1: PointRef, p2: PointRef, distance: Scalar) -> Self {
-        Self::new(p1, p2, Axis::Vertical, distance)
+        Self::new(p1, p2, Rot2::up(), distance)
     }
 
     pub fn new_horizontal(p1: PointRef, p2: PointRef, distance: Scalar) -> Self {
-        Self::new(p1, p2, Axis::Horizontal, distance)
+        Self::new(p1, p2, Rot2::right(), distance)
     }
 }
 
@@ -124,27 +121,20 @@ impl Relation for AlignedDistance {
         use Region2::*;
         let (mut p1, mut p2) = (self.p1.borrow_mut(), self.p2.borrow_mut());
         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.clone(), angle, Region1::Full));
+            let angle = self.angle + Rot2::up();
+            let line = Region2::Line(Line2::new(p1.clone(), angle, Region1::Full)).simplify();
+            trace!("AlignedDistance line: {}, p2 constraint: {}", line, p2.pos.constraints());
             let new_constraint = p2.pos.constraints().clone().intersection(line).simplify();
+            trace!("AlignedDistance new_constraint: {}", new_constraint);
             p2.pos.reconstrain(new_constraint);
             ResolveResult::from_r2(p2.pos.constraints())
         };
-        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()),
-        };
+        let offset: Vec2<Scalar> = self.angle * self.distance;
         match (&mut p1.pos.constraints(), &mut p2.pos.constraints()) {
             (Empty, _) | (_, Empty) => ResolveResult::Overconstrained,
             (Singleton(p1), Singleton(p2)) => {
                 let r = p2.clone() - p1.clone();
-                let d = match self.axis {
-                    Axis::Horizontal => r.x,
-                    Axis::Vertical => r.y,
-                };
+                let d = self.angle.dot(r);
                 // if relative_eq!(d, self.distance) {
                     ResolveResult::Constrained
                 // } else {
@@ -157,3 +147,9 @@ impl Relation for AlignedDistance {
         }
     }
 }
+
+pub struct Midpoint {
+    pub p1: PointRef,
+    pub p2: PointRef,
+    pub mid: PointRef,
+}