nalgebra/tests/geometry/dual_quaternion.rs

303 lines
11 KiB
Rust
Raw Normal View History

2021-03-01 00:52:14 +08:00
#![cfg(feature = "proptest-support")]
2021-01-29 07:45:34 +08:00
#![allow(non_snake_case)]
use na::{DualQuaternion, Point3, Unit, UnitDualQuaternion, UnitQuaternion, Vector3};
2021-01-29 07:45:34 +08:00
2021-03-01 00:52:14 +08:00
use crate::proptest::*;
use proptest::{prop_assert, proptest};
proptest!(
#[test]
fn isometry_equivalence(iso in isometry3(), p in point3(), v in vector3()) {
2021-01-29 07:45:34 +08:00
let dq = UnitDualQuaternion::from_isometry(&iso);
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(iso * p, dq * p, epsilon = 1.0e-7));
prop_assert!(relative_eq!(iso * v, dq * v, epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
}
2021-03-01 00:52:14 +08:00
#[test]
fn inverse_is_identity(i in unit_dual_quaternion(), p in point3(), v in vector3()) {
2021-01-29 07:45:34 +08:00
let ii = i.inverse();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(i * ii, UnitDualQuaternion::identity(), epsilon = 1.0e-7)
2021-01-29 07:45:34 +08:00
&& relative_eq!(ii * i, UnitDualQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!((i * ii) * p, p, epsilon = 1.0e-7)
&& relative_eq!((ii * i) * p, p, epsilon = 1.0e-7)
&& relative_eq!((i * ii) * v, v, epsilon = 1.0e-7)
2021-03-01 00:52:14 +08:00
&& relative_eq!((ii * i) * v, v, epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
}
2021-01-29 08:29:12 +08:00
#[cfg_attr(rustfmt, rustfmt_skip)]
2021-03-01 00:52:14 +08:00
#[test]
2021-01-29 07:45:34 +08:00
fn multiply_equals_alga_transform(
2021-03-01 00:52:14 +08:00
dq in unit_dual_quaternion(),
v in vector3(),
p in point3()
) {
prop_assert!(dq * v == dq.transform_vector(&v)
2021-01-29 07:45:34 +08:00
&& dq * p == dq.transform_point(&p)
&& relative_eq!(
dq.inverse() * v,
dq.inverse_transform_vector(&v),
epsilon = 1.0e-7
)
&& relative_eq!(
dq.inverse() * p,
dq.inverse_transform_point(&p),
epsilon = 1.0e-7
2021-03-01 00:52:14 +08:00
));
2021-01-29 07:45:34 +08:00
}
#[cfg_attr(rustfmt, rustfmt_skip)]
2021-03-01 00:52:14 +08:00
#[test]
2021-01-29 07:45:34 +08:00
fn composition(
2021-03-01 00:52:14 +08:00
dq in unit_dual_quaternion(),
uq in unit_quaternion(),
t in translation3(),
v in vector3(),
p in point3()
) {
2021-01-29 07:45:34 +08:00
// (rotation × dual quaternion) * point = rotation × (dual quaternion * point)
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!((uq * dq) * v, uq * (dq * v), epsilon = 1.0e-7));
prop_assert!(relative_eq!((uq * dq) * p, uq * (dq * p), epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
// (dual quaternion × rotation) * point = dual quaternion × (rotation * point)
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!((dq * uq) * v, dq * (uq * v), epsilon = 1.0e-7));
prop_assert!(relative_eq!((dq * uq) * p, dq * (uq * p), epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
// (translation × dual quaternion) * point = translation × (dual quaternion * point)
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!((t * dq) * v, (dq * v), epsilon = 1.0e-7));
prop_assert!(relative_eq!((t * dq) * p, t * (dq * p), epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
// (dual quaternion × translation) * point = dual quaternion × (translation * point)
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!((dq * t) * v, dq * v, epsilon = 1.0e-7));
prop_assert!(relative_eq!((dq * t) * p, dq * (t * p), epsilon = 1.0e-7));
2021-01-29 07:45:34 +08:00
}
#[cfg_attr(rustfmt, rustfmt_skip)]
#[test]
fn sclerp_is_defined_for_identical_orientations(
dq in unit_dual_quaternion(),
s in -1.0f64..2.0f64,
t in translation3(),
) {
// Should not panic.
prop_assert!(relative_eq!(dq.sclerp(&dq, 0.0), dq, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq, 0.5), dq, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq, 1.0), dq, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq, s), dq, epsilon = 1.0e-7));
let unit = UnitDualQuaternion::identity();
prop_assert!(relative_eq!(unit.sclerp(&unit, 0.0), unit, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit, 0.5), unit, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit, 1.0), unit, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit, s), unit, epsilon = 1.0e-7));
// ScLERPing two unit dual quaternions with nearly equal rotation
// components should result in a unit dual quaternion with a rotation
// component nearly equal to either input.
let dq2 = t * dq;
prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.0).real, dq.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq2, 0.5).real, dq.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq2, 1.0).real, dq.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(dq.sclerp(&dq2, s).real, dq.real, epsilon = 1.0e-7));
// ScLERPing two unit dual quaternions with nearly equal rotation
// components should result in a unit dual quaternion with a translation
// component which is nearly equal to linearly interpolating the
// translation components of the inputs.
prop_assert!(relative_eq!(
dq.sclerp(&dq2, s).translation().vector,
dq.translation().vector.lerp(&dq2.translation().vector, s),
epsilon = 1.0e-7
));
let unit2 = t * unit;
prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.0).real, unit.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit2, 0.5).real, unit.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit2, 1.0).real, unit.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(unit.sclerp(&unit2, s).real, unit.real, epsilon = 1.0e-7));
prop_assert!(relative_eq!(
unit.sclerp(&unit2, s).translation().vector,
unit.translation().vector.lerp(&unit2.translation().vector, s),
epsilon = 1.0e-7
));
}
#[cfg_attr(rustfmt, rustfmt_skip)]
#[test]
fn sclerp_is_not_defined_for_opposite_orientations(
dq in unit_dual_quaternion(),
s in 0.1f64..0.9f64,
t in translation3(),
t2 in translation3(),
v in vector3(),
) {
let iso = dq.to_isometry();
let rot = iso.rotation;
if let Some((axis, angle)) = rot.axis_angle() {
let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
let dqf = flipped * rot.inverse() * dq.clone();
prop_assert!(dq.try_sclerp(&dqf, 0.5, 1.0e-7).is_none());
prop_assert!(dq.try_sclerp(&dqf, s, 1.0e-7).is_none());
}
let dq2 = t * dq;
let iso2 = dq2.to_isometry();
let rot2 = iso2.rotation;
if let Some((axis, angle)) = rot2.axis_angle() {
let flipped = UnitQuaternion::from_axis_angle(&axis, angle + std::f64::consts::PI);
let dq3f = t2 * flipped * rot.inverse() * dq.clone();
prop_assert!(dq2.try_sclerp(&dq3f, 0.5, 1.0e-7).is_none());
prop_assert!(dq2.try_sclerp(&dq3f, s, 1.0e-7).is_none());
}
if let Some(axis) = Unit::try_new(v, 1.0e-7) {
let unit = UnitDualQuaternion::identity();
let flip = UnitQuaternion::from_axis_angle(&axis, std::f64::consts::PI);
let unitf = flip * unit;
prop_assert!(unit.try_sclerp(&unitf, 0.5, 1.0e-7).is_none());
prop_assert!(unit.try_sclerp(&unitf, s, 1.0e-7).is_none());
let unit2f = t * unit * flip;
prop_assert!(unit.try_sclerp(&unit2f, 0.5, 1.0e-7).is_none());
prop_assert!(unit.try_sclerp(&unit2f, s, 1.0e-7).is_none());
}
}
2021-01-29 07:45:34 +08:00
#[cfg_attr(rustfmt, rustfmt_skip)]
2021-03-01 00:52:14 +08:00
#[test]
2021-01-29 07:45:34 +08:00
fn all_op_exist(
2021-03-01 00:52:14 +08:00
dq in dual_quaternion(),
udq in unit_dual_quaternion(),
uq in unit_quaternion(),
s in PROPTEST_F64,
t in translation3(),
v in vector3(),
p in point3()
) {
2021-01-29 08:27:40 +08:00
let dqMs: DualQuaternion<_> = dq * s;
let dqMdq: DualQuaternion<_> = dq * dq;
let dqMudq: DualQuaternion<_> = dq * udq;
let udqMdq: DualQuaternion<_> = udq * dq;
let iMi: UnitDualQuaternion<_> = udq * udq;
let iMuq: UnitDualQuaternion<_> = udq * uq;
let iDi: UnitDualQuaternion<_> = udq / udq;
let iDuq: UnitDualQuaternion<_> = udq / uq;
let iMp: Point3<_> = udq * p;
let iMv: Vector3<_> = udq * v;
let iMt: UnitDualQuaternion<_> = udq * t;
let tMi: UnitDualQuaternion<_> = t * udq;
let uqMi: UnitDualQuaternion<_> = uq * udq;
let uqDi: UnitDualQuaternion<_> = uq / udq;
let mut dqMs1 = dq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut dqMdq1 = dq;
let mut dqMdq2 = dq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut dqMudq1 = dq;
let mut dqMudq2 = dq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut iMt1 = udq;
let mut iMt2 = udq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut iMi1 = udq;
let mut iMi2 = udq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut iMuq1 = udq;
let mut iMuq2 = udq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut iDi1 = udq;
let mut iDi2 = udq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
let mut iDuq1 = udq;
let mut iDuq2 = udq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
dqMs1 *= s;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
dqMdq1 *= dq;
dqMdq2 *= &dq;
2021-01-29 07:45:34 +08:00
2021-01-29 08:27:40 +08:00
dqMudq1 *= udq;
dqMudq2 *= &udq;
2021-01-29 07:45:34 +08:00
iMt1 *= t;
iMt2 *= &t;
2021-01-29 08:27:40 +08:00
iMi1 *= udq;
iMi2 *= &udq;
2021-01-29 07:45:34 +08:00
iMuq1 *= uq;
iMuq2 *= &uq;
2021-01-29 08:27:40 +08:00
iDi1 /= udq;
iDi2 /= &udq;
2021-01-29 07:45:34 +08:00
iDuq1 /= uq;
iDuq2 /= &uq;
2021-03-01 00:52:14 +08:00
prop_assert!(dqMs == dqMs1
2021-01-29 08:27:40 +08:00
&& dqMdq == dqMdq1
&& dqMdq == dqMdq2
&& dqMudq == dqMudq1
&& dqMudq == dqMudq2
&& iMt == iMt1
2021-01-29 07:45:34 +08:00
&& iMt == iMt2
&& iMi == iMi1
&& iMi == iMi2
&& iMuq == iMuq1
&& iMuq == iMuq2
&& iDi == iDi1
&& iDi == iDi2
&& iDuq == iDuq1
&& iDuq == iDuq2
2021-01-29 08:27:40 +08:00
&& dqMs == &dq * s
&& dqMdq == &dq * &dq
&& dqMdq == dq * &dq
&& dqMdq == &dq * dq
&& dqMudq == &dq * &udq
&& dqMudq == dq * &udq
&& dqMudq == &dq * udq
&& udqMdq == &udq * &dq
&& udqMdq == udq * &dq
&& udqMdq == &udq * dq
&& iMi == &udq * &udq
&& iMi == udq * &udq
&& iMi == &udq * udq
&& iMuq == &udq * &uq
&& iMuq == udq * &uq
&& iMuq == &udq * uq
&& iDi == &udq / &udq
&& iDi == udq / &udq
&& iDi == &udq / udq
&& iDuq == &udq / &uq
&& iDuq == udq / &uq
&& iDuq == &udq / uq
&& iMp == &udq * &p
&& iMp == udq * &p
&& iMp == &udq * p
&& iMv == &udq * &v
&& iMv == udq * &v
&& iMv == &udq * v
&& iMt == &udq * &t
&& iMt == udq * &t
&& iMt == &udq * t
&& tMi == &t * &udq
&& tMi == t * &udq
&& tMi == &t * udq
&& uqMi == &uq * &udq
&& uqMi == uq * &udq
&& uqMi == &uq * udq
&& uqDi == &uq / &udq
&& uqDi == uq / &udq
2021-03-01 00:52:14 +08:00
&& uqDi == &uq / udq)
2021-01-29 07:45:34 +08:00
}
);