Compare commits
No commits in common. 'solve-eqns' and 'master' have entirely different histories.
solve-eqns
...
master
11 changed files with 899 additions and 2079 deletions
@ -1,582 +0,0 @@
@@ -1,582 +0,0 @@
|
||||
use std::borrow::Borrow; |
||||
use std::collections::BTreeMap; |
||||
use std::fmt; |
||||
|
||||
use super::eqn::{Eqn, Eqns}; |
||||
use super::unknown::{Unknown, UnknownSet, Unknowns}; |
||||
use super::Scalar; |
||||
use std::f64::NAN; |
||||
|
||||
#[derive(Clone, Debug, PartialEq)] |
||||
pub enum Expr { |
||||
Unkn(Unknown), |
||||
Const(Scalar), |
||||
Sum(Exprs), |
||||
Neg(Box<Expr>), |
||||
Product(Exprs), |
||||
Div(Box<Expr>, Box<Expr>), |
||||
} |
||||
|
||||
pub type Exprs = Vec<Expr>; |
||||
|
||||
impl Unknowns for Exprs { |
||||
fn unknowns(&self) -> UnknownSet { |
||||
self.iter().flat_map(|e: &Expr| e.unknowns()).collect() |
||||
} |
||||
fn has_unknowns(&self) -> bool { |
||||
self.iter().any(|e: &Expr| e.has_unknowns()) |
||||
} |
||||
fn has_unknown(&self, u: Unknown) -> bool { |
||||
self.iter().any(|e: &Expr| e.has_unknown(u)) |
||||
} |
||||
} |
||||
|
||||
fn write_separated_exprs(es: &[Expr], f: &mut fmt::Formatter, sep: &str) -> fmt::Result { |
||||
write!(f, "(")?; |
||||
let mut is_first = es.len() > 1; |
||||
for e in es { |
||||
if is_first { |
||||
is_first = false; |
||||
} else { |
||||
write!(f, "{}", sep)? |
||||
} |
||||
write!(f, "{}", e)? |
||||
} |
||||
write!(f, ")")?; |
||||
Ok(()) |
||||
} |
||||
|
||||
fn remove_common_terms(l: &mut Vec<Expr>, r: &mut Vec<Expr>) -> Vec<Expr> { |
||||
let common: Vec<_> = l.drain_filter(|e| r.contains(e)).collect(); |
||||
common.iter().for_each(|e| { |
||||
r.remove_item(e); |
||||
}); |
||||
common |
||||
} |
||||
|
||||
fn remove_term(terms: &mut Vec<Expr>, term: &Expr) -> Option<Expr> { |
||||
terms.remove_item(term) |
||||
} |
||||
|
||||
fn sum_fold(l: Expr, r: Expr) -> Expr { |
||||
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); |
||||
if comm.is_empty() { |
||||
Expr::new_sum(Product(l), Product(r)) |
||||
} else { |
||||
Expr::new_product(Product(comm), Expr::new_sum(Product(l), Product(r)).collapse()) |
||||
} |
||||
} |
||||
(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.))).collapse(), |
||||
None => Expr::new_sum(Product(l), r), |
||||
} |
||||
} |
||||
(Div(box ln, box ld), Div(box rn, box rd)) => { |
||||
if ld == rd { |
||||
Expr::new_div(Expr::new_sum(ln, rn), ld).collapse() |
||||
} else { |
||||
Expr::new_div(Expr::new_sum(Expr::new_product(ln, rd.clone()), Expr::new_product(rn, ld.clone())), |
||||
Expr::new_product(ld, rd)).collapse() |
||||
} |
||||
} |
||||
(l, r) => Expr::new_sum(l, r), |
||||
} |
||||
} |
||||
|
||||
fn group_sum(es: Exprs) -> Exprs { |
||||
use Expr::*; |
||||
let mut common: BTreeMap<UnknownSet, Expr> = BTreeMap::new(); |
||||
for e in es { |
||||
match e { |
||||
Const(c) if relative_eq!(c, 0.) => { |
||||
continue; |
||||
} |
||||
_ => (), |
||||
}; |
||||
let unkns = e.unknowns(); |
||||
match common.get_mut(&unkns) { |
||||
None => { |
||||
common.insert(unkns, e); |
||||
} |
||||
Some(existing) => { |
||||
match existing { |
||||
Sum(ref mut es) => { |
||||
// already failed at merging, so just add it to the list
|
||||
es.push(e); |
||||
} |
||||
other => { |
||||
*other = sum_fold(other.clone(), e); |
||||
} |
||||
}; |
||||
} |
||||
}; |
||||
} |
||||
for c in common.values() { |
||||
trace!("group sum value: {}", c); |
||||
} |
||||
common |
||||
.into_iter() |
||||
.map(|(_, v)| v) |
||||
.filter(|e| match e { |
||||
Const(c) if relative_eq!(*c, 0.) => false, |
||||
_ => true, |
||||
}) |
||||
.collect() |
||||
} |
||||
|
||||
fn product_fold(l: Expr, r: Expr) -> Expr { |
||||
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 |
||||
} else { |
||||
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 es2 = es.clone(); |
||||
let mut consts: Option<Scalar> = None; |
||||
let mut other = Exprs::new(); |
||||
for e in es { |
||||
let unkns = e.unknowns(); |
||||
match e { |
||||
Const(c) => match consts { |
||||
None => consts = Some(c), |
||||
Some(cs) => consts = Some(c * cs), |
||||
}, |
||||
e => other.push(e), |
||||
} |
||||
} |
||||
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 { |
||||
let es_pre = es.clone(); |
||||
use itertools::Itertools; |
||||
use Expr::*; |
||||
for e in &mut es { |
||||
*e = e.clone().distribute(); |
||||
} |
||||
let sums = es |
||||
.drain_filter(|e| match e { |
||||
Sum(_) => true, |
||||
_ => false, |
||||
}) |
||||
.map(|e| { |
||||
trace!("sum in product: {}", e); |
||||
match e.simplify() { |
||||
Sum(es) => es, |
||||
o => vec![o], |
||||
} |
||||
}); |
||||
let products: Vec<_> = sums.multi_cartesian_product().collect(); |
||||
if products.is_empty() { |
||||
trace!("distribute_product_sums: no sums to distribute"); |
||||
return Product(es); |
||||
} |
||||
let sums = products |
||||
.into_iter() |
||||
.map(|mut prod| { |
||||
prod.extend(es.clone()); |
||||
trace!("prod: {}", Product(prod.clone())); |
||||
Product(prod) |
||||
}) |
||||
.collect(); |
||||
let res = Sum(sums); |
||||
trace!("distribute_product_sums: {} => {}", Product(es_pre), res); |
||||
res |
||||
} |
||||
|
||||
impl Unknowns for Expr { |
||||
fn unknowns(&self) -> UnknownSet { |
||||
use Expr::*; |
||||
match self { |
||||
Unkn(u) => u.unknowns(), |
||||
Const(_) => UnknownSet::default(), |
||||
Sum(es) | Product(es) => es.unknowns(), |
||||
Div(l, r) => l.unknowns().union(&r.unknowns()).cloned().collect(), |
||||
Neg(e) => e.unknowns(), |
||||
} |
||||
} |
||||
fn has_unknowns(&self) -> bool { |
||||
use Expr::*; |
||||
match self { |
||||
Unkn(u) => u.has_unknowns(), |
||||
Const(_) => false, |
||||
Sum(es) | Product(es) => es.has_unknowns(), |
||||
Div(l, r) => l.has_unknowns() || r.has_unknowns(), |
||||
Neg(e) => e.has_unknowns(), |
||||
} |
||||
} |
||||
fn has_unknown(&self, u: Unknown) -> bool { |
||||
use Expr::*; |
||||
match self { |
||||
Unkn(u1) => u1.has_unknown(u), |
||||
Const(_) => false, |
||||
Sum(es) | Product(es) => es.has_unknown(u), |
||||
Div(l, r) => l.has_unknown(u) || r.has_unknown(u), |
||||
Neg(e) => e.has_unknown(u), |
||||
} |
||||
} |
||||
} |
||||
|
||||
impl Expr { |
||||
pub fn new_sum(e1: Expr, e2: Expr) -> Expr { |
||||
Expr::Sum(vec![e1, e2]) |
||||
} |
||||
pub fn new_product(e1: Expr, e2: Expr) -> Expr { |
||||
Expr::Product(vec![e1, e2]) |
||||
} |
||||
pub fn new_neg(e1: Expr) -> Expr { |
||||
Expr::Neg(Box::new(e1)) |
||||
} |
||||
pub fn new_div(num: Expr, den: Expr) -> Expr { |
||||
Expr::Div(Box::new(num), Box::new(den)) |
||||
} |
||||
pub fn new_minus(e1: Expr, e2: Expr) -> Expr { |
||||
Expr::Sum(vec![e1, Expr::new_neg(e2)]) |
||||
} |
||||
pub fn new_inv(den: Expr) -> Expr { |
||||
Expr::new_div(Expr::Const(1.), den) |
||||
} |
||||
|
||||
pub fn is_zero(&self) -> bool { |
||||
use Expr::*; |
||||
match self.clone().simplify() { |
||||
Const(c) => relative_eq!(c, 0.), |
||||
_ => false, |
||||
} |
||||
} |
||||
|
||||
pub fn is_one(&self) -> bool { |
||||
use Expr::*; |
||||
match self.clone().simplify() { |
||||
Const(c) => relative_eq!(c, 1.), |
||||
_ => false, |
||||
} |
||||
} |
||||
|
||||
pub fn substitute<'a>(self, eqns: &Eqns<'a>) -> Expr { |
||||
use Expr::*; |
||||
for eqn in (&eqns.0 as &Borrow<[Eqn]>).borrow() { |
||||
if self == eqn.0 { |
||||
return eqn.1.clone(); |
||||
} |
||||
} |
||||
match self { |
||||
Sum(mut es) => { |
||||
for e in &mut es { |
||||
*e = e.clone().substitute(eqns); |
||||
} |
||||
Sum(es) |
||||
} |
||||
Product(mut es) => { |
||||
for e in &mut es { |
||||
*e = e.clone().substitute(eqns); |
||||
} |
||||
Product(es) |
||||
} |
||||
Neg(mut e) => { |
||||
*e = e.substitute(eqns); |
||||
Neg(e) |
||||
} |
||||
Div(mut num, mut den) => { |
||||
*num = num.substitute(eqns); |
||||
*den = den.substitute(eqns); |
||||
Div(num, den) |
||||
} |
||||
other => other, |
||||
} |
||||
} |
||||
|
||||
pub fn collapse(self) -> Expr { |
||||
use Expr::*; |
||||
match self { |
||||
Sum(es) => { |
||||
let mut new_es: Vec<_> = es |
||||
.into_iter() |
||||
.map(|e| e.collapse()) |
||||
.flat_map(|e| match e { |
||||
Sum(more_es) => more_es, |
||||
other => vec![other], |
||||
}) |
||||
.collect(); |
||||
|
||||
let consts = new_es.drain_filter(|e| match e { |
||||
Const(_) => true, |
||||
_ => false |
||||
}).fold(Const(0.), |l, r| match (l, r) { |
||||
(Const(lc), Const(rc)) => Const(lc + rc), |
||||
_ => unreachable!(), |
||||
}); |
||||
new_es.push(consts); |
||||
|
||||
match new_es.len() { |
||||
0 => Const(0.), // none
|
||||
1 => new_es.into_iter().next().unwrap(), // one
|
||||
_ => Sum(new_es), // many
|
||||
} |
||||
} |
||||
Product(es) => { |
||||
let new_es: Vec<_> = es |
||||
.into_iter() |
||||
.map(|e| e.collapse()) |
||||
.flat_map(|e| match e { |
||||
Product(more_es) => more_es, |
||||
other => vec![other], |
||||
}) |
||||
.collect(); |
||||
match new_es.len() { |
||||
0 => Const(1.), // none
|
||||
1 => new_es.into_iter().next().unwrap(), // one
|
||||
_ => { |
||||
if new_es.iter().any(|e| e.is_zero()) { |
||||
Const(0.) |
||||
} else { |
||||
Product(new_es) |
||||
} |
||||
}, // many
|
||||
} |
||||
} |
||||
Neg(mut v) => { |
||||
*v = v.collapse(); |
||||
match v { |
||||
box Const(c) => Const(-c), |
||||
box Neg(v) => *v, |
||||
box Product(mut es) => { |
||||
es.push(Const(-1.)); |
||||
Product(es).collapse() |
||||
} |
||||
e => Product(vec![Const(-1.), *e]).collapse(), |
||||
} |
||||
} |
||||
Div(mut num, mut den) => { |
||||
*num = num.collapse(); |
||||
*den = den.collapse(); |
||||
match (num, den) { |
||||
(box Const(num), box Const(den)) => Const(num / den), |
||||
(box Const(num), den) => { |
||||
if relative_eq!(num, 0.) { |
||||
Const(0.) |
||||
} else { |
||||
Div(Box::new(Const(num)), den) |
||||
} |
||||
} |
||||
(num, box Const(den)) => { |
||||
if relative_eq!(den, 1.) { |
||||
*num |
||||
} else { |
||||
Expr::new_product(*num, Const(1. / den)).collapse() |
||||
} |
||||
} |
||||
(num, box Div(dennum, denden)) => { |
||||
Div(Box::new(Product(vec![*num, *denden])), dennum).collapse() |
||||
} |
||||
(box Product(mut es), box den) => match es.remove_item(&den) { |
||||
Some(_) => Product(es), |
||||
None => Expr::new_div(Product(es), den), |
||||
}, |
||||
(num, den) => { |
||||
if num == den { |
||||
Expr::Const(1.) |
||||
} else { |
||||
Div(num, den) |
||||
} |
||||
} |
||||
} |
||||
} |
||||
e => e, |
||||
} |
||||
} |
||||
|
||||
pub fn simplify(self) -> 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()) |
||||
.flat_map(|e| match e { |
||||
Sum(more_es) => more_es, |
||||
other => vec![other], |
||||
}) |
||||
.collect(); |
||||
new_es = group_sum(new_es); |
||||
trace!( |
||||
"simplify sum {} => {}", |
||||
Sum(pre_new_es), |
||||
Sum(new_es.clone()) |
||||
); |
||||
|
||||
match new_es.len() { |
||||
0 => Const(0.), // none
|
||||
1 => new_es.into_iter().next().unwrap(), // one
|
||||
_ => Sum(new_es), // many
|
||||
} |
||||
} |
||||
Product(es) => { |
||||
let pre_new_es = es.clone(); |
||||
let new_es: Vec<_> = es |
||||
.into_iter() |
||||
.map(|e| e.simplify()) |
||||
.flat_map(|e| match e { |
||||
Product(more_es) => more_es, |
||||
other => vec![other], |
||||
}) |
||||
.collect(); |
||||
let new_es = group_product(new_es); |
||||
trace!( |
||||
"simplify product {} => {}", |
||||
Product(pre_new_es), |
||||
Product(new_es.clone()) |
||||
); |
||||
match new_es.len() { |
||||
0 => Const(1.), // none
|
||||
1 => new_es.into_iter().next().unwrap(), // one
|
||||
_ => Product(new_es), // many
|
||||
} |
||||
} |
||||
Neg(mut v) => { |
||||
*v = v.simplify(); |
||||
trace!("simplify neg {}", Neg(v.clone())); |
||||
match v { |
||||
box Const(c) => Const(-c), |
||||
box Neg(v) => *v, |
||||
box Product(mut es) => { |
||||
es.push(Const(-1.)); |
||||
Product(es).simplify() |
||||
} |
||||
e => Product(vec![Const(-1.), *e]).simplify(), |
||||
} |
||||
} |
||||
Div(mut num, mut den) => { |
||||
*num = num.simplify(); |
||||
*den = den.simplify(); |
||||
trace!("simplify div {}", Div(num.clone(), den.clone())); |
||||
match (num, den) { |
||||
(box Const(num), box Const(den)) => Const(num / den), |
||||
(num, box Const(den)) => { |
||||
if relative_eq!(den, 1.) { |
||||
*num |
||||
} else { |
||||
Expr::new_product(*num, Const(1. / den)).simplify() |
||||
} |
||||
} |
||||
(num, box Div(dennum, denden)) => { |
||||
Div(Box::new(Product(vec![*num, *denden])), dennum).simplify() |
||||
} |
||||
(box Product(mut es), box den) => match es.remove_item(&den) { |
||||
Some(_) => Product(es).simplify(), |
||||
None => Expr::new_div(Product(es), den), |
||||
}, |
||||
(num, den) => { |
||||
if num == den { |
||||
Expr::Const(1.) |
||||
} else { |
||||
Div(num, den) |
||||
} |
||||
} |
||||
} |
||||
} |
||||
Const(c) => if c.is_nan() { |
||||
println!("NAN!"); |
||||
Const(c) |
||||
} else { Const(c) }, |
||||
e => e, |
||||
} |
||||
} |
||||
|
||||
pub fn distribute(self) -> Expr { |
||||
use Expr::*; |
||||
match self { |
||||
Sum(mut es) => { |
||||
let es_pre = es.clone(); |
||||
for e in &mut es { |
||||
*e = e.clone().distribute(); |
||||
} |
||||
let res = Sum(es); |
||||
trace!("distribute sum {} => {}", Sum(es_pre), res); |
||||
res |
||||
} |
||||
Product(es) => distribute_product_sums(es), |
||||
Div(num, den) => match (num, den) { |
||||
(box Sum(es), box den) => Sum(es |
||||
.into_iter() |
||||
.map(|e| Expr::new_div(e, den.clone()).distribute()) |
||||
.collect()), |
||||
(num, den) => Div(num, den), |
||||
}, |
||||
Neg(v) => match v { |
||||
box Const(c) => Const(-c), |
||||
box Sum(mut es) => { |
||||
for e in &mut es { |
||||
*e = Expr::new_neg(e.clone()).distribute(); |
||||
} |
||||
Sum(es) |
||||
} |
||||
box Product(mut es) => { |
||||
for e in &mut es { |
||||
*e = e.clone().distribute(); |
||||
} |
||||
es.push(Const(-1.)); |
||||
Product(es) |
||||
} |
||||
box Neg(v) => v.distribute(), |
||||
box Div(mut num, mut den) => { |
||||
*num = Neg(num.clone()); |
||||
Div(num, den).distribute() |
||||
} |
||||
e => Neg(Box::new(e.distribute())), |
||||
}, |
||||
e => e, |
||||
} |
||||
} |
||||
} |
||||
|
||||
impl fmt::Display for Expr { |
||||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { |
||||
use Expr::*; |
||||
match self { |
||||
Unkn(u) => write!(f, "{}", u), |
||||
Const(c) => write!(f, "{}", c), |
||||
Sum(es) => write_separated_exprs(es, f, " + "), |
||||
Product(es) => write_separated_exprs(es, f, " * "), |
||||
Div(num, den) => write!(f, "({} / {})", num, den), |
||||
Neg(e) => write!(f, "-({})", e), |
||||
} |
||||
} |
||||
} |
||||
@ -1,23 +1,217 @@
@@ -1,23 +1,217 @@
|
||||
pub mod eqn; |
||||
pub mod expr; |
||||
pub mod ops; |
||||
pub mod region; |
||||
pub mod unknown; |
||||
pub mod vec; |
||||
|
||||
pub use eqn::{Eqn, Eqns}; |
||||
pub use expr::{Expr, Exprs}; |
||||
pub use ops::*; |
||||
pub use region::{GenericRegion, Line2, Region, Region1, Region2}; |
||||
pub use unknown::{Unknown, UnknownSet, Unknowns}; |
||||
pub use vec::*; |
||||
|
||||
pub type Scalar = f64; |
||||
|
||||
// #[derive(Clone, Copy, PartialEq, Debug)]
|
||||
// pub enum Value {
|
||||
// Known(Scalar),
|
||||
// Unkn(Unknown),
|
||||
// }
|
||||
pub type Vec2 = nalgebra::Vector2<Scalar>; |
||||
pub type Point2 = nalgebra::Point2<Scalar>; |
||||
|
||||
pub type Value = Expr; |
||||
pub type Rot2 = nalgebra::UnitComplex<Scalar>; |
||||
|
||||
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) |
||||
} |
||||
|
||||
fn contains(&self, n: &Scalar) -> bool { |
||||
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, |
||||
} |
||||
} |
||||
|
||||
fn nearest(&self, s: &Scalar) -> Option<Scalar> { |
||||
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), |
||||
_ => None, |
||||
}, |
||||
Union(r1, r2) => { |
||||
let distance = |a: Scalar, b: Scalar| (a - b).abs(); |
||||
match (r1.nearest(s), r2.nearest(s)) { |
||||
(None, None) => None, |
||||
(Some(n), None) | (None, Some(n)) => Some(n), |
||||
(Some(n1), Some(n2)) => Some({ |
||||
if distance(*s, n1) <= distance(*s, n2) { |
||||
n1 |
||||
} else { |
||||
n2 |
||||
} |
||||
}), |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
// line starting at start, point at angle dir, with range extent
|
||||
// ie. start + (cos dir, sin dir) * t for t in extent
|
||||
#[derive(Clone, Debug)] |
||||
pub struct Line2 { |
||||
start: Point2, |
||||
dir: Rot2, |
||||
extent: Region1, |
||||
} |
||||
|
||||
impl Line2 { |
||||
pub fn new(start: Point2, dir: Rot2, extent: Region1) -> Self { |
||||
Self { start, dir, extent } |
||||
} |
||||
|
||||
pub fn evaluate(&self, t: Scalar) -> Point2 { |
||||
self.start + self.dir * Vec2::new(t, 0.) |
||||
} |
||||
|
||||
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); |
||||
let (a_0, a_v, b_0, b_v) = (a.start, a.dir, b.start, b.dir); |
||||
let (a_c, a_s, b_c, b_s) = ( |
||||
a_v.cos_angle(), |
||||
a_v.sin_angle(), |
||||
b_v.cos_angle(), |
||||
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); |
||||
Region2::Singleton(b.evaluate(t_b)) |
||||
} |
||||
} |
||||
|
||||
#[derive(Clone, Debug)] |
||||
pub enum Region2 { |
||||
Empty, |
||||
// single point at 0
|
||||
Singleton(Point2), |
||||
Line(Line2), |
||||
#[allow(dead_code)] |
||||
Union(Box<Region2>, Box<Region2>), |
||||
Full, |
||||
} |
||||
|
||||
impl Region<Point2> 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| relative_eq!(n, p)) |
||||
} |
||||
|
||||
fn nearest(&self, p: &Point2) -> Option<Point2> { |
||||
use Region2::*; |
||||
match self { |
||||
Empty => None, |
||||
Full => Some(*p), |
||||
Singleton(n) => Some(*n), |
||||
Line(line) => Some(line.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 Region2 { |
||||
pub fn union(r1: Region2, r2: Region2) -> Region2 { |
||||
use Region2::*; |
||||
match (r1, r2) { |
||||
(Empty, r) | (r, Empty) => r, |
||||
(Full, _) | (_, Full) => Full, |
||||
(r1, r2) => Union(Box::new(r1), Box::new(r2)), |
||||
} |
||||
} |
||||
|
||||
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) |
||||
} else { |
||||
Empty |
||||
} |
||||
} |
||||
(Singleton(n), o) | (o, Singleton(n)) => { |
||||
if o.contains(n) { |
||||
Singleton(*n) |
||||
} else { |
||||
Empty |
||||
} |
||||
} |
||||
(Line(l1), Line(l2)) => l1.intersect(l2), |
||||
(Union(un1, un2), o) | (o, Union(un1, un2)) => { |
||||
Self::union(un1.intersect(o), un2.intersect(o)) |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
@ -1,197 +0,0 @@
@@ -1,197 +0,0 @@
|
||||
use std::ops; |
||||
|
||||
use super::{Expr, Scalar, Unknown}; |
||||
|
||||
impl From<Scalar> for Expr { |
||||
fn from(c: Scalar) -> Expr { |
||||
Expr::Const(c) |
||||
} |
||||
} |
||||
|
||||
impl From<Unknown> for Expr { |
||||
fn from(u: Unknown) -> Expr { |
||||
Expr::Unkn(u) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Expr> for Expr { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Expr) -> Expr { |
||||
Expr::new_sum(self, rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Scalar> for Expr { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Scalar) -> Expr { |
||||
Expr::new_sum(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Unknown> for Expr { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Unknown) -> Expr { |
||||
Expr::new_sum(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Expr> for Expr { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Expr) -> Expr { |
||||
Expr::new_minus(self, rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Scalar> for Expr { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Scalar) -> Expr { |
||||
Expr::new_minus(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Unknown> for Expr { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Unknown) -> Expr { |
||||
Expr::new_minus(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Expr> for Expr { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Expr) -> Expr { |
||||
Expr::new_product(self, rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Scalar> for Expr { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Scalar) -> Expr { |
||||
Expr::new_product(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Unknown> for Expr { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Unknown) -> Expr { |
||||
Expr::new_product(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Expr> for Expr { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Expr) -> Expr { |
||||
Expr::new_div(self, rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Scalar> for Expr { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Scalar) -> Expr { |
||||
Expr::new_div(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Unknown> for Expr { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Unknown) -> Expr { |
||||
Expr::new_div(self, rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Neg for Expr { |
||||
type Output = Expr; |
||||
fn neg(self) -> Expr { |
||||
Expr::new_neg(self) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Expr> for Unknown { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Expr) -> Expr { |
||||
Expr::new_sum(self.into(), rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Scalar> for Unknown { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Scalar) -> Expr { |
||||
Expr::new_sum(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Add<Unknown> for Unknown { |
||||
type Output = Expr; |
||||
fn add(self, rhs: Unknown) -> Expr { |
||||
Expr::new_sum(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Expr> for Unknown { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Expr) -> Expr { |
||||
Expr::new_minus(self.into(), rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Scalar> for Unknown { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Scalar) -> Expr { |
||||
Expr::new_minus(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Sub<Unknown> for Unknown { |
||||
type Output = Expr; |
||||
fn sub(self, rhs: Unknown) -> Expr { |
||||
Expr::new_minus(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Expr> for Unknown { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Expr) -> Expr { |
||||
Expr::new_product(self.into(), rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Scalar> for Unknown { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Scalar) -> Expr { |
||||
Expr::new_product(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Mul<Unknown> for Unknown { |
||||
type Output = Expr; |
||||
fn mul(self, rhs: Unknown) -> Expr { |
||||
Expr::new_product(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Expr> for Unknown { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Expr) -> Expr { |
||||
Expr::new_div(self.into(), rhs) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Scalar> for Unknown { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Scalar) -> Expr { |
||||
Expr::new_div(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Div<Unknown> for Unknown { |
||||
type Output = Expr; |
||||
fn div(self, rhs: Unknown) -> Expr { |
||||
Expr::new_div(self.into(), rhs.into()) |
||||
} |
||||
} |
||||
|
||||
impl ops::Neg for Unknown { |
||||
type Output = Expr; |
||||
fn neg(self) -> Expr { |
||||
Expr::new_neg(self.into()) |
||||
} |
||||
} |
||||
@ -1,437 +0,0 @@
@@ -1,437 +0,0 @@
|
||||
use std::fmt; |
||||
|
||||
use super::{eqn, Expr, Point2, Rot2, Scalar, Value}; |
||||
|
||||
// pub type Vec2 = nalgebra::Vector2<Value>;
|
||||
// pub type Point2 = nalgebra::Point2<Value>;
|
||||
|
||||
// pub type Rot2 = nalgebra::UnitComplex<Value>;
|
||||
|
||||
pub trait GenericRegion { |
||||
fn full() -> Self; |
||||
fn intersection(self, other: Self) -> Self; |
||||
fn simplify(self) -> Self; |
||||
fn substitute(self, eqns: &eqn::Eqns) -> Self; |
||||
} |
||||
|
||||
pub trait Region<T>: GenericRegion { |
||||
fn singleton(value: T) -> Self; |
||||
|
||||
fn nearest(&self, value: &T) -> Option<T>; |
||||
fn contains(&self, value: &T) -> Option<bool>; |
||||
} |
||||
|
||||
#[derive(Clone, Debug)] |
||||
pub enum Region1 { |
||||
Empty, |
||||
Singleton(Value), |
||||
Range(Value, Value), |
||||
Intersection(Box<Region1>, Box<Region1>), |
||||
// Union(Box<Region1>, Box<Region1>),
|
||||
Full, |
||||
} |
||||
|
||||
impl fmt::Display for Region1 { |
||||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { |
||||
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 GenericRegion for Region1 { |
||||
fn intersection(self, other: Region1) -> Self { |
||||
Region1::Intersection(Box::new(self), Box::new(other)) |
||||
} |
||||
|
||||
fn full() -> Self { |
||||
Region1::Full |
||||
} |
||||
|
||||
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, |
||||
} |
||||
} |
||||
|
||||
fn substitute(self, eqns: &eqn::Eqns) -> Self { |
||||
use Region1::*; |
||||
match self { |
||||
Singleton(n) => Singleton(n.substitute(eqns)), |
||||
Range(l, u) => Range(l.substitute(eqns), u.substitute(eqns)), |
||||
Intersection(r1, r2) => r1.substitute(eqns).intersection(r2.substitute(eqns)), |
||||
other => other, |
||||
} |
||||
} |
||||
} |
||||
|
||||
impl Region<Scalar> for Region1 { |
||||
fn singleton(value: Scalar) -> Self { |
||||
Region1::Singleton(value.into()) |
||||
} |
||||
|
||||
fn contains(&self, n: &Scalar) -> Option<bool> { |
||||
use Expr::Const; |
||||
use Region1::*; |
||||
match self { |
||||
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<Scalar> { |
||||
use Expr::Const; |
||||
use Region1::*; |
||||
match self { |
||||
Empty => None, |
||||
Full => Some(*s), |
||||
Singleton(n) => match n { |
||||
Const(c) => Some(*c), |
||||
_ => None, |
||||
}, |
||||
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, |
||||
(Some(n), None) | (None, Some(n)) => Some(n), |
||||
(Some(n1), Some(n2)) => Some({ |
||||
if distance(*s, n1) <= distance(*s, n2) { |
||||
n1 |
||||
} else { |
||||
n2 |
||||
} |
||||
}), |
||||
} |
||||
}*/ |
||||
} |
||||
} |
||||
} |
||||
|
||||
// line starting at start, point at angle dir, with range extent
|
||||
// ie. start + (cos dir, sin dir) * t for t in extent
|
||||
#[derive(Clone, Debug)] |
||||
pub struct Line2 { |
||||
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, t): ⟨x, y⟩ = {} + t{} & t ∈ {} }}", |
||||
self.start, self.dir, self.extent |
||||
) |
||||
} |
||||
} |
||||
|
||||
impl Line2 { |
||||
pub fn new(start: Point2<Value>, dir: Rot2, extent: Region1) -> Self { |
||||
Self { start, dir, extent } |
||||
} |
||||
|
||||
pub fn evaluate(&self, t: Value) -> Point2<Value> { |
||||
self.start.clone() + self.dir * t |
||||
} |
||||
|
||||
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 + Rot2::cardinal(1); |
||||
let perp = Line2::new(p.clone(), perp_dir, Region1::Full); |
||||
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 - other.dir; |
||||
if relative_eq!(dirs.sin(), 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, |
||||
b.start.clone(), |
||||
b.dir, |
||||
); |
||||
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 |
||||
- a_0.y.clone() * a_c |
||||
- b_0.x.clone() * a_s |
||||
+ b_0.y.clone() * a_c) |
||||
/ (a_s * b_c - a_c * b_s); |
||||
// Region2::Singleton(b.evaluate(t_b))
|
||||
trace!("intersect a: {}, b: {}, t_b = {}", a, b, 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) |
||||
} |
||||
|
||||
pub fn substitute(self, eqns: &eqn::Eqns) -> Self { |
||||
Line2 { |
||||
start: self.start.substitute(eqns), |
||||
dir: self.dir, |
||||
extent: self.extent.substitute(eqns), |
||||
} |
||||
} |
||||
} |
||||
|
||||
#[derive(Clone, Debug)] |
||||
pub enum Region2 { |
||||
Empty, |
||||
// single point at 0
|
||||
Singleton(Point2<Value>), |
||||
Line(Line2), |
||||
// #[allow(dead_code)]
|
||||
// Union(Box<Region2>, Box<Region2>),
|
||||
Intersection(Box<Region2>, Box<Region2>), |
||||
Full, |
||||
} |
||||
|
||||
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 GenericRegion for Region2 { |
||||
fn full() -> Self { |
||||
Region2::Full |
||||
} |
||||
|
||||
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)), |
||||
} |
||||
} |
||||
|
||||
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, |
||||
} |
||||
} |
||||
|
||||
fn substitute(self, eqns: &eqn::Eqns) -> Self { |
||||
use Region2::*; |
||||
match self { |
||||
Singleton(n) => Singleton(n.substitute(eqns)), |
||||
Line(l) => Line(l.substitute(eqns)), |
||||
Intersection(r1, r2) => r1.substitute(eqns).intersection(r2.substitute(eqns)), |
||||
other => other, |
||||
} |
||||
} |
||||
} |
||||
|
||||
impl Region<Point2<Scalar>> for Region2 { |
||||
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 singleton(value: Point2<Value>) -> Self { |
||||
Region2::Singleton(value) |
||||
} |
||||
|
||||
fn contains(&self, p: &Point2<Value>) -> Option<bool> { |
||||
self.nearest(p) |
||||
.map(|n| n.simplify() == p.clone().simplify()) |
||||
} |
||||
|
||||
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)), |
||||
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 |
||||
} |
||||
}), |
||||
} |
||||
}*/ |
||||
} |
||||
} |
||||
} |
||||
|
||||
impl Region2 { |
||||
/* |
||||
pub fn union(r1: Region2, r2: Region2) -> Region2 { |
||||
use Region2::*; |
||||
match (r1, r2) { |
||||
(Empty, r) | (r, Empty) => r, |
||||
(Full, _) | (_, Full) => Full, |
||||
(r1, r2) => Union(Box::new(r1), Box::new(r2)), |
||||
} |
||||
} |
||||
*/ |
||||
|
||||
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) |
||||
} else { |
||||
Region2::intersection(Singleton(n1), Singleton(n2)) |
||||
} |
||||
} |
||||
(Singleton(n), o) | (o, Singleton(n)) => { |
||||
if o.contains(&n).unwrap_or(false) { |
||||
Singleton(n) |
||||
} else { |
||||
Region2::intersection(Singleton(n), 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)), |
||||
} |
||||
} |
||||
} |
||||
@ -1,47 +0,0 @@
@@ -1,47 +0,0 @@
|
||||
use std::collections::BTreeSet; |
||||
use std::fmt; |
||||
use std::iter::FromIterator; |
||||
|
||||
use super::Scalar; |
||||
|
||||
// an unknown variable with an id
|
||||
#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)] |
||||
pub struct Unknown(pub i64); |
||||
|
||||
pub type UnknownSet = BTreeSet<Unknown>; |
||||
|
||||
pub trait Unknowns { |
||||
fn unknowns(&self) -> UnknownSet; |
||||
fn has_unknowns(&self) -> bool; |
||||
fn has_unknown(&self, u: Unknown) -> bool; |
||||
} |
||||
|
||||
impl Unknowns for Scalar { |
||||
fn unknowns(&self) -> UnknownSet { |
||||
UnknownSet::new() |
||||
} |
||||
fn has_unknowns(&self) -> bool { |
||||
false |
||||
} |
||||
fn has_unknown(&self, _: Unknown) -> bool { |
||||
false |
||||
} |
||||
} |
||||
|
||||
impl Unknowns for Unknown { |
||||
fn unknowns(&self) -> UnknownSet { |
||||
FromIterator::from_iter(Some(*self)) |
||||
} |
||||
fn has_unknowns(&self) -> bool { |
||||
true |
||||
} |
||||
fn has_unknown(&self, u: Unknown) -> bool { |
||||
*self == u |
||||
} |
||||
} |
||||
|
||||
impl fmt::Display for Unknown { |
||||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { |
||||
write!(f, "u{}", self.0) |
||||
} |
||||
} |
||||
@ -1,291 +0,0 @@
@@ -1,291 +0,0 @@
|
||||
use super::eqn::Eqns; |
||||
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.distribute().simplify(), |
||||
y: self.y.distribute().simplify(), |
||||
} |
||||
} |
||||
|
||||
pub fn substitute(self, eqns: &Eqns) -> Self { |
||||
Self { |
||||
x: self.x.substitute(eqns), |
||||
y: self.y.substitute(eqns), |
||||
} |
||||
} |
||||
} |
||||
|
||||
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 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() |
||||
} |
||||
|
||||
pub fn from_angle(angle: Scalar) -> Self { |
||||
Self { |
||||
cos: angle.cos(), |
||||
sin: angle.sin(), |
||||
} |
||||
} |
||||
|
||||
pub fn from_angle_deg(angle_deg: Scalar) -> Self { |
||||
Self::from_angle(angle_deg * std::f64::consts::PI / 180.) |
||||
} |
||||
|
||||
pub fn cardinal(index: i64) -> Self { |
||||
match index % 4 { |
||||
0 => Rot2 { cos: 1., sin: 0. }, |
||||
1 => Rot2 { cos: 0., sin: 1. }, |
||||
2 => Rot2 { cos: -1., sin: 0. }, |
||||
3 => Rot2 { cos: 0., sin: -1. }, |
||||
_ => 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, |
||||
} |
||||
} |
||||
|
||||
pub fn dot(self, v: Vec2<Value>) -> Value { |
||||
v.x * self.cos + v.y * 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 * rhs.cos - self.sin * rhs.sin, |
||||
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 * rhs.cos + self.sin * rhs.sin, |
||||
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, |
||||
} |
||||
} |
||||
} |
||||
Loading…
Reference in new issue