use crate::common::PCoords;
use crate::geom3::align3::{AlignStorage3, Dof6, iso3_from_param};
use crate::na::{Matrix3, UnitQuaternion};
use crate::{Iso3, Point3, Vector3};
const SK_X: Matrix3<f64> = Matrix3::new(0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0);
const SK_Y: Matrix3<f64> = Matrix3::new(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0);
const SK_Z: Matrix3<f64> = Matrix3::new(0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0);
#[derive(Clone, Debug)]
pub struct AlignValues3 {
pub transform: Iso3,
pub align: Iso3,
pub dof: Dof6,
pub dtx: Vector3,
pub dty: Vector3,
pub dtz: Vector3,
m_drx: Matrix3<f64>,
m_dry: Matrix3<f64>,
m_drz: Matrix3<f64>,
pre_rot: Iso3,
}
impl AlignValues3 {
pub fn drx(&self, point: &impl PCoords<3>) -> Vector3 {
self.m_drx * (self.pre_rot * Point3::from(point.coords())).coords()
}
pub fn dry(&self, point: &impl PCoords<3>) -> Vector3 {
self.m_dry * (self.pre_rot * Point3::from(point.coords())).coords()
}
pub fn drz(&self, point: &impl PCoords<3>) -> Vector3 {
self.m_drz * (self.pre_rot * Point3::from(point.coords())).coords()
}
}
#[derive(Clone, Copy, Debug)]
pub enum AlignOrigin3 {
Origin,
Center(Point3),
Local(Iso3),
}
#[derive(Clone, Debug)]
pub struct AlignParams3 {
pub local: Iso3,
pub dof: Dof6,
pub offset: Iso3,
storage: AlignStorage3,
}
impl AlignParams3 {
pub fn new(local: AlignOrigin3, offset: Option<Iso3>, dof: Option<Dof6>) -> Self {
let local = match local {
AlignOrigin3::Origin => Iso3::identity(),
AlignOrigin3::Center(p) => Iso3::translation(p.x, p.y, p.z),
AlignOrigin3::Local(t) => t,
};
let working = offset.unwrap_or_else(Iso3::identity);
let dof = dof.unwrap_or_else(Dof6::all);
Self {
local,
dof,
offset: working,
storage: AlignStorage3::default(),
}
}
pub fn from_local(local: Iso3, dof: Option<Dof6>) -> Self {
let working = local;
Self::new(AlignOrigin3::Local(local), Some(working), dof)
}
pub fn from_center(center: Point3, dof: Option<Dof6>) -> Self {
let local = Iso3::translation(center.x, center.y, center.z);
let working = local;
Self::new(AlignOrigin3::Local(local), Some(working), dof)
}
pub fn from_origin(dof: Option<Dof6>) -> Self {
Self::new(AlignOrigin3::Origin, None, dof)
}
pub fn tx(&self) -> f64 {
self.storage[0]
}
pub fn ty(&self) -> f64 {
self.storage[1]
}
pub fn tz(&self) -> f64 {
self.storage[2]
}
pub fn rx(&self) -> f64 {
self.storage[3]
}
pub fn ry(&self) -> f64 {
self.storage[4]
}
pub fn rz(&self) -> f64 {
self.storage[5]
}
pub fn compute_transform(&self) -> Iso3 {
self.compute_values().transform
}
pub fn compute_values(&self) -> AlignValues3 {
let align = iso3_from_param(&self.storage);
let transform = self.offset * align * self.local.inverse();
let dtx = self.offset * Vector3::new(1.0, 0.0, 0.0);
let dty = self.offset * Vector3::new(0.0, 1.0, 0.0);
let dtz = self.offset * Vector3::new(0.0, 0.0, 1.0);
let (m_drx, m_dry, m_drz) = euler_partials(self.ry(), self.rz());
let pre_rot = align.translation.inverse() * self.offset.inverse();
let post_rot = self.offset.rotation.to_rotation_matrix();
AlignValues3 {
transform,
align,
dof: self.dof,
dtx,
dty,
dtz,
m_drx: post_rot * m_drx,
m_dry: post_rot * m_dry,
m_drz: post_rot * m_drz,
pre_rot,
}
}
pub fn storage(&self) -> AlignStorage3 {
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.tz {
self.storage[2] = 0.0;
}
if !self.dof.rx {
self.storage[3] = 0.0;
}
if !self.dof.ry {
self.storage[4] = 0.0;
}
if !self.dof.rz {
self.storage[5] = 0.0;
}
}
pub fn set_storage(&mut self, storage: AlignStorage3) {
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) -> AlignParams3 {
self.with_index(0, tx)
}
pub fn with_ty(&self, ty: f64) -> AlignParams3 {
self.with_index(1, ty)
}
pub fn with_tz(&self, tz: f64) -> AlignParams3 {
self.with_index(2, tz)
}
pub fn with_rx(&self, rx: f64) -> AlignParams3 {
self.with_index(3, rx)
}
pub fn with_ry(&self, ry: f64) -> AlignParams3 {
self.with_index(4, ry)
}
pub fn with_rz(&self, rz: f64) -> AlignParams3 {
self.with_index(5, rz)
}
fn with_index(&self, index: usize, value: f64) -> AlignParams3 {
let mut params = self.clone();
params.set_index(index, value);
params
}
pub fn with_storage(&self, storage: AlignStorage3) -> AlignParams3 {
let mut params = self.clone();
params.set_storage(storage);
params
}
}
fn euler_partials(ry: f64, rz: f64) -> (Matrix3<f64>, Matrix3<f64>, Matrix3<f64>) {
let y = UnitQuaternion::from_euler_angles(0.0, ry, 0.0).to_rotation_matrix();
let z = UnitQuaternion::from_euler_angles(0.0, 0.0, rz).to_rotation_matrix();
let dy = z * SK_Y * z.transpose();
let dx = z * y * SK_X * y.transpose() * z.transpose();
(dx, dy, SK_Z)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::common::random_geometry::RandomGeometry3;
use crate::geom3::IsoExtensions3;
use crate::geom3::align3::params::{AlignOrigin3, AlignParams3};
use crate::na::{Rotation, Rotation3, UnitQuaternion};
use crate::{Iso3, Point3, Vector3};
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 = AlignParams3::from_origin(None).with_rz(PI / 2.0);
let test_point = Point3::new(1.0, 2.0, 3.0);
let expected = Point3::new(-2.0, 1.0, 3.0);
assert_relative_eq!(
expected,
params.compute_values().transform * test_point,
epsilon = 1e-8
);
}
#[test]
fn rotation_around_center() {
let rc = Point3::new(1.0, 0.0, 0.0);
let params = AlignParams3::from_center(rc, None).with_rz(PI / 2.0);
let test_point = Point3::new(2.0, 2.0, 3.0);
let expected = Point3::new(-1.0, 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 = Dof6::new(false, true, true, true, false, true);
let params = AlignParams3::from_origin(Some(dof));
assert_eq!(params.with_tx(1.0).tx(), 0.0);
assert_eq!(params.with_ry(1.0).ry(), 0.0);
assert_eq!(params.with_ty(1.0).ty(), 1.0);
assert_eq!(params.with_rx(1.0).rx(), 1.0);
let all = params.with_storage(AlignStorage3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0));
assert_eq!(all.tx(), 0.0);
assert_eq!(all.ry(), 0.0);
assert_eq!(all.ty(), 2.0);
assert_eq!(all.rz(), 6.0);
}
#[test]
fn translations_are_in_local_origin_directions() {
let local = Iso3::from_basis_xy(
&Vector3::new(0.0, -1.0, 0.0),
&Vector3::new(0.0, 0.0, 1.0),
None,
)
.unwrap();
let params = AlignParams3::from_local(local, None).with_tx(1.0);
let test_point = Point3::new(1.0, 2.0, 3.0);
let expected = Point3::new(1.0, 1.0, 3.0);
assert_relative_eq!(
expected,
params.compute_values().transform * test_point,
epsilon = 1e-8
);
}
#[test]
fn partials_of_translations_at_zero() {
let params = AlignParams3::from_origin(None);
let current = params.compute_values();
assert_relative_eq!(current.dtx, Vector3::x_axis(), epsilon = 1e-12);
assert_relative_eq!(current.dty, Vector3::y_axis(), epsilon = 1e-12);
assert_relative_eq!(current.dtz, Vector3::z_axis(), epsilon = 1e-12);
}
#[test]
fn partials_of_translations_with_rotations() {
let params = AlignParams3::from_origin(None)
.with_rx(0.1)
.with_ry(0.2)
.with_rz(0.3);
let test_point = Point3::new(1.0, 2.0, 3.0);
let exp_x = finite_diff(¶ms, &test_point, 0);
let exp_y = finite_diff(¶ms, &test_point, 1);
let exp_z = finite_diff(¶ms, &test_point, 2);
let c = params.compute_values();
assert_relative_eq!(c.dtx, Vector3::x_axis(), epsilon = 1e-12);
assert_relative_eq!(c.dty, Vector3::y_axis(), epsilon = 1e-12);
assert_relative_eq!(c.dtz, Vector3::z_axis(), epsilon = 1e-12);
assert_relative_eq!(exp_x, c.dtx, epsilon = 1e-8);
assert_relative_eq!(exp_y, c.dty, epsilon = 1e-8);
assert_relative_eq!(exp_z, c.dtz, epsilon = 1e-8);
}
#[test]
fn stress_partials_translations() {
let mut rg = RandomGeometry3::new();
for _ in 0..1000 {
let local = rg.iso3(10.0);
let working = rg.iso3(10.0);
let params = AlignParams3::new(AlignOrigin3::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 exp_z = finite_diff(¶ms, &test_point, 2);
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);
assert_relative_eq!(exp_z, c.dtz, epsilon = 1e-6);
}
}
#[test]
fn partials_of_rotation_at_zero() {
let params = AlignParams3::from_origin(None);
let current = params.compute_values();
let p_local = Point3::new(1.0, 2.0, 3.0);
let p = current.transform * p_local;
let exp_rx = finite_diff(¶ms, &p_local, 3);
let exp_ry = finite_diff(¶ms, &p_local, 4);
let exp_rz = finite_diff(¶ms, &p_local, 5);
assert_relative_eq!(exp_rx, current.drx(&p), epsilon = 1e-6);
assert_relative_eq!(exp_ry, current.dry(&p), epsilon = 1e-6);
assert_relative_eq!(exp_rz, current.drz(&p), epsilon = 1e-6);
}
#[test]
fn verify_euler_order_is_x_y_z() {
let rx = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), PI / 4.0).to_rotation_matrix();
let ry = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), PI / 5.0).to_rotation_matrix();
let rz = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), PI / 6.0).to_rotation_matrix();
let expected =
UnitQuaternion::from_euler_angles(PI / 4.0, PI / 5.0, PI / 6.0).to_rotation_matrix();
let actual = rz * ry * rx;
assert_relative_eq!(expected, actual, epsilon = 1e-12);
}
#[test]
fn euler_partial_rx() {
let f = EulerFixture::new(PI / 4.0, 0.0, 0.0);
let p_local = Point3::new(1.0, 1.0, 1.0);
let p_rot = f.rot * p_local;
let (rdx, rdy, rdz) = euler_partials(f.ry, f.rz);
assert_relative_eq!(f.exp_z(&p_local), rdz * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_y(&p_local), rdy * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_x(&p_local), rdx * p_rot.coords, epsilon = 1e-6);
}
#[test]
fn euler_partial_ry() {
let f = EulerFixture::new(0.0, PI / 4.0, 0.0);
let p_local = Point3::new(1.0, 1.0, 1.0);
let p_rot = f.rot * p_local;
let (rdx, rdy, rdz) = euler_partials(f.ry, f.rz);
assert_relative_eq!(f.exp_z(&p_local), rdz * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_y(&p_local), rdy * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_x(&p_local), rdx * p_rot.coords, epsilon = 1e-6);
}
#[test]
fn euler_partial_rz() {
let f = EulerFixture::new(0.0, 0.0, PI / 4.0);
let p_local = Point3::new(1.0, 1.0, 1.0);
let p_rot = f.rot * p_local;
let (rdx, rdy, rdz) = euler_partials(f.ry, f.rz);
assert_relative_eq!(f.exp_z(&p_local), rdz * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_y(&p_local), rdy * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_x(&p_local), rdx * p_rot.coords, epsilon = 1e-6);
}
#[test]
fn stress_euler_partial_rotated() {
let mut rg = RandomGeometry3::new();
for _ in 0..1000 {
let f = EulerFixture::new(rg.f64_sym(PI), rg.f64_sym(PI), rg.f64_sym(PI));
let p_local = rg.point(10.0);
let p_rot = f.rot * p_local;
let (rdx, rdy, rdz) = euler_partials(f.ry, f.rz);
assert_relative_eq!(f.exp_z(&p_local), rdz * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_y(&p_local), rdy * p_rot.coords, epsilon = 1e-6);
assert_relative_eq!(f.exp_x(&p_local), rdx * p_rot.coords, epsilon = 1e-6);
}
}
#[test]
fn stress_partials_rotation() {
let mut rg = RandomGeometry3::new();
for _ in 0..1000 {
let local = rg.iso3(10.0);
let working = rg.iso3(10.0);
let params = AlignParams3::new(AlignOrigin3::Local(local), Some(working), None)
.with_storage(rg.vector(PI));
let c = params.compute_values();
let test_point = rg.point(10.0);
let exp_x = finite_diff(¶ms, &test_point, 3);
let exp_y = finite_diff(¶ms, &test_point, 4);
let exp_z = finite_diff(¶ms, &test_point, 5);
let target_point = c.transform * test_point;
assert_relative_eq!(exp_x, c.drx(&target_point), epsilon = 1e-3);
assert_relative_eq!(exp_y, c.dry(&target_point), epsilon = 1e-3);
assert_relative_eq!(exp_z, c.drz(&target_point), epsilon = 1e-3);
}
}
struct EulerFixture {
rx: f64,
ry: f64,
rz: f64,
rot: Rotation<f64, 3>,
}
impl EulerFixture {
fn new(rx: f64, ry: f64, rz: f64) -> Self {
let rot = UnitQuaternion::from_euler_angles(rx, ry, rz).to_rotation_matrix();
Self { rx, ry, rz, rot }
}
fn exp_x(&self, point: &Point3) -> Vector3 {
euler_finite_diff(self.rx, self.ry, self.rz, Ax::X, point)
}
fn exp_y(&self, point: &Point3) -> Vector3 {
euler_finite_diff(self.rx, self.ry, self.rz, Ax::Y, point)
}
fn exp_z(&self, point: &Point3) -> Vector3 {
euler_finite_diff(self.rx, self.ry, self.rz, Ax::Z, point)
}
}
enum Ax {
X,
Y,
Z,
}
fn euler_finite_diff(rx: f64, ry: f64, rz: f64, axis: Ax, point: &Point3) -> Vector3 {
let t0 = match axis {
Ax::X => unit_quat(rx - ANGLE_EPSILON, ry, rz),
Ax::Y => unit_quat(rx, ry - ANGLE_EPSILON, rz),
Ax::Z => unit_quat(rx, ry, rz - ANGLE_EPSILON),
};
let t1 = match axis {
Ax::X => unit_quat(rx + ANGLE_EPSILON, ry, rz),
Ax::Y => unit_quat(rx, ry + ANGLE_EPSILON, rz),
Ax::Z => unit_quat(rx, ry, rz + ANGLE_EPSILON),
};
let p0 = t0 * point;
let p1 = t1 * point;
(p1 - p0) / (2.0 * ANGLE_EPSILON)
}
fn unit_quat(rx: f64, ry: f64, rz: f64) -> Rotation3<f64> {
UnitQuaternion::from_euler_angles(rx, ry, rz).to_rotation_matrix()
}
fn finite_diff(params: &AlignParams3, point: &Point3, index: usize) -> Vector3 {
let mut w0 = params.clone();
let mut w1 = params.clone();
let stored = params.storage();
let eps = if index < 3 {
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)
}
}