From ea86890647a61a6a127a3604ebeaa18db08c5fd8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 31 May 2015 22:52:18 +0200 Subject: [PATCH 1/2] Add the `RotationTo` trait to compute the delta rotation between two elements. Use `::angle_between` or `.angle_to` to compute the rotation angle between two vectors/rotation matrices/unit quaternions. Use `::rotation_between` or `.rotation_to` to compute the rotation matrix/unit quaternion to transform a vector/rotation matrix/uniq quaternion to another. Fix #130. --- src/lib.rs | 17 +++++++++++++++- src/structs/quat.rs | 20 +++++++++++++++++- src/structs/rot.rs | 35 +++++++++++++++++++++++++++++-- src/structs/spec/vec.rs | 43 ++++++++++++++++++++++++++++++++++++++- src/structs/vec_macros.rs | 6 +++--- src/traits/geometry.rs | 16 +++++++++++++++ src/traits/mod.rs | 5 +++-- 7 files changed, 132 insertions(+), 10 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 9c10add5..5ed1613d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -133,7 +133,7 @@ pub use traits::{ POrdering, PntAsVec, Repeat, - Rotate, Rotation, RotationMatrix, RotationWithTranslation, + Rotate, Rotation, RotationMatrix, RotationWithTranslation, RotationTo, Row, Shape, SquareMat, @@ -569,6 +569,21 @@ pub fn append_rotation_wrt_center + Copy, RotationWithTranslation::append_rotation_wrt_center(m, amount) } +/* + * RotationTo + */ +/// Computes the angle of the rotation needed to transfom `a` to `b`. +#[inline(always)] +pub fn angle_between(a: &V, b: &V) -> V::AngleType { + a.angle_to(b) +} + +/// Computes the rotation needed to transform `a` to `b`. +#[inline(always)] +pub fn rotation_between(a: &V, b: &V) -> V::DeltaRotationType { + a.rotation_to(b) +} + /* * RotationMatrix */ diff --git a/src/structs/quat.rs b/src/structs/quat.rs index 24f70bc1..604a1e20 100644 --- a/src/structs/quat.rs +++ b/src/structs/quat.rs @@ -12,7 +12,7 @@ use structs::{Vec3, Pnt3, Rot3, Mat3}; use traits::operations::{ApproxEq, Inv, POrd, POrdering, Axpy}; use traits::structure::{Cast, Indexable, Iterable, IterableMut, Dim, Shape, BaseFloat, BaseNum, Bounded, Repeat}; -use traits::geometry::{Norm, Rotation, Rotate, Transform}; +use traits::geometry::{Norm, Rotation, Rotate, RotationTo, Transform}; #[cfg(feature="arbitrary")] use quickcheck::{Arbitrary, Gen}; @@ -456,6 +456,24 @@ impl> Rotate> for UnitQuat { } } +impl> RotationTo for UnitQuat { + type AngleType = N; + type DeltaRotationType = UnitQuat; + + #[inline] + fn angle_to(&self, other: &Self) -> N { + let delta = self.rotation_to(other); + let _2 = ::one::() + ::one(); + + _2 * delta.q.vector().norm().atan2(delta.q.w) + } + + #[inline] + fn rotation_to(&self, other: &Self) -> UnitQuat { + *other / *self + } +} + impl> Transform> for UnitQuat { #[inline] fn transform(&self, v: &Vec3) -> Vec3 { diff --git a/src/structs/rot.rs b/src/structs/rot.rs index dca149b4..ebf24217 100644 --- a/src/structs/rot.rs +++ b/src/structs/rot.rs @@ -5,8 +5,8 @@ use std::ops::{Mul, Neg, Index}; use rand::{Rand, Rng}; use num::{Zero, One}; -use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, Transform, ToHomogeneous, - Norm, Cross}; +use traits::geometry::{Rotate, Rotation, AbsoluteRotate, RotationMatrix, RotationTo, Transform, + ToHomogeneous, Norm, Cross}; use traits::structure::{Cast, Dim, Row, Col, BaseFloat, BaseNum, Eye, Diag}; use traits::operations::{Absolute, Inv, Transpose, ApproxEq}; use structs::vec::{Vec1, Vec2, Vec3, Vec4}; @@ -71,6 +71,21 @@ impl Rotation> for Rot2 { } } +impl RotationTo for Rot2 { + type AngleType = N; + type DeltaRotationType = Rot2; + + #[inline] + fn angle_to(&self, other: &Self) -> N { + self.rotation_to(other).rotation().norm() + } + + #[inline] + fn rotation_to(&self, other: &Self) -> Rot2 { + *other * ::inv(self).unwrap() + } +} + impl Rand for Rot2 { #[inline] fn rand(rng: &mut R) -> Rot2 { @@ -283,6 +298,22 @@ Rotation> for Rot3 { } } +impl RotationTo for Rot3 { + type AngleType = N; + type DeltaRotationType = Rot3; + + #[inline] + fn angle_to(&self, other: &Self) -> N { + // FIXME: refactor to avoid the normalization of the rotation axisangle vector. + self.rotation_to(other).rotation().norm() + } + + #[inline] + fn rotation_to(&self, other: &Self) -> Rot3 { + *other * ::inv(self).unwrap() + } +} + impl Rand for Rot3 { #[inline] fn rand(rng: &mut R) -> Rot3 { diff --git a/src/structs/spec/vec.rs b/src/structs/spec/vec.rs index fcb0fa1c..09e8f63a 100644 --- a/src/structs/spec/vec.rs +++ b/src/structs/spec/vec.rs @@ -1,9 +1,50 @@ use std::ops::{Sub, Mul, Neg}; use num::{Zero, One}; use traits::structure::{Cast, Row, Basis, BaseFloat}; -use traits::geometry::{Norm, Cross, CrossMatrix, UniformSphereSample}; +use traits::geometry::{Norm, Cross, CrossMatrix, RotationTo, UniformSphereSample}; use structs::vec::{Vec1, Vec2, Vec3, Vec4}; use structs::mat::Mat3; +use structs::rot::{Rot2, Rot3}; + +impl RotationTo for Vec2 { + type AngleType = N; + type DeltaRotationType = Rot2; + + #[inline] + fn angle_to(&self, other: &Self) -> N { + ::cross(self, other).x.atan2(::dot(self, other)) + } + + #[inline] + fn rotation_to(&self, other: &Self) -> Rot2 { + Rot2::new(Vec1::new(self.angle_to(other))) + } +} + +impl RotationTo for Vec3 { + type AngleType = N; + type DeltaRotationType = Rot3; + + #[inline] + fn angle_to(&self, other: &Self) -> N { + ::cross(self, other).norm().atan2(::dot(self, other)) + } + + #[inline] + fn rotation_to(&self, other: &Self) -> Rot3 { + let mut axis = ::cross(self, other); + let norm = axis.normalize_mut(); + + if ::is_zero(&norm) { + ::one() + } + else { + let axis_angle = axis * norm.atan2(::dot(self, other)); + + Rot3::new(axis_angle) + } + } +} impl + Sub> Cross for Vec2 { type CrossProductType = Vec1; diff --git a/src/structs/vec_macros.rs b/src/structs/vec_macros.rs index 8b92dd09..6c7079f0 100644 --- a/src/structs/vec_macros.rs +++ b/src/structs/vec_macros.rs @@ -79,7 +79,7 @@ macro_rules! at_fast_impl( // However, f32/f64 does not implement Ord… macro_rules! ord_impl( ($t: ident, $comp0: ident, $($compN: ident),*) => ( - impl POrd for $t { + impl POrd for $t { #[inline] fn inf(&self, other: &$t) -> $t { $t::new(self.$comp0.min(other.$comp0) @@ -291,7 +291,7 @@ macro_rules! container_impl( macro_rules! basis_impl( ($t: ident, $dim: expr) => ( - impl> Basis for $t { + impl> Basis for $t { #[inline] fn canonical_basis) -> bool>(mut f: F) { for i in 0..$dim { @@ -547,7 +547,7 @@ macro_rules! translation_impl( macro_rules! norm_impl( ($t: ident, $($compN: ident),+) => ( - impl Norm for $t { + impl Norm for $t { #[inline] fn sqnorm(&self) -> N { Dot::dot(self, self) diff --git a/src/traits/geometry.rs b/src/traits/geometry.rs index 29b3a51a..ffc2db7e 100644 --- a/src/traits/geometry.rs +++ b/src/traits/geometry.rs @@ -64,6 +64,22 @@ pub trait Rotation { fn set_rotation(&mut self, V); } +/// Trait of object that can be rotated to be superimposed with another one of the same nature. +pub trait RotationTo { + /// Type of the angle between two elements. + type AngleType; + + /// Type of the rotation between two elements. + type DeltaRotationType; + + /// Computes an angle nedded to transform the first element to the second one using a + /// rotation. + fn angle_to(&self, other: &Self) -> Self::AngleType; + + /// Computes the smallest rotation needed to transform the first element to the second one. + fn rotation_to(&self, other: &Self) -> Self::DeltaRotationType; +} + /// Trait of objects able to rotate other objects. /// /// This is typically implemented by matrices which rotate vectors. diff --git a/src/traits/mod.rs b/src/traits/mod.rs index 30506892..1594064d 100644 --- a/src/traits/mod.rs +++ b/src/traits/mod.rs @@ -1,8 +1,9 @@ //! Mathematical traits. pub use traits::geometry::{AbsoluteRotate, Cross, CrossMatrix, Dot, FromHomogeneous, Norm, Orig, - Rotate, Rotation, RotationMatrix, RotationWithTranslation, ToHomogeneous, - Transform, Transformation, Translate, Translation, UniformSphereSample}; + Rotate, Rotation, RotationMatrix, RotationWithTranslation, RotationTo, + ToHomogeneous, Transform, Transformation, Translate, Translation, + UniformSphereSample}; pub use traits::structure::{FloatVec, FloatPnt, Basis, Cast, Col, Dim, Indexable, Iterable, IterableMut, Mat, SquareMat, Row, NumVec, NumPnt, PntAsVec, ColSlice, From b859b3280480a05e4c9169e3be7900aedc6e34e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Mon, 1 Jun 2015 22:37:54 +0200 Subject: [PATCH 2/2] Add tests for `::angle_between` and `::rotation_between`. --- tests/mat.rs | 46 ++++++++++++++++++++++++++++++++++-- tests/quat.rs | 21 +++++++++++++++++ tests/vec.rs | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++- 3 files changed, 128 insertions(+), 3 deletions(-) diff --git a/tests/mat.rs b/tests/mat.rs index c041569a..311b8906 100644 --- a/tests/mat.rs +++ b/tests/mat.rs @@ -2,8 +2,8 @@ extern crate nalgebra as na; extern crate rand; use rand::random; -use na::{Vec1, Vec3, Mat1, Mat2, Mat3, Mat4, Mat5, Mat6, Rot3, Persp3, PerspMat3, Ortho3, OrthoMat3, - DMat, DVec, Row, Col, BaseFloat}; +use na::{Vec1, Vec3, Mat1, Mat2, Mat3, Mat4, Mat5, Mat6, Rot2, Rot3, Persp3, PerspMat3, Ortho3, + OrthoMat3, DMat, DVec, Row, Col, BaseFloat}; macro_rules! test_inv_mat_impl( ($t: ty) => ( @@ -153,6 +153,48 @@ fn test_inv_rotation3() { } } +#[test] +fn test_rot3_rotation_between() { + let r1: Rot3 = random(); + let r2: Rot3 = random(); + + let delta = na::rotation_between(&r1, &r2); + + assert!(na::approx_eq(&(delta * r1), &r2)) +} + +#[test] +fn test_rot3_angle_between() { + let r1: Rot3 = random(); + let r2: Rot3 = random(); + + let delta = na::rotation_between(&r1, &r2); + let delta_angle = na::angle_between(&r1, &r2); + + assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle)) +} + +#[test] +fn test_rot2_rotation_between() { + let r1: Rot2 = random(); + let r2: Rot2 = random(); + + let delta = na::rotation_between(&r1, &r2); + + assert!(na::approx_eq(&(delta * r1), &r2)) +} + +#[test] +fn test_rot2_angle_between() { + let r1: Rot2 = random(); + let r2: Rot2 = random(); + + let delta = na::rotation_between(&r1, &r2); + let delta_angle = na::angle_between(&r1, &r2); + + assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle)) +} + #[test] fn test_mean_dmat() { let mat = DMat::from_row_vec( diff --git a/tests/quat.rs b/tests/quat.rs index df30a954..279ea623 100644 --- a/tests/quat.rs +++ b/tests/quat.rs @@ -69,3 +69,24 @@ fn test_quat_euler_angles() { assert!(na::approx_eq(&q.to_rot(), &m)) } } + +#[test] +fn test_quat_rotation_between() { + let q1: UnitQuat = random(); + let q2: UnitQuat = random(); + + let delta = na::rotation_between(&q1, &q2); + + assert!(na::approx_eq(&(delta * q1), &q2)) +} + +#[test] +fn test_quat_angle_between() { + let q1: UnitQuat = random(); + let q2: UnitQuat = random(); + + let delta = na::rotation_between(&q1, &q2); + let delta_angle = na::angle_between(&q1, &q2); + + assert!(na::approx_eq(&na::norm(&na::rotation(&delta)), &delta_angle)) +} diff --git a/tests/vec.rs b/tests/vec.rs index 140dca65..daceba03 100644 --- a/tests/vec.rs +++ b/tests/vec.rs @@ -2,7 +2,7 @@ extern crate nalgebra as na; extern crate rand; use rand::random; -use na::{Vec0, Vec1, Vec2, Vec3, Vec4, Vec5, Vec6, Mat3, Iterable, IterableMut}; +use na::{Vec0, Vec1, Vec2, Vec3, Vec4, Vec5, Vec6, Mat3, Rot2, Rot3, Iterable, IterableMut}; macro_rules! test_iterator_impl( ($t: ty, $n: ty) => ( @@ -317,3 +317,65 @@ fn test_outer_vec3() { 8.0, 10.0, 12.0, 12.0, 15.0, 18.0)); } + + +#[test] +fn test_vec3_rotation_between() { + for _ in (0usize .. 10000) { + let v1: Vec3 = random(); + + let mut v2: Vec3 = random(); + v2 = na::normalize(&v2) * na::norm(&v1); + + let rot = na::rotation_between(&v1, &v2); + + assert!(na::approx_eq(&(rot * v1), &v2)) + } +} + +#[test] +fn test_vec3_angle_between() { + for _ in (0usize .. 10000) { + let vec: Vec3 = random(); + let other: Vec3 = random(); + + // Ensure the axis we are using is orthogonal to `vec`. + let axis_ang = na::cross(&vec, &other); + let ang = na::norm(&axis_ang); + let rot = Rot3::new(axis_ang); + + let delta = na::angle_between(&vec, &(rot * vec)); + + assert!(na::approx_eq(&ang, &delta)) + } +} + + +#[test] +fn test_vec2_rotation_between() { + for _ in (0usize .. 10000) { + let v1: Vec2 = random(); + + let mut v2: Vec2 = random(); + v2 = na::normalize(&v2) * na::norm(&v1); + + let rot = na::rotation_between(&v1, &v2); + + assert!(na::approx_eq(&(rot * v1), &v2)) + } +} + +#[test] +fn test_vec2_angle_between() { + for _ in (0usize .. 10000) { + let axis_ang: Vec1 = random(); + let ang = na::norm(&axis_ang); + + let rot: Rot2 = Rot2::new(axis_ang); + let vec: Vec2 = random(); + + let delta = na::angle_between(&vec, &(rot * vec)); + + assert!(na::approx_eq(&ang, &delta)) + } +}