From d09f239a72309cee71cddad7d3dd2fce716d23b5 Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Sat, 2 Feb 2019 23:41:56 -0800
Subject: [PATCH] add AlignedDistance relation

---
 src/main.rs     |  9 +++----
 src/relation.rs | 72 ++++++++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 74 insertions(+), 7 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 24922d8..8a0bdbf 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -32,12 +32,9 @@ fn main() {
     };
     let c2 = relation::PointAngle::new_vertical(p1.clone(), p2.clone());
     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 c4 = 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)];
     let mut constrained: Vec<Box<dyn Relation>> = Vec::new();
     let mut any_underconstrained = true;
     let mut any_constrained = true;
diff --git a/src/relation.rs b/src/relation.rs
index 2fedde3..b32ea13 100644
--- a/src/relation.rs
+++ b/src/relation.rs
@@ -1,5 +1,5 @@
 use crate::entity::{Point as PointEntity, PointRef};
-use crate::math::{Line2, Point2, Region1, Region2, Rot2};
+use crate::math::{Line2, Vec2, Point2, Region1, Region2, Rot2, Scalar};
 
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub enum ResolveResult {
@@ -86,3 +86,73 @@ impl Relation for PointAngle {
         }
     }
 }
+
+pub enum Axis {
+    Vertical,
+    Horizontal,
+}
+
+pub struct AlignedDistance {
+    pub p1: PointRef,
+    pub p2: PointRef,
+    pub axis: Axis,
+    pub distance: Scalar,
+}
+
+impl AlignedDistance {
+    pub fn new(p1: PointRef, p2: PointRef, axis: Axis, distance: Scalar) -> Self {
+        Self {
+            p1,
+            p2,
+            axis,
+            distance,
+        }
+    }
+
+    pub fn new_vertical(p1: PointRef, p2: PointRef, distance: Scalar) -> Self {
+        Self::new(p1, p2, Axis::Vertical, distance)
+    }
+
+    pub fn new_horizontal(p1: PointRef, p2: PointRef, distance: Scalar) -> Self {
+        Self::new(p1, p2, Axis::Horizontal, distance)
+    }
+}
+
+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 angle = match self.axis {
+                Axis::Horizontal => Rot2::from_cos_sin_unchecked(0., 1.),
+                Axis::Vertical => Rot2::from_cos_sin_unchecked(1., 0.),
+            };
+            let line = Region2::Line(Line2::new(p1, angle, Region1::Full));
+            let new_constraint = p2.pos.constraints().intersect(&line);
+            p2.pos.reconstrain(new_constraint);
+            ResolveResult::from_r2(p2.pos.constraints())
+        };
+        let offset = match self.axis {
+            Axis::Horizontal => Vec2::new(self.distance, 0.),
+            Axis::Vertical => Vec2::new(0., self.distance),
+        };
+        match (&mut p1.pos.constraints(), &mut p2.pos.constraints()) {
+            (Empty, _) | (_, Empty) => ResolveResult::Overconstrained,
+            (Singleton(p1), Singleton(p2)) => {
+                let r = p2 - p1;
+                let d = match self.axis {
+                    Axis::Horizontal => r.x,
+                    Axis::Vertical => r.y,
+                };
+                if relative_eq!(d, self.distance) {
+                    ResolveResult::Constrained
+                } else {
+                    ResolveResult::Overconstrained
+                }
+            }
+            (Singleton(pos), _) => constrain_line(pos + offset, &mut *p2),
+            (_, Singleton(pos)) => constrain_line(pos - offset, &mut *p1),
+            _ => ResolveResult::Underconstrained,
+        }
+    }
+}