From d9fbe9c2647afb4de48a3d6c44cdb8aae7af9214 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sat, 2 Feb 2019 23:11:59 -0800
Subject: [PATCH] actually finish if underconstrained

---
 src/main.rs | 54 ++++++++++++++++++++++++++++++++++++++++-------------
 src/math.rs | 41 +++++++++++++++++++++++++---------------
 2 files changed, 67 insertions(+), 28 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index cbeb8f1..24922d8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,3 +1,5 @@
+#![feature(drain_filter)]
+
 extern crate nalgebra;
 #[macro_use]
 extern crate approx;
@@ -15,7 +17,7 @@ fn main() {
     let origin = Point::new_ref(Var::new_single(Point2::new(0., 0.)));
     let p1 = Point::new_ref(Var::new_full(Point2::new(1., 1.)));
     let p2 = Point::new_ref(Var::new_full(Point2::new(4., 4.)));
-    let p3 = Point::new_ref(Var::new_single(Point2::new(2., 2.)));
+    let p3 = Point::new_ref(Var::new_full(Point2::new(2., 2.)));
     let mut points: Vec<PointRef> = vec![origin.clone(), p1.clone(), p2.clone(), p3.clone()];
     let print_points = |points: &Vec<PointRef>| {
         println!(
@@ -29,23 +31,49 @@ fn main() {
         p2: p1.clone(),
     };
     let c2 = relation::PointAngle::new_vertical(p1.clone(), p2.clone());
-    let c3 = relation::PointAngle::new_horizontal(p1.clone(), p2.clone());
-    let c4 = relation::PointAngle::new_horizontal(p1.clone(), p3.clone());
-    let mut relations: Vec<Box<dyn Relation>> =
-        vec![Box::new(c1), Box::new(c2), Box::new(c3), Box::new(c4)];
-    let mut has_underconstrained = true;
-    while has_underconstrained {
-        has_underconstrained = false;
-        for r in &relations {
+    let c3 = relation::PointAngle::new_horizontal(p2.clone(), p3.clone());
+    // let c4 = relation::PointAngle::new_horizontal(p1.clone(), p3.clone());
+    let mut relations: Vec<Box<dyn Relation>> = 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;
+    let mut any_overconstrained = false;
+    while any_underconstrained && any_constrained && !any_overconstrained {
+        any_underconstrained = false;
+        any_constrained = false;
+        let newly_constrained = relations.drain_filter(|r| {
             let rr = r.resolve();
-            if rr == ResolveResult::Underconstrained {
-                has_underconstrained = true;
-            }
             println!("resolve result: {:?}", rr);
             println!(
                 "origin, p1, p2, p3:\n {:?}\n {:?}\n {:?}\n {:?}",
                 origin, p1, p2, p3
             );
-        }
+            match rr {
+                ResolveResult::Underconstrained => {
+                    any_underconstrained = true;
+                    false
+                }
+                ResolveResult::Constrained => {
+                    any_constrained = true;
+                    true
+                }
+                ResolveResult::Overconstrained => {
+                    any_overconstrained = true;
+                    true
+                }
+            }
+        });
+        constrained.extend(newly_constrained);
+    }
+    if any_overconstrained {
+        println!("The system is overconstrained")
+    } else if any_underconstrained {
+        println!("Some constraints could not be solved")
+    } else {
+        println!("All constraints have been solved")
     }
 }
diff --git a/src/math.rs b/src/math.rs
index 37d8c8e..779f4ef 100644
--- a/src/math.rs
+++ b/src/math.rs
@@ -65,10 +65,29 @@ impl Line2 {
         self.start + self.dir * Vec2::new(t, 0.)
     }
 
-    pub fn intersect(&self, other: &Line2) -> Option<Point2> {
-        // if two lines are parallel
-        if relative_eq!((self.dir / other.dir).sin_angle(), 0.) {
-            return None;
+    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);
@@ -80,7 +99,7 @@ impl Line2 {
             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);
-        Some(b.evaluate(t_b))
+        Region2::Singleton(b.evaluate(t_b))
     }
 }
 
@@ -114,12 +133,7 @@ impl Region<Point2> for Region2 {
             Empty => None,
             Full => Some(*p),
             Singleton(n) => Some(*n),
-            Line(line) => {
-                // rotate angle 90 degrees
-                let perp_dir = line.dir * Rot2::from_cos_sin_unchecked(0., 1.);
-                let perp = Line2::new(*p, perp_dir, Region1::Full);
-                perp.intersect(line)
-            }
+            Line(line) => Some(line.nearest(p)),
             Union(r1, r2) => {
                 use nalgebra::distance;
                 match (r1.nearest(p), r2.nearest(p)) {
@@ -158,10 +172,7 @@ impl Region2 {
                     Empty
                 }
             }
-            (Line(l1), Line(l2)) => match l1.intersect(l2) {
-                Some(p) => Singleton(p),
-                None => Empty,
-            },
+            (Line(l1), Line(l2)) => l1.intersect(l2),
             _ => unimplemented!(),
         }
     }