nalgebra/tests/geometry/quaternion.rs

267 lines
7.6 KiB
Rust
Raw Normal View History

2021-03-01 00:52:14 +08:00
#![cfg(feature = "proptest-support")]
#![allow(non_snake_case)]
2021-03-01 00:52:14 +08:00
use na::{Unit, UnitQuaternion};
2021-03-01 00:52:14 +08:00
use crate::proptest::*;
use proptest::{prop_assert, proptest};
proptest!(
/*
*
* Euler angles.
*
*/
2021-03-01 00:52:14 +08:00
#[test]
fn from_euler_angles(r in PROPTEST_F64, p in PROPTEST_F64, y in PROPTEST_F64) {
2020-04-06 00:33:03 +08:00
let roll = UnitQuaternion::from_euler_angles(r, 0.0, 0.0);
let pitch = UnitQuaternion::from_euler_angles(0.0, p, 0.0);
2020-04-06 00:33:03 +08:00
let yaw = UnitQuaternion::from_euler_angles(0.0, 0.0, y);
let rpy = UnitQuaternion::from_euler_angles(r, p, y);
2020-04-06 00:33:03 +08:00
let rroll = roll.to_rotation_matrix();
let rpitch = pitch.to_rotation_matrix();
2020-04-06 00:33:03 +08:00
let ryaw = yaw.to_rotation_matrix();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(rroll[(0, 0)], 1.0, epsilon = 1.0e-7)); // rotation wrt. x axis.
prop_assert!(relative_eq!(rpitch[(1, 1)], 1.0, epsilon = 1.0e-7)); // rotation wrt. y axis.
prop_assert!(relative_eq!(ryaw[(2, 2)], 1.0, epsilon = 1.0e-7)); // rotation wrt. z axis.
prop_assert!(relative_eq!(yaw * pitch * roll, rpy, epsilon = 1.0e-7));
}
2021-03-01 00:52:14 +08:00
#[test]
fn euler_angles(r in PROPTEST_F64, p in PROPTEST_F64, y in PROPTEST_F64) {
let rpy = UnitQuaternion::from_euler_angles(r, p, y);
2018-12-29 19:12:56 +08:00
let (roll, pitch, yaw) = rpy.euler_angles();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(
2020-04-06 00:33:03 +08:00
UnitQuaternion::from_euler_angles(roll, pitch, yaw),
rpy,
epsilon = 1.0e-7
2021-03-01 00:52:14 +08:00
))
}
/*
*
* From/to rotation matrix.
*
*/
2021-03-01 00:52:14 +08:00
#[test]
fn unit_quaternion_rotation_conversion(q in unit_quaternion()) {
2020-04-06 00:33:03 +08:00
let r = q.to_rotation_matrix();
let qq = UnitQuaternion::from_rotation_matrix(&r);
let rr = qq.to_rotation_matrix();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(q, qq, epsilon = 1.0e-7) && relative_eq!(r, rr, epsilon = 1.0e-7))
}
/*
*
* Point/Vector transformation.
*
*/
2020-04-06 00:33:03 +08:00
2021-03-01 00:52:14 +08:00
#[test]
2020-04-06 02:36:26 +08:00
#[cfg_attr(rustfmt, rustfmt_skip)]
2020-04-06 00:33:03 +08:00
fn unit_quaternion_transformation(
2021-03-01 00:52:14 +08:00
q in unit_quaternion(),
v in vector3(),
p in point3()
) {
let r = q.to_rotation_matrix();
let rv = r * v;
let rp = r * p;
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(q * v, rv, epsilon = 1.0e-7)
2020-04-06 00:33:03 +08:00
&& relative_eq!(q * &v, rv, epsilon = 1.0e-7)
&& relative_eq!(&q * v, rv, epsilon = 1.0e-7)
&& relative_eq!(&q * &v, rv, epsilon = 1.0e-7)
&& relative_eq!(q * p, rp, epsilon = 1.0e-7)
&& relative_eq!(q * &p, rp, epsilon = 1.0e-7)
&& relative_eq!(&q * p, rp, epsilon = 1.0e-7)
2021-03-01 00:52:14 +08:00
&& relative_eq!(&q * &p, rp, epsilon = 1.0e-7))
}
/*
*
* Inversion.
*
*/
2021-03-01 00:52:14 +08:00
#[test]
fn unit_quaternion_inv(q in unit_quaternion()) {
let iq = q.inverse();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(&iq * &q, UnitQuaternion::identity(), epsilon = 1.0e-7)
2020-04-06 00:33:03 +08:00
&& relative_eq!(iq * &q, UnitQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!(&iq * q, UnitQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!(iq * q, UnitQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!(&q * &iq, UnitQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!(q * &iq, UnitQuaternion::identity(), epsilon = 1.0e-7)
&& relative_eq!(&q * iq, UnitQuaternion::identity(), epsilon = 1.0e-7)
2021-03-01 00:52:14 +08:00
&& relative_eq!(q * iq, UnitQuaternion::identity(), epsilon = 1.0e-7))
}
/*
*
2017-08-16 01:36:38 +08:00
* Quaterion * Vector == Rotation * Vector
*
*/
2021-03-01 00:52:14 +08:00
#[test]
fn unit_quaternion_mul_vector(q in unit_quaternion(), v in vector3(), p in point3()) {
let r = q.to_rotation_matrix();
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(q * v, r * v, epsilon = 1.0e-7));
prop_assert!(relative_eq!(q * p, r * p, epsilon = 1.0e-7));
// Equivalence q = -q
2021-03-01 00:52:14 +08:00
prop_assert!(relative_eq!(UnitQuaternion::new_unchecked(-q.into_inner()) * v, r * v, epsilon = 1.0e-7));
prop_assert!(relative_eq!(UnitQuaternion::new_unchecked(-q.into_inner()) * p, r * p, epsilon = 1.0e-7));
}
/*
*
* Unit quaternion double-covering.
*
*/
2021-03-01 00:52:14 +08:00
#[test]
fn unit_quaternion_double_covering(q in unit_quaternion()) {
2020-04-06 00:33:03 +08:00
let mq = UnitQuaternion::new_unchecked(-q.into_inner());
2021-03-01 00:52:14 +08:00
prop_assert!(mq == q && mq.angle() == q.angle() && mq.axis() == q.axis())
}
// Test that all operators (incl. all combinations of references) work.
// See the top comment on `geometry/quaternion_ops.rs` for details on which operations are
// supported.
2021-03-01 00:52:14 +08:00
#[test]
2020-04-06 02:36:26 +08:00
#[cfg_attr(rustfmt, rustfmt_skip)]
2020-04-06 00:33:03 +08:00
fn all_op_exist(
2021-03-01 00:52:14 +08:00
q in quaternion(),
uq in unit_quaternion(),
v in vector3(),
p in point3(),
r in rotation3(),
s in PROPTEST_F64
) {
2016-12-05 06:00:12 +08:00
let uv = Unit::new_normalize(v);
let qpq = q + q;
let qmq = q - q;
let qMq = q * q;
2020-04-06 00:33:03 +08:00
let mq = -q;
let qMs = q * s;
let qDs = q / s;
let sMq = s * q;
let uqMuq = uq * uq;
2020-04-06 00:33:03 +08:00
let uqMr = uq * r;
let rMuq = r * uq;
let uqDuq = uq / uq;
2020-04-06 00:33:03 +08:00
let uqDr = uq / r;
let rDuq = r / uq;
2020-04-06 00:33:03 +08:00
let uqMp = uq * p;
let uqMv = uq * v;
let uqMuv = uq * uv;
let mut qMs1 = q;
let mut qMq1 = q;
let mut qMq2 = q;
let mut qpq1 = q;
let mut qpq2 = q;
let mut qmq1 = q;
let mut qmq2 = q;
let mut uqMuq1 = uq;
let mut uqMuq2 = uq;
let mut uqMr1 = uq;
let mut uqMr2 = uq;
let mut uqDuq1 = uq;
let mut uqDuq2 = uq;
let mut uqDr1 = uq;
let mut uqDr2 = uq;
qMs1 *= s;
qMq1 *= q;
qMq2 *= &q;
qpq1 += q;
qpq2 += &q;
qmq1 -= q;
qmq2 -= &q;
uqMuq1 *= uq;
uqMuq2 *= &uq;
uqMr1 *= r;
uqMr2 *= &r;
uqDuq1 /= uq;
uqDuq2 /= &uq;
uqDr1 /= r;
uqDr2 /= &r;
2021-03-01 00:52:14 +08:00
prop_assert!(qMs1 == qMs
2020-04-06 00:33:03 +08:00
&& qMq1 == qMq
&& qMq1 == qMq2
&& qpq1 == qpq
&& qpq1 == qpq2
&& qmq1 == qmq
&& qmq1 == qmq2
&& uqMuq1 == uqMuq
&& uqMuq1 == uqMuq2
&& uqMr1 == uqMr
&& uqMr1 == uqMr2
&& uqDuq1 == uqDuq
&& uqDuq1 == uqDuq2
&& uqDr1 == uqDr
&& uqDr1 == uqDr2
&& qpq == &q + &q
&& qpq == q + &q
&& qpq == &q + q
&& qmq == &q - &q
&& qmq == q - &q
&& qmq == &q - q
&& qMq == &q * &q
&& qMq == q * &q
&& qMq == &q * q
&& mq == -&q
&& qMs == &q * s
&& qDs == &q / s
&& sMq == s * &q
&& uqMuq == &uq * &uq
&& uqMuq == uq * &uq
&& uqMuq == &uq * uq
&& uqMr == &uq * &r
&& uqMr == uq * &r
&& uqMr == &uq * r
&& rMuq == &r * &uq
&& rMuq == r * &uq
&& rMuq == &r * uq
&& uqDuq == &uq / &uq
&& uqDuq == uq / &uq
&& uqDuq == &uq / uq
&& uqDr == &uq / &r
&& uqDr == uq / &r
&& uqDr == &uq / r
&& rDuq == &r / &uq
&& rDuq == r / &uq
&& rDuq == &r / uq
&& uqMp == &uq * &p
&& uqMp == uq * &p
&& uqMp == &uq * p
&& uqMv == &uq * &v
&& uqMv == uq * &v
&& uqMv == &uq * v
&& uqMuv == &uq * &uv
&& uqMuv == uq * &uv
2021-03-01 00:52:14 +08:00
&& uqMuv == &uq * uv)
}
);