Compare commits
12 Commits
master
...
eqn_relati
| Author | SHA1 | Date |
|---|---|---|
 Alex Mikhalev
|
8a33818790 | 6 years ago |
|
Alex Mikhalev
|
3abe5c6573 | 6 years ago |
|
Alex Mikhalev
|
a6c4906773 | 6 years ago |
|
Alex Mikhalev
|
233c8eaad5 | 6 years ago |
|
Alex Mikhalev
|
461088da3a | 6 years ago |
|
Alex Mikhalev
|
dc674dbce0 | 6 years ago |
|
Alex Mikhalev
|
f9b1f8924c | 6 years ago |
|
Alex Mikhalev
|
157535b5ff | 6 years ago |
|
Alex Mikhalev
|
8da067fec2 | 6 years ago |
|
Alex Mikhalev
|
2d5d2fda4b | 6 years ago |
|
Alex Mikhalev
|
556719f52d | 6 years ago |
|
Alex Mikhalev
|
6cede1f2dd | 6 years ago |
11 changed files with 1729 additions and 839 deletions
@ -0,0 +1,460 @@ |
|||||||
|
use std::collections::BTreeMap; |
||||||
|
use std::fmt; |
||||||
|
|
||||||
|
use super::eqn::Eqns; |
||||||
|
use super::unknown::{Unknown, UnknownSet, Unknowns}; |
||||||
|
use super::Scalar; |
||||||
|
|
||||||
|
#[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: &Exprs, f: &mut fmt::Formatter, sep: &str) -> fmt::Result { |
||||||
|
let mut is_first = true; |
||||||
|
write!(f, "(")?; |
||||||
|
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))) |
||||||
|
} |
||||||
|
} |
||||||
|
(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.))), |
||||||
|
None => Expr::new_sum(Product(l), r), |
||||||
|
} |
||||||
|
} |
||||||
|
(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 { |
||||||
|
let unkns = e.unknowns(); |
||||||
|
match common.get_mut(&unkns) { |
||||||
|
None => { |
||||||
|
match e { |
||||||
|
Const(c) if relative_eq!(c, 0.) => (), |
||||||
|
e => { |
||||||
|
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).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.simplify() { |
||||||
|
Const(c) => relative_eq!(c, 0.), |
||||||
|
_ => false, |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
pub fn is_one(self) -> bool { |
||||||
|
use Expr::*; |
||||||
|
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 { |
||||||
|
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]), |
||||||
|
} |
||||||
|
} |
||||||
|
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)) |
||||||
|
} |
||||||
|
} |
||||||
|
(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), |
||||||
|
None => Expr::new_div(Product(es), den), |
||||||
|
}, |
||||||
|
(num, den) => { |
||||||
|
if num == den { |
||||||
|
Expr::Const(1.) |
||||||
|
} else { |
||||||
|
Div(num, den) |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
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(mut num, mut den) => { |
||||||
|
*num = num.distribute(); |
||||||
|
*den = den.distribute(); |
||||||
|
match (num, den) { |
||||||
|
(box Sum(es), box den) => Sum(es |
||||||
|
.into_iter() |
||||||
|
.map(|e| Expr::new_div(e, den.clone())) |
||||||
|
.collect()), |
||||||
|
(mut num, mut den) => Div(num, den), |
||||||
|
} |
||||||
|
} |
||||||
|
Neg(v) => match v { |
||||||
|
// box Sum(mut l, mut r) => {
|
||||||
|
// *l = Neg(l.clone()).distribute();
|
||||||
|
// *r = Neg(r.clone()).distribute();
|
||||||
|
// Sum(l, r)
|
||||||
|
// }
|
||||||
|
// box Product(mut l, r) => {
|
||||||
|
// *l = Neg(l.clone()).distribute();
|
||||||
|
// Product(l, r)
|
||||||
|
// }
|
||||||
|
box Neg(v) => v.distribute(), |
||||||
|
box Div(mut num, mut den) => { |
||||||
|
*num = Neg(num.clone()).distribute(); |
||||||
|
*den = Neg(den.clone()).distribute(); |
||||||
|
Div(num, den) |
||||||
|
} |
||||||
|
e => Neg(e), |
||||||
|
}, |
||||||
|
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,217 +1,23 @@ |
|||||||
pub mod eqn; |
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; |
pub type Scalar = f64; |
||||||
|
|
||||||
pub type Vec2 = nalgebra::Vector2<Scalar>; |
// #[derive(Clone, Copy, PartialEq, Debug)]
|
||||||
pub type Point2 = nalgebra::Point2<Scalar>; |
// pub enum Value {
|
||||||
|
// Known(Scalar),
|
||||||
|
// Unkn(Unknown),
|
||||||
|
// }
|
||||||
|
|
||||||
pub type Rot2 = nalgebra::UnitComplex<Scalar>; |
pub type Value = Expr; |
||||||
|
|
||||||
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)) |
|
||||||
} |
|
||||||
} |
|
||||||
} |
|
||||||
} |
|
||||||
|
|||||||
@ -0,0 +1,197 @@ |
|||||||
|
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()) |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,437 @@ |
|||||||
|
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 evaluate_with(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 evaluate_with(self, eqns: &eqn::Eqns) -> Self { |
||||||
|
use Region1::*; |
||||||
|
match self { |
||||||
|
Singleton(n) => Singleton(n.evaluate_with(eqns)), |
||||||
|
Range(l, u) => Range(l.evaluate_with(eqns), u.evaluate_with(eqns)), |
||||||
|
Intersection(r1, r2) => r1.evaluate_with(eqns).intersection(r2.evaluate_with(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.clone() * 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.clone() + 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.clone() - other.dir.clone(); |
||||||
|
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.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() |
||||||
|
- 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))
|
||||||
|
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 evaluate_with(self, eqns: &eqn::Eqns) -> Self { |
||||||
|
Line2 { |
||||||
|
start: self.start.evaluate_with(eqns), |
||||||
|
dir: self.dir, |
||||||
|
extent: self.extent.evaluate_with(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 evaluate_with(self, eqns: &eqn::Eqns) -> Self { |
||||||
|
use Region2::*; |
||||||
|
match self { |
||||||
|
Singleton(n) => Singleton(n.evaluate_with(eqns)), |
||||||
|
Line(l) => Line(l.evaluate_with(eqns)), |
||||||
|
Intersection(r1, r2) => r1.evaluate_with(eqns).intersection(r2.evaluate_with(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)), |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,47 @@ |
|||||||
|
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) |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,303 @@ |
|||||||
|
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 evaluate_with(self, eqns: &Eqns) -> Self { |
||||||
|
Self { |
||||||
|
x: self.x.evaluate_with(eqns), |
||||||
|
y: self.y.evaluate_with(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().into(), |
||||||
|
sin: angle.sin().into(), |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
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.).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, |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
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.clone() * rhs.cos.clone() - self.sin.clone() * rhs.sin.clone(), |
||||||
|
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.clone() * rhs.cos.clone() + self.sin.clone() * rhs.sin.clone(), |
||||||
|
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