418 lines
11 KiB
Rust
418 lines
11 KiB
Rust
use crate::point::Point;
|
|
static NEAR_ZERO: f64 = 0.000001;
|
|
|
|
use std::ops::Add;
|
|
use std::ops::Mul;
|
|
use std::ops::MulAssign;
|
|
use std::ops::Sub;
|
|
|
|
impl Add for Pt {
|
|
type Output = Self;
|
|
|
|
fn add(self, other: Self) -> Self {
|
|
Self {
|
|
x: self.x + other.x,
|
|
y: self.y + other.y,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Sub for Pt {
|
|
type Output = Self;
|
|
|
|
fn sub(self, other: Self) -> Self {
|
|
Self {
|
|
x: self.x - other.x,
|
|
y: self.y - other.y,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl MulAssign<f64> for Pt {
|
|
fn mul_assign(&mut self, rhs: f64) {
|
|
self.x *= rhs;
|
|
self.y *= rhs;
|
|
}
|
|
}
|
|
|
|
impl Mul<f64> for Pt {
|
|
type Output = Self;
|
|
|
|
fn mul(self, rhs: f64) -> Self {
|
|
Pt {
|
|
x: self.x * rhs,
|
|
y: self.y * rhs,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
|
|
pub struct Pt {
|
|
pub x: f64,
|
|
pub y: f64,
|
|
}
|
|
|
|
impl From<Pt> for (f64, f64) {
|
|
fn from(pt: Pt) -> Self {
|
|
(pt.x, pt.y)
|
|
}
|
|
}
|
|
|
|
impl From<(f64, f64)> for Pt {
|
|
fn from((x, y): (f64, f64)) -> Self {
|
|
Pt { x, y }
|
|
}
|
|
}
|
|
|
|
impl From<&(f64, f64)> for Pt {
|
|
fn from((x, y): &(f64, f64)) -> Self {
|
|
Pt { x: *x, y: *y }
|
|
}
|
|
}
|
|
|
|
impl Pt {
|
|
pub fn new(x: f64, y: f64) -> Self {
|
|
Self { x, y }
|
|
}
|
|
|
|
pub fn mul_assign(&mut self, rhs: f64) {
|
|
self.x *= rhs;
|
|
self.y *= rhs;
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct CartesianEquation {
|
|
pub a: f64,
|
|
pub b: f64,
|
|
pub c: f64,
|
|
}
|
|
|
|
#[allow(dead_code)]
|
|
impl CartesianEquation {
|
|
pub fn new_from_tuple(((x0, y0), (x1, y1)): ((f64, f64), (f64, f64))) -> Self {
|
|
let p0 = Pt { x: x0, y: y0 };
|
|
let p1 = Pt { x: x1, y: y1 };
|
|
|
|
CartesianEquation::new_from_pt(&p0, &p1)
|
|
}
|
|
|
|
pub fn new_from_pt(p0: &Pt, p1: &Pt) -> Self {
|
|
let a = p0.y - p1.y;
|
|
let b = p1.x - p0.x;
|
|
let c = -(a * p0.x + b * p0.y);
|
|
|
|
Self { a, b, c }
|
|
}
|
|
|
|
pub fn from_parametric_eq(pt: &Pt, dir: &Pt) -> Self {
|
|
let p0 = pt.clone();
|
|
let p1 = Pt {
|
|
x: pt.x + dir.x,
|
|
y: pt.y + dir.y,
|
|
};
|
|
CartesianEquation::new_from_pt(&p0, &p1)
|
|
}
|
|
|
|
pub fn new(a: f64, b: f64, c: f64) -> Self {
|
|
Self { a, b, c }
|
|
}
|
|
|
|
pub fn is_similar(&self, other: &Self) -> bool {
|
|
if self.b.abs() > NEAR_ZERO {
|
|
let y0 = -self.a / self.b;
|
|
let y1 = -other.a / other.b;
|
|
|
|
return (y0 - y1).abs() <= NEAR_ZERO;
|
|
} else {
|
|
let x0 = -self.b / self.a;
|
|
let x1 = -other.b / other.a;
|
|
|
|
return (x0 - x1).abs() <= NEAR_ZERO;
|
|
}
|
|
}
|
|
|
|
pub fn intersection(&self, other: &Self) -> Option<Pt> {
|
|
let (a0, b0, c0) = (self.a, self.b, self.c);
|
|
let (a1, b1, c1) = (other.a, other.b, other.c);
|
|
|
|
if self.is_similar(other) {
|
|
return None;
|
|
}
|
|
|
|
let x = (b0 * (c0 - c1) - c0 * (b0 - b1)) / (b0 * (a1 - a0) + a0 * (b0 - b1));
|
|
let y = (a0 * (c0 - c1) - c0 * (a0 - a1)) / (a0 * (b1 - b0) + b0 * (a0 - a1));
|
|
|
|
Some(Pt { x, y })
|
|
}
|
|
}
|
|
|
|
pub struct EqAffine {
|
|
pub a: f64,
|
|
pub b: f64,
|
|
pub dst: f64,
|
|
}
|
|
|
|
impl EqAffine {
|
|
pub fn new(a0: f64, b0: f64, a1: f64, b1: f64) -> Self {
|
|
let dx = a1 - a0;
|
|
let dy = b1 - b0;
|
|
let dst = (dx * dx + dy * dy).sqrt();
|
|
let a = dy / dx;
|
|
let b = b0 - a * a0;
|
|
EqAffine { a, b, dst }
|
|
}
|
|
|
|
pub fn get_val(&self, var: f64) -> f64 {
|
|
self.a * var + self.b
|
|
}
|
|
|
|
pub fn get_val_dst(&self, var: f64) -> f64 {
|
|
self.dst * self.get_val(var)
|
|
}
|
|
}
|
|
|
|
//#[derive(Serialize,Deserialize,Debug,Clone,Copy)]
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct Keystone {
|
|
pub p0: Pt,
|
|
pub p1: Pt,
|
|
pub p2: Pt,
|
|
pub p3: Pt,
|
|
pub fx: Option<Pt>,
|
|
pub fy: Option<Pt>,
|
|
pub factor: f64,
|
|
pub translate: f64,
|
|
}
|
|
|
|
#[allow(dead_code)]
|
|
impl Keystone {
|
|
pub fn new(
|
|
pt0: (f64, f64),
|
|
pt1: (f64, f64),
|
|
pt2: (f64, f64),
|
|
pt3: (f64, f64),
|
|
factor: f64,
|
|
translate: f64,
|
|
) -> Self {
|
|
let eqx1 = CartesianEquation::new_from_pt(&Pt::from(pt0), &Pt::from(pt1));
|
|
let eqx2 = CartesianEquation::new_from_pt(&Pt::from(pt3), &Pt::from(pt2));
|
|
|
|
let eqy1 = CartesianEquation::new_from_pt(&Pt::from(pt0), &Pt::from(pt3));
|
|
let eqy2 = CartesianEquation::new_from_pt(&Pt::from(pt1), &Pt::from(pt2));
|
|
|
|
let fx = eqx1.intersection(&eqx2);
|
|
let fy = eqy1.intersection(&eqy2);
|
|
|
|
Keystone {
|
|
p0: Pt::from(pt0),
|
|
p1: Pt::from(pt1),
|
|
p2: Pt::from(pt2),
|
|
p3: Pt::from(pt3),
|
|
fx,
|
|
fy,
|
|
factor,
|
|
translate,
|
|
}
|
|
}
|
|
|
|
pub fn transform(&self, pt: &Point) -> Point {
|
|
let x = (pt.x as f64 + self.translate) / self.factor;
|
|
let y = (pt.y as f64 + self.translate) / self.factor;
|
|
let ptx: Pt = Pt {
|
|
x: self.p0.x + x * (self.p1.x - self.p0.x),
|
|
y: self.p0.y + x * (self.p1.y - self.p0.y),
|
|
};
|
|
|
|
let pty: Pt = Pt {
|
|
x: self.p0.x + y * (self.p3.x - self.p0.x),
|
|
y: self.p0.y + y * (self.p3.y - self.p0.y),
|
|
};
|
|
|
|
let eqx = if self.fx.is_some() {
|
|
CartesianEquation::new_from_pt(&(self.fx.unwrap()), &pty)
|
|
} else {
|
|
let dir = Pt {
|
|
x: self.p1.x - self.p0.x,
|
|
y: self.p1.y - self.p0.y,
|
|
};
|
|
CartesianEquation::from_parametric_eq(&pty, &dir)
|
|
};
|
|
|
|
let eqy = if self.fy.is_some() {
|
|
CartesianEquation::new_from_pt(&(self.fy.unwrap()), &ptx)
|
|
} else {
|
|
let dir = Pt {
|
|
x: self.p3.x - self.p0.x,
|
|
y: self.p3.y - self.p0.y,
|
|
};
|
|
CartesianEquation::from_parametric_eq(&ptx, &dir)
|
|
};
|
|
|
|
let intersection = eqx.intersection(&eqy).unwrap();
|
|
|
|
Point {
|
|
x: intersection.x as f32,
|
|
y: intersection.y as f32,
|
|
..*pt
|
|
}
|
|
}
|
|
}
|
|
|
|
//impl Transformers for Keystone {
|
|
// fn apply(&self, point_list: &[Point]) -> Vec<Point> {
|
|
// point_list.iter()
|
|
// .map(| pt | {
|
|
// self.transform(pt)
|
|
// }).collect()
|
|
// }
|
|
//}
|
|
|
|
#[cfg(test)]
|
|
mod test_keystone_correction {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn simple_y_focal() {
|
|
let keystone = Keystone::new((0.1, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.);
|
|
let must_be = Pt::new(0.5, -4.0);
|
|
|
|
let x_are_similar = (must_be.x - keystone.fy.unwrap().x).abs() < NEAR_ZERO;
|
|
let y_are_similar = (must_be.y - keystone.fy.unwrap().y).abs() < NEAR_ZERO;
|
|
|
|
assert!(x_are_similar);
|
|
assert!(y_are_similar);
|
|
assert_eq!(None, keystone.fx);
|
|
}
|
|
|
|
#[test]
|
|
fn simple_x_focal() {
|
|
let keystone = Keystone::new((0.0, 0.0), (1.0, 0.1), (1.0, 0.9), (0.0, 1.0), 1., 0.);
|
|
let must_be = Pt::new(5., 0.5);
|
|
|
|
let x_are_similar = (must_be.x - keystone.fx.unwrap().x).abs() < NEAR_ZERO;
|
|
let y_are_similar = (must_be.y - keystone.fx.unwrap().y).abs() < NEAR_ZERO;
|
|
|
|
println!("keystone:{:?}", keystone);
|
|
assert!(x_are_similar);
|
|
assert!(y_are_similar);
|
|
assert_eq!(None, keystone.fy);
|
|
}
|
|
|
|
#[test]
|
|
fn zero_correction() {
|
|
// on change rien
|
|
let keystone = Keystone::new((0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (0.0, 1.0), 1., 0.);
|
|
let pt = Point::from((0.34, 0.2, 0));
|
|
let res = keystone.transform(&pt);
|
|
|
|
assert!(((pt.x - res.x).abs() as f64) < NEAR_ZERO);
|
|
assert!(((pt.y - res.y).abs() as f64) < NEAR_ZERO);
|
|
}
|
|
|
|
#[test]
|
|
fn x_correction_1() {
|
|
let keystone = Keystone::new((0.1, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.);
|
|
let pt = Point::from((0.1, 0., 0));
|
|
let res = keystone.transform(&pt);
|
|
let must_be = Point::from((0.18, 0., 0));
|
|
|
|
assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO);
|
|
assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO);
|
|
}
|
|
|
|
#[test]
|
|
fn x_correction_2() {
|
|
let keystone = Keystone::new((0.0, 0.0), (0.9, 0.0), (1.0, 1.0), (0.0, 1.0), 1.0, 0.);
|
|
let pt = Point::from((0.1, 0.1, 0));
|
|
let res = keystone.transform(&pt);
|
|
let must_be = Point::from((0.091, 0.1, 0));
|
|
|
|
assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO);
|
|
assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO);
|
|
}
|
|
|
|
#[test]
|
|
fn y_correction_1() {
|
|
let keystone = Keystone::new((0.0, 0.0), (1.0, 0.1), (1.0, 0.9), (0.0, 1.0), 1.0, 0.);
|
|
let pt = Point::from((0., 0.1, 0));
|
|
let res = keystone.transform(&pt);
|
|
let must_be = Point::from((0., 0.1, 0));
|
|
|
|
assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO);
|
|
assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO);
|
|
}
|
|
|
|
#[test]
|
|
fn y_correction_2() {
|
|
let keystone = Keystone::new((0.0, 0.0), (1.0, 0.0), (1.0, 0.9), (0.0, 1.0), 1.0, 0.);
|
|
let pt = Point::from((0.1, 0.1, 0));
|
|
let res = keystone.transform(&pt);
|
|
let must_be = Point::from((0.1, 0.099, 0));
|
|
|
|
assert!(((must_be.x - res.x).abs() as f64) < NEAR_ZERO);
|
|
assert!(((must_be.y - res.y).abs() as f64) < NEAR_ZERO);
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod test_cartesian_equation {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn similar_eq_1() {
|
|
let p0a = Pt::new(0.1, 0.0);
|
|
let p1a = Pt::new(0.9, 0.0);
|
|
let eq0 = CartesianEquation::new_from_pt(&p0a, &p1a);
|
|
|
|
let p0b = Pt::new(0.0, 1.0);
|
|
let p1b = Pt::new(1.0, 1.0);
|
|
let eq1 = CartesianEquation::new_from_pt(&p0b, &p1b);
|
|
|
|
assert!(eq0.is_similar(&eq1));
|
|
}
|
|
|
|
#[test]
|
|
fn similar_eq_2() {
|
|
let p0a = Pt::new(0.0, 0.0);
|
|
let p1a = Pt::new(0.0, 1.0);
|
|
let eq0 = CartesianEquation::new_from_pt(&p0a, &p1a);
|
|
|
|
let p0b = Pt::new(1.0, 0.1);
|
|
let p1b = Pt::new(1.0, 0.9);
|
|
let eq1 = CartesianEquation::new_from_pt(&p0b, &p1b);
|
|
|
|
assert!(eq0.is_similar(&eq1));
|
|
}
|
|
|
|
#[test]
|
|
fn paralle_value() {
|
|
let (p0, p1) = (Pt { x: 0., y: 0. }, Pt { x: 1., y: 4. });
|
|
let (p2, p3) = (Pt { x: 1., y: 3. }, Pt { x: 0., y: -1. });
|
|
let eq0 = CartesianEquation::new_from_pt(&p0, &p1);
|
|
let eq1 = CartesianEquation::new_from_pt(&p2, &p3);
|
|
|
|
let res = eq0.intersection(&eq1);
|
|
|
|
assert_eq!(res, None);
|
|
}
|
|
|
|
#[test]
|
|
fn test_intersection() {
|
|
let d0 = Pt { x: 1., y: 1. };
|
|
let p0 = Pt { x: 0., y: 0. };
|
|
|
|
let d1 = Pt { x: -1., y: 1. };
|
|
let p1 = Pt { x: 4., y: 0. };
|
|
|
|
let must_be = Pt { x: 2., y: 2. };
|
|
|
|
let eq0 = CartesianEquation::from_parametric_eq(&p0, &d0);
|
|
let eq1 = CartesianEquation::from_parametric_eq(&p1, &d1);
|
|
|
|
let intersect = eq0.intersection(&eq1).unwrap();
|
|
|
|
assert_eq!(must_be, intersect);
|
|
}
|
|
}
|