use crate::common::PCoords;
use crate::geom2::align2::{AlignStorage2, Dof3};
use crate::na::{Matrix2, Translation2, UnitComplex};
use crate::{Iso2, Point2, Vector2};
const SK2: Matrix2<f64> = Matrix2::new(0.0, -1.0, 1.0, 0.0);
#[derive(Clone, Debug)]
pub struct AlignValues2 {
pub transform: Iso2,
pub align: Iso2,
pub dof: Dof3,
pub dtx: Vector2,
pub dty: Vector2,
m_drz: Matrix2<f64>,
pre_rot: Iso2,
}
impl AlignValues2 {
pub fn drz(&self, point: &impl PCoords<2>) -> Vector2 {
self.m_drz * (self.pre_rot * Point2::from(point.coords())).coords()
}
}
#[derive(Clone, Copy, Debug)]
pub enum AlignOrigin2 {
Origin,
Center(Point2),
Local(Iso2),
}
#[derive(Clone, Debug)]
pub struct AlignParams2 {
pub local: Iso2,
pub dof: Dof3,
pub offset: Iso2,
storage: AlignStorage2,
}
impl AlignParams2 {
pub fn new(local: AlignOrigin2, offset: Option<Iso2>, dof: Option<Dof3>) -> Self {
let local = match local {
AlignOrigin2::Origin => Iso2::identity(),
AlignOrigin2::Center(p) => Iso2::translation(p.x, p.y),
AlignOrigin2::Local(t) => t,
};
let offset = offset.unwrap_or_else(Iso2::identity);
let dof = dof.unwrap_or_else(Dof3::all);
Self {
local,
dof,
offset,
storage: AlignStorage2::zeros(),
}
}
pub fn from_local(local: Iso2, dof: Option<Dof3>) -> Self {
Self::new(AlignOrigin2::Local(local), Some(local), dof)
}
pub fn from_center(center: Point2, dof: Option<Dof3>) -> Self {
let local = Iso2::translation(center.x, center.y);
Self::new(AlignOrigin2::Local(local), Some(local), dof)
}
pub fn from_origin(dof: Option<Dof3>) -> Self {
Self::new(AlignOrigin2::Origin, None, dof)
}
pub fn tx(&self) -> f64 {
self.storage[0]
}
pub fn ty(&self) -> f64 {
self.storage[1]
}
pub fn rz(&self) -> f64 {
self.storage[2]
}
pub fn compute_transform(&self) -> Iso2 {
self.compute_values().transform
}
pub fn compute_values(&self) -> AlignValues2 {
let align = align_from_storage(&self.storage);
let transform = self.offset * align * self.local.inverse();
let dtx = self.offset * Vector2::new(1.0, 0.0);
let dty = self.offset * Vector2::new(0.0, 1.0);
let pre_rot = align.translation.inverse() * self.offset.inverse();
let post_rot = self.offset.rotation.to_rotation_matrix();
AlignValues2 {
transform,
align,
dof: self.dof,
dtx,
dty,
m_drz: post_rot * SK2,
pre_rot,
}
}
pub fn storage(&self) -> AlignStorage2 {
self.storage
}
fn enforce_constraint(&mut self) {
if !self.dof.tx {
self.storage[0] = 0.0;
}
if !self.dof.ty {
self.storage[1] = 0.0;
}
if !self.dof.rz {
self.storage[2] = 0.0;
}
}
pub fn set_storage(&mut self, storage: AlignStorage2) {
self.storage = storage;
self.enforce_constraint();
}
pub fn set_index(&mut self, index: usize, value: f64) {
self.storage[index] = value;
self.enforce_constraint();
}
pub fn with_tx(&self, tx: f64) -> AlignParams2 {
self.with_index(0, tx)
}
pub fn with_ty(&self, ty: f64) -> AlignParams2 {
self.with_index(1, ty)
}
pub fn with_rz(&self, rz: f64) -> AlignParams2 {
self.with_index(2, rz)
}
fn with_index(&self, index: usize, value: f64) -> AlignParams2 {
let mut params = self.clone();
params.set_index(index, value);
params
}
pub fn with_storage(&self, storage: AlignStorage2) -> AlignParams2 {
let mut params = self.clone();
params.set_storage(storage);
params
}
}
fn align_from_storage(p: &AlignStorage2) -> Iso2 {
Iso2::from_parts(Translation2::new(p.x, p.y), UnitComplex::new(p.z))
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::RandomGeometry2;
use approx::assert_relative_eq;
use std::f64::consts::PI;
const ANGLE_EPSILON: f64 = 1e-8;
const TRANS_EPSILON: f64 = 1e-8;
#[test]
fn rotation_around_origin() {
let params = AlignParams2::from_origin(None).with_rz(PI / 2.0);
let test_point = Point2::new(1.0, 2.0);
let expected = Point2::new(-2.0, 1.0);
assert_relative_eq!(
expected,
params.compute_values().transform * test_point,
epsilon = 1e-8
);
}
#[test]
fn rotation_around_center() {
let rc = Point2::new(1.0, 0.0);
let params = AlignParams2::from_center(rc, None).with_rz(PI / 2.0);
let test_point = Point2::new(2.0, 2.0);
let expected = Point2::new(-1.0, 1.0);
assert_relative_eq!(
expected,
params.compute_values().transform * test_point,
epsilon = 1e-8
);
}
#[test]
fn translations_are_in_local_origin_directions() {
let local = Iso2::rotation(PI / 2.0);
let params = AlignParams2::from_local(local, None).with_tx(1.0);
let test_point = Point2::new(1.0, 2.0);
let expected = Point2::new(1.0, 3.0);
assert_relative_eq!(
expected,
params.compute_values().transform * test_point,
epsilon = 1e-8
);
}
#[test]
fn builders_respect_locked_dof() {
let dof = Dof3::new(false, true, true);
let params = AlignParams2::from_origin(Some(dof));
assert_eq!(params.with_tx(1.0).tx(), 0.0);
assert_eq!(params.with_ty(1.0).ty(), 1.0);
assert_eq!(params.with_rz(1.0).rz(), 1.0);
let all = params.with_storage(AlignStorage2::new(1.0, 2.0, 3.0));
assert_eq!(all.tx(), 0.0);
assert_eq!(all.ty(), 2.0);
assert_eq!(all.rz(), 3.0);
}
#[test]
fn partials_of_translations_at_zero() {
let params = AlignParams2::from_origin(None);
let current = params.compute_values();
assert_relative_eq!(current.dtx, Vector2::x_axis(), epsilon = 1e-12);
assert_relative_eq!(current.dty, Vector2::y_axis(), epsilon = 1e-12);
}
#[test]
fn partials_of_translations_with_rotation() {
let params = AlignParams2::from_origin(None).with_rz(0.3);
let test_point = Point2::new(1.0, 2.0);
let exp_x = finite_diff(¶ms, &test_point, 0);
let exp_y = finite_diff(¶ms, &test_point, 1);
let c = params.compute_values();
assert_relative_eq!(c.dtx, Vector2::x_axis(), epsilon = 1e-12);
assert_relative_eq!(c.dty, Vector2::y_axis(), epsilon = 1e-12);
assert_relative_eq!(exp_x, c.dtx, epsilon = 1e-8);
assert_relative_eq!(exp_y, c.dty, epsilon = 1e-8);
}
#[test]
fn stress_partials_translations() {
let mut rg = RandomGeometry2::new();
for _ in 0..1000 {
let local = rg.iso2(10.0);
let working = rg.iso2(10.0);
let params = AlignParams2::new(AlignOrigin2::Local(local), Some(working), None)
.with_storage(rg.vector(PI));
let test_point = rg.point(10.0);
let exp_x = finite_diff(¶ms, &test_point, 0);
let exp_y = finite_diff(¶ms, &test_point, 1);
let c = params.compute_values();
assert_relative_eq!(exp_x, c.dtx, epsilon = 1e-6);
assert_relative_eq!(exp_y, c.dty, epsilon = 1e-6);
}
}
#[test]
fn partials_of_rotation_at_zero() {
let params = AlignParams2::from_origin(None);
let current = params.compute_values();
let p_local = Point2::new(1.0, 2.0);
let p = current.transform * p_local;
let exp_rz = finite_diff(¶ms, &p_local, 2);
assert_relative_eq!(exp_rz, current.drz(&p), epsilon = 1e-6);
}
#[test]
fn stress_partials_rotation() {
let mut rg = RandomGeometry2::new();
for _ in 0..1000 {
let local = rg.iso2(10.0);
let working = rg.iso2(10.0);
let params = AlignParams2::new(AlignOrigin2::Local(local), Some(working), None)
.with_storage(rg.vector(PI));
let c = params.compute_values();
let test_point = rg.point(10.0);
let exp_rz = finite_diff(¶ms, &test_point, 2);
let target_point = c.transform * test_point;
assert_relative_eq!(exp_rz, c.drz(&target_point), epsilon = 1e-3);
}
}
fn finite_diff(params: &AlignParams2, point: &Point2, index: usize) -> Vector2 {
let mut w0 = params.clone();
let mut w1 = params.clone();
let stored = params.storage();
let eps = if index < 2 {
TRANS_EPSILON
} else {
ANGLE_EPSILON
};
w0.set_index(index, stored[index] - eps);
w1.set_index(index, stored[index] + eps);
let p0 = w0.compute_values().transform * point;
let p1 = w1.compute_values().transform * point;
(p1 - p0) / (2.0 * eps)
}
}