From 2d5d2fda4b849a8f9873c8ded87a2aaad2d2cba1 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Tue, 12 Feb 2019 13:47:22 -0800 Subject: [PATCH] lots of cool work --- src/entity.rs | 15 +- src/main.rs | 39 ++-- src/math/eqn.rs | 65 +++++-- src/math/mod.rs | 478 ++++++++++++++++++++++++------------------------ src/math/vec.rs | 276 ++++++++++++++++++++++++++++ src/relation.rs | 16 +- 6 files changed, 609 insertions(+), 280 deletions(-) create mode 100644 src/math/vec.rs diff --git a/src/entity.rs b/src/entity.rs index c8bd284..8b6e6a5 100644 --- a/src/entity.rs +++ b/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> { @@ -9,6 +10,12 @@ pub struct Var> { constraints: TRegion, } +impl + fmt::Display> fmt::Display for Var { + fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { + write!(f, "{{ {} ∈ {} }}", self.value, self.constraints) + } +} + impl> Var { pub fn new(value: T, constraints: TRegion) -> Self { Self { value, constraints } @@ -42,7 +49,7 @@ impl> Var { } type ScalarVar = Var; -type PointVar = Var; +type PointVar = Var, 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, diff --git a/src/main.rs b/src/main.rs index 3489d95..424328f 100644 --- a/src/main.rs +++ b/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 u2 = math::eqn::Unknown(2); - let u1 = eqn::Expr::from(1.); - let u2 = eqn::Expr::from(1.); + let u1 = math::eqn::Unknown(1); + let u2 = math::eqn::Unknown(2); + // let u1 = 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 = vec![origin.clone(), p1.clone(), p2.clone(), p3.clone()]; let print_points = |points: &Vec| { println!( - "origin, p1, p2, p3:\n {:?}\n {:?}\n {:?}\n {:?}", - points[0], points[1], points[2], points[3], + "origin, p1, p2, p3:\n {}\n {}\n {}\n {}", + points[0].borrow(), points[1].borrow(), points[2].borrow(), points[3].borrow(), ); }; print_points(&points); - // let c1 = relation::Coincident { - // p1: origin.clone(), - // p2: p1.clone(), - // }; - let c2 = relation::PointAngle::new_vertical(p1.clone(), p2.clone()); + let c1 = relation::Coincident { + p1: origin.clone(), + p2: p1.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> = - 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> = 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!( - "origin, p1, p2, p3:\n {:?}\n {:?}\n {:?}\n {:?}", - origin, p1, p2, p3 - ); + print_points(&points); 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 => { diff --git a/src/math/eqn.rs b/src/math/eqn.rs index ce57283..b07fa11 100644 --- a/src/math/eqn.rs +++ b/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 = BTreeMap::new(); + // let mut common: BTreeMap = BTreeMap::new(); + let mut common: Option = 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 { - Const(c) => relative_eq!(*c, 0.), + match self.simplify() { + Const(c) => relative_eq!(c, 0.), _ => false, } } - pub fn is_one(&self) -> bool { + pub fn is_one(self) -> bool { use Expr::*; - match self { - Const(c) => relative_eq!(*c, 1.), + match self.simplify() { + 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 { diff --git a/src/math/mod.rs b/src/math/mod.rs index 99ca5ff..44c6945 100644 --- a/src/math/mod.rs +++ b/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; // pub type Point2 = nalgebra::Point2; -#[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 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 struct Point2 { - pub x: Value, - pub y: Value, -} - -impl Point2 { - pub fn new(x: Value, y: Value) -> Point2 { - Point2 { x, y } - } -} +// pub type Rot2 = nalgebra::UnitComplex; -impl std::ops::Add for Point2 { - type Output = Point2; - fn add(self, rhs: Vec2) -> Point2 { - Point2 { - x: self.x + rhs.x, - y: self.y + rhs.y, - } - } -} +pub trait Region { + fn full() -> Self; + fn singleton(value: T) -> Self; + // fn intersection(self, other: Self) -> Self; -impl std::ops::Sub for Point2 { - type Output = Point2; - fn sub(self, rhs: Vec2) -> Point2 { - Point2 { - x: self.x - rhs.x, - y: self.y - rhs.y, - } - } + fn nearest(&self, value: &T) -> Option; + fn contains(&self, value: &T) -> Option; } -impl std::ops::Sub for Point2 { - type Output = Vec2; - fn sub(self, rhs: Point2) -> Vec2 { - Vec2 { - x: self.x - rhs.x, - y: self.y - rhs.y, - } - } +#[derive(Clone, Debug)] +pub enum Region1 { + Empty, + Singleton(Value), + Range(Value, Value), + Intersection(Box, Box), + // Union(Box, Box), + Full, } -use std::fmt; - -impl fmt::Display for Point2 { +impl fmt::Display for Region1 { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { - write!(f, "({}, {})", self.x, self.y) - } -} - -#[derive(Clone, PartialEq, Debug)] -pub struct Rot2 { - cos: Value, - 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 for Rot2 { - type Output = Vec2; - fn mul(self, rhs: Scalar) -> Vec2 { - Vec2 { - x: self.cos * rhs, - y: self.sin * rhs, - } - } -} - -impl std::ops::Mul 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 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, + use Region1::*; + match self { + Empty => write!(f, "Ø"), + Singleton(v) => write!(f, "{{ {} }}", v), + Range(l, u) => write!(f, "[ {}, {} ]", l, u), + Intersection(r1, r2) => write!(f, "{} ∩ {}", r1, r2), + Full => write!(f, "ℝ") } } } -impl std::ops::Sub 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 Region1 { + pub fn intersection(self, other: Region1) -> Self { + Region1::Intersection(Box::new(self), Box::new(other)) } -} -impl std::ops::Mul for Rot2 { - type Output = Vec2; - fn mul(self, rhs: Vec2) -> Vec2 { - Vec2 { - x: self.cos.clone() * rhs.x.clone() - self.sin.clone() * rhs.y.clone(), - y: self.cos * rhs.y + self.sin * rhs.x, + pub fn simplify(self) -> Self { + use Region1::*; + match self { + Singleton(n) => Singleton(n.simplify()), + Range(l, u) => Range(l.simplify(), u.simplify()), + Intersection(r1, r2) => r1.simplify().intersection(r2.simplify()), + other => other, } } } -// pub type Rot2 = nalgebra::UnitComplex; - -pub trait Region { - fn full() -> Self; - fn singleton(value: T) -> Self; - - fn nearest(&self, value: &T) -> Option; - fn contains(&self, value: &T) -> bool; -} - -#[derive(Clone, Debug)] -pub enum Region1 { - Empty, - Singleton(Scalar), - Range(Scalar, Scalar), - Union(Box, Box), - Full, -} - impl Region 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 { + use Expr::Const; 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, + Empty => Some(false), + Singleton(n1) => match n1 { + Const(c) => Some(relative_eq!(c, n)), + _ => None, + }, + 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 { + use Expr::Const; 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), + Singleton(n) => match n { + Const(c) => Some(*c), _ => 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 for Region1 { } }), } - } + }*/ } } } @@ -265,54 +143,91 @@ impl Region for Region1 { // ie. start + (cos dir, sin dir) * t for t in extent #[derive(Clone, Debug)] pub struct Line2 { - start: Point2, + start: Point2, 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, dir: Rot2, extent: Region1) -> Self { Self { start, dir, extent } } - pub fn evaluate(&self, t: Value) -> Point2 { + pub fn evaluate(&self, t: Value) -> Point2 { self.start.clone() + self.dir.clone() * t } - pub fn nearest(&self, p: &Point2) -> Point2 { + pub fn evaluate_extent(&self) -> Option> { + 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) -> Point2 { // 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) { - np - } else { - panic!("Line2::nearest not found!"); + match self.intersect(&perp) { + Region2::Singleton(np) => np, + Region2::Line(l) => l.evaluate_extent().expect("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); - return if relative_eq!(starts.y, 0.) { + let starts = self.dir.conj() * (other.start.clone() - self.start.clone()); + 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), Line(Line2), - #[allow(dead_code)] - Union(Box, Box), + // #[allow(dead_code)] + // Union(Box, Box), + Intersection(Box, Box), Full, } -impl Region 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> for Region2 { fn full() -> Self { Region2::Full } - fn singleton(value: Point2) -> Self { + fn singleton(value: Point2) -> Self { + Region2::Singleton(value.into()) + } + + fn contains(&self, p: &Point2) -> Option { + self.nearest(p).map(|n| n == *p) + } + + fn nearest(&self, p: &Point2) -> Option> { + 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 = 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> 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| true /*relative_eq!(n, p)*/) + fn contains(&self, p: &Point2) -> Option { + self.nearest(p).map(|n| n.simplify() == p.clone().simplify()) } - fn nearest(&self, p: &Point2) -> Option { + fn nearest(&self, p: &Point2) -> Option> { 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) { - n1 - // } else { - // n2 - // } + if distance(p, &n1) <= distance(p, &n2) { + n1 + } else { + 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) { - Singleton(n.clone()) + if o.contains(&n).unwrap_or(false) { + Singleton(n) } else { - Empty + Region2::intersection(Singleton(n), o) } } - (Line(l1), Line(l2)) => l1.intersect(l2), - (Union(un1, un2), o) | (o, Union(un1, un2)) => { - Self::union(un1.intersect(o), un2.intersect(o)) + (Intersection(r1, r2), o) | (o, Intersection(r1, r2)) => { + r1.intersect(*r2).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, } } } diff --git a/src/math/vec.rs b/src/math/vec.rs new file mode 100644 index 0000000..6890405 --- /dev/null +++ b/src/math/vec.rs @@ -0,0 +1,276 @@ +use super::{Scalar, Value}; + +use std::ops; + +#[derive(Clone, PartialEq, Debug)] +pub struct Vec2 { + pub x: T, + pub y: T, +} + +impl Vec2 { + pub fn new(x: T, y: T) -> Self { + Self { x, y } + } +} + +impl Vec2 { + 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 { + self.clone() / self.normal() + } +} + +impl Vec2 { + pub fn simplify(self) -> Self { + Self { + x: self.x.simplify(), + y: self.y.simplify(), + } + } +} + +impl, U> ops::Add> for Vec2 { + type Output = Vec2; + fn add(self, rhs: Vec2) -> Self::Output { + Self::Output { + x: self.x + rhs.x, + y: self.y + rhs.y, + } + } +} + +impl, U> ops::Sub> for Vec2 { + type Output = Vec2; + fn sub(self, rhs: Vec2) -> Self::Output { + Self::Output { + x: self.x - rhs.x, + y: self.y - rhs.y, + } + } +} + +impl, U: Clone> ops::Mul for Vec2 { + type Output = Vec2; + fn mul(self, rhs: U) -> Self::Output { + Self::Output { + x: self.x * rhs.clone(), + y: self.y * rhs, + } + } +} + +impl, U: Clone> ops::Div for Vec2 { + type Output = Vec2; + 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 { + pub x: T, + pub y: T, +} + +impl Point2 { + pub fn new(x: T, y: T) -> Point2 { + Point2 { x, y } + } +} + +impl Point2 { + pub fn simplify(self) -> Self { + Self { + x: self.x.simplify(), + y: self.y.simplify(), + } + } +} + +impl From> for Point2 { + fn from(sp: Point2) -> Self { + Self { x: sp.x.into(), y: sp.y.into() } + } +} + +impl, U> ops::Add> for Point2 { + type Output = Point2; + fn add(self, rhs: Vec2) -> Self::Output { + Point2 { + x: self.x + rhs.x, + y: self.y + rhs.y, + } + } +} + +impl, U> ops::Sub> for Point2 { + type Output = Point2; + fn sub(self, rhs: Vec2) -> Self::Output { + Point2 { + x: self.x - rhs.x, + y: self.y - rhs.y, + } + } +} + +impl, U> ops::Sub> for Point2 { + type Output = Vec2; + fn sub(self, rhs: Point2) -> Self::Output { + Vec2 { + x: self.x - rhs.x, + y: self.y - rhs.y, + } + } +} + +use std::fmt; + +impl fmt::Display for Point2 { + 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> for Rot2 { + fn from(v: Vec2) -> Rot2 { + let v = v.normalize(); + Rot2 { cos: v.x, sin: v.y } + } +} + +impl ops::Mul for Rot2 { + type Output = Vec2; + fn mul(self, rhs: Scalar) -> Vec2 { + Vec2 { + x: self.cos * rhs, + y: self.sin * rhs, + } + } +} + +impl ops::Mul for Rot2 { + type Output = Vec2; + fn mul(self, rhs: Value) -> Vec2 { + Vec2 { + x: rhs.clone() * self.cos, + y: rhs * self.sin, + } + } +} + +impl ops::Add 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 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> for Rot2 { + type Output = Vec2; + fn mul(self, rhs: Vec2) -> Vec2 { + Vec2 { + x: self.cos * rhs.x - self.sin * rhs.y, + y: self.cos * rhs.y + self.sin * rhs.x, + } + } +} + +impl ops::Mul> for Rot2 { + type Output = Vec2; + fn mul(self, rhs: Vec2) -> Vec2 { + Vec2 { + x: rhs.x.clone() * self.cos - rhs.y.clone() * self.sin, + y: rhs.y * self.cos + rhs.x * self.sin, + } + } +} diff --git a/src/relation.rs b/src/relation.rs index 3b1ba0b..c84b788 100644 --- a/src/relation.rs +++ b/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, 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, 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 new_constraint = p2.pos.constraints().intersect(&line); + let line = Region2::Line(Line2::new(p1.clone(), angle, Region1::Full)); + 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 = match self.axis { Axis::Horizontal => Vec2::new(self.distance.into(), (0.).into()), Axis::Vertical => Vec2::new((0.).into(), self.distance.into()), };