Switch to an Euler angle type for defining rotations
This commit is contained in:
parent
a521a9254f
commit
0259acb87f
8 changed files with 220 additions and 114 deletions
|
@ -21,7 +21,7 @@ extern crate cgmath;
|
||||||
|
|
||||||
use rand::{IsaacRng, Rng};
|
use rand::{IsaacRng, Rng};
|
||||||
use test::Bencher;
|
use test::Bencher;
|
||||||
use cgmath::{Quaternion, Basis2, Basis3, Vector3, Rotation2, Rotation3, Rad};
|
use cgmath::*;
|
||||||
|
|
||||||
#[path="common/macros.rs"]
|
#[path="common/macros.rs"]
|
||||||
#[macro_use] mod macros;
|
#[macro_use] mod macros;
|
||||||
|
@ -55,7 +55,7 @@ fn _bench_rot3_from_axisangle(bh: &mut Bencher) {
|
||||||
bench_from_axis_angle::<Basis3<f32>>(bh)
|
bench_from_axis_angle::<Basis3<f32>>(bh)
|
||||||
}
|
}
|
||||||
|
|
||||||
bench_construction!(_bench_rot2_from_axisangle, Basis2<f32>, Rotation2::from_angle [ angle: Rad<f32> ]);
|
bench_construction!(_bench_rot2_from_axisangle, Basis2<f32>, Basis2::from_angle [ angle: Rad<f32> ]);
|
||||||
|
|
||||||
bench_construction!(_bench_quat_from_euler_angles, Quaternion<f32>, Rotation3::from_euler [roll: Rad<f32>, pitch: Rad<f32>, yaw: Rad<f32>]);
|
bench_construction!(_bench_quat_from_euler_angles, Quaternion<f32>, Quaternion::from [src: Euler<Rad<f32>>]);
|
||||||
bench_construction!(_bench_rot3_from_euler_angles, Basis3<f32>, Rotation3::from_euler [roll: Rad<f32>, pitch: Rad<f32>, yaw: Rad<f32>]);
|
bench_construction!(_bench_rot3_from_euler_angles, Basis3<f32>, Basis3::from [src: Euler<Rad<f32>>]);
|
||||||
|
|
127
src/euler.rs
Normal file
127
src/euler.rs
Normal file
|
@ -0,0 +1,127 @@
|
||||||
|
// Copyright 2016 The CGMath Developers. For a full listing of the authors,
|
||||||
|
// refer to the Cargo.toml file at the top-level directory of this distribution.
|
||||||
|
//
|
||||||
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
// you may not use this file except in compliance with the License.
|
||||||
|
// You may obtain a copy of the License at
|
||||||
|
//
|
||||||
|
// http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
//
|
||||||
|
// Unless required by applicable law or agreed to in writing, software
|
||||||
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
// See the License for the specific language governing permissions and
|
||||||
|
// limitations under the License.
|
||||||
|
|
||||||
|
use rand::{Rand, Rng};
|
||||||
|
use num_traits::{cast, Zero};
|
||||||
|
|
||||||
|
use structure::*;
|
||||||
|
|
||||||
|
use angle::Rad;
|
||||||
|
use approx::ApproxEq;
|
||||||
|
use quaternion::Quaternion;
|
||||||
|
use num::BaseFloat;
|
||||||
|
|
||||||
|
/// A set of [Euler angles] representing a rotation in three-dimensional space.
|
||||||
|
///
|
||||||
|
/// This type is marked as `#[repr(C, packed)]`.
|
||||||
|
///
|
||||||
|
/// # Defining rotations using Euler angles
|
||||||
|
///
|
||||||
|
/// Note that while [Euler angles] are intuitive to define, they are prone to
|
||||||
|
/// [gimbal lock] and are challenging to interpolate between. Instead we
|
||||||
|
/// recommend that you convert them to a more robust representation, such as a
|
||||||
|
/// quaternion or or rotation matrix. To this end, `From<Euler<A>>` conversions
|
||||||
|
/// are provided for the following types:
|
||||||
|
///
|
||||||
|
/// - [`Basis3`](struct.Basis3.html)
|
||||||
|
/// - [`Matrix3`](struct.Matrix3.html)
|
||||||
|
/// - [`Matrix4`](struct.Matrix4.html)
|
||||||
|
/// - [`Quaternion`](struct.Quaternion.html)
|
||||||
|
///
|
||||||
|
/// For example, to define a quaternion that applies the following:
|
||||||
|
///
|
||||||
|
/// 1. a 45° rotation around the _x_ axis
|
||||||
|
/// 2. a 180° rotation around the _y_ axis
|
||||||
|
/// 3. a -30° rotation around the _z_ axis
|
||||||
|
///
|
||||||
|
/// you can use the following code:
|
||||||
|
///
|
||||||
|
/// ```
|
||||||
|
/// use cgmath::{Deg, Euler, Quaternion};
|
||||||
|
///
|
||||||
|
/// let rotation = Quaternion::from(Euler {
|
||||||
|
/// x: Deg::new(45.0),
|
||||||
|
/// y: Deg::new(180.0),
|
||||||
|
/// z: Deg::new(15.0),
|
||||||
|
/// });
|
||||||
|
/// ```
|
||||||
|
///
|
||||||
|
/// [Euler angles]: https://en.wikipedia.org/wiki/Euler_angles
|
||||||
|
/// [gimbal lock]: https://en.wikipedia.org/wiki/Gimbal_lock#Gimbal_lock_in_applied_mathematics
|
||||||
|
/// [convert]: #defining-rotations-using-euler-angles
|
||||||
|
#[repr(C, packed)]
|
||||||
|
#[derive(Copy, Clone, Debug)]
|
||||||
|
#[derive(PartialEq, Eq)]
|
||||||
|
#[derive(RustcEncodable, RustcDecodable)]
|
||||||
|
pub struct Euler<A: Angle> {
|
||||||
|
/// The angle to apply around the _x_ axis.
|
||||||
|
pub x: A,
|
||||||
|
/// The angle to apply around the _y_ axis.
|
||||||
|
pub y: A,
|
||||||
|
/// The angle to apply around the _z_ axis.
|
||||||
|
pub z: A,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<S: BaseFloat> From<Quaternion<S>> for Euler<Rad<S>> {
|
||||||
|
fn from(src: Quaternion<S>) -> Euler<Rad<S>> {
|
||||||
|
let sig: S = cast(0.499).unwrap();
|
||||||
|
let two: S = cast(2).unwrap();
|
||||||
|
let one: S = cast(1).unwrap();
|
||||||
|
|
||||||
|
let (qw, qx, qy, qz) = (src.s, src.v.x, src.v.y, src.v.z);
|
||||||
|
let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz);
|
||||||
|
|
||||||
|
let unit = sqx + sqy + sqz + sqw;
|
||||||
|
let test = qx * qy + qz * qw;
|
||||||
|
|
||||||
|
if test > sig * unit {
|
||||||
|
Euler {
|
||||||
|
x: Rad::turn_div_4(),
|
||||||
|
y: Rad::zero(),
|
||||||
|
z: Rad::atan2(qx, qw) * two,
|
||||||
|
}
|
||||||
|
} else if test < -sig * unit {
|
||||||
|
Euler {
|
||||||
|
x: -Rad::turn_div_4(),
|
||||||
|
y: Rad::zero(),
|
||||||
|
z: Rad::atan2(qx, qw) * two,
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
Euler {
|
||||||
|
x: Rad::asin(two * (qx * qy + qz * qw)),
|
||||||
|
y: Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)),
|
||||||
|
z: Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A: Angle> ApproxEq for Euler<A> {
|
||||||
|
type Epsilon = A::Unitless;
|
||||||
|
|
||||||
|
#[inline]
|
||||||
|
fn approx_eq_eps(&self, other: &Euler<A>, epsilon: &A::Unitless) -> bool {
|
||||||
|
self.x.approx_eq_eps(&other.x, epsilon) &&
|
||||||
|
self.y.approx_eq_eps(&other.y, epsilon) &&
|
||||||
|
self.z.approx_eq_eps(&other.z, epsilon)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A: Angle + Rand> Rand for Euler<A> {
|
||||||
|
#[inline]
|
||||||
|
fn rand<R: Rng>(rng: &mut R) -> Euler<A> {
|
||||||
|
Euler { x: rng.gen(), y: rng.gen(), z: rng.gen() }
|
||||||
|
}
|
||||||
|
}
|
|
@ -64,6 +64,7 @@ pub use quaternion::Quaternion;
|
||||||
pub use vector::{Vector2, Vector3, Vector4, dot, vec2, vec3, vec4};
|
pub use vector::{Vector2, Vector3, Vector4, dot, vec2, vec3, vec4};
|
||||||
|
|
||||||
pub use angle::{Deg, Rad, deg, rad};
|
pub use angle::{Deg, Rad, deg, rad};
|
||||||
|
pub use euler::Euler;
|
||||||
pub use point::{Point2, Point3};
|
pub use point::{Point2, Point3};
|
||||||
pub use rotation::*;
|
pub use rotation::*;
|
||||||
pub use transform::*;
|
pub use transform::*;
|
||||||
|
@ -88,6 +89,7 @@ mod quaternion;
|
||||||
mod vector;
|
mod vector;
|
||||||
|
|
||||||
mod angle;
|
mod angle;
|
||||||
|
mod euler;
|
||||||
mod point;
|
mod point;
|
||||||
mod rotation;
|
mod rotation;
|
||||||
mod transform;
|
mod transform;
|
||||||
|
|
|
@ -25,6 +25,7 @@ use structure::*;
|
||||||
|
|
||||||
use angle::Rad;
|
use angle::Rad;
|
||||||
use approx::ApproxEq;
|
use approx::ApproxEq;
|
||||||
|
use euler::Euler;
|
||||||
use num::BaseFloat;
|
use num::BaseFloat;
|
||||||
use point::Point3;
|
use point::Point3;
|
||||||
use quaternion::Quaternion;
|
use quaternion::Quaternion;
|
||||||
|
@ -159,24 +160,6 @@ impl<S: BaseFloat> Matrix3<S> {
|
||||||
S::zero(), S::zero(), S::one())
|
S::zero(), S::zero(), S::one())
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Create a rotation matrix from a set of euler angles.
|
|
||||||
///
|
|
||||||
/// # Parameters
|
|
||||||
///
|
|
||||||
/// - `x`: the angular rotation around the `x` axis (pitch).
|
|
||||||
/// - `y`: the angular rotation around the `y` axis (yaw).
|
|
||||||
/// - `z`: the angular rotation around the `z` axis (roll).
|
|
||||||
pub fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Matrix3<S> {
|
|
||||||
// http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
|
|
||||||
let (sx, cx) = Rad::sin_cos(x);
|
|
||||||
let (sy, cy) = Rad::sin_cos(y);
|
|
||||||
let (sz, cz) = Rad::sin_cos(z);
|
|
||||||
|
|
||||||
Matrix3::new(cy * cz, cy * sz, -sy,
|
|
||||||
-cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy,
|
|
||||||
sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Create a rotation matrix from an angle around an arbitrary axis.
|
/// Create a rotation matrix from an angle around an arbitrary axis.
|
||||||
pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix3<S> {
|
pub fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Matrix3<S> {
|
||||||
let (s, c) = Rad::sin_cos(angle);
|
let (s, c) = Rad::sin_cos(angle);
|
||||||
|
@ -196,6 +179,37 @@ impl<S: BaseFloat> Matrix3<S> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
impl<A> From<Euler<A>> for Matrix3<<A as Angle>::Unitless> where
|
||||||
|
A: Angle + Into<Rad<<A as Angle>::Unitless>>,
|
||||||
|
{
|
||||||
|
fn from(src: Euler<A>) -> Matrix3<A::Unitless> {
|
||||||
|
// http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
|
||||||
|
let (sx, cx) = Rad::sin_cos(src.x.into());
|
||||||
|
let (sy, cy) = Rad::sin_cos(src.y.into());
|
||||||
|
let (sz, cz) = Rad::sin_cos(src.z.into());
|
||||||
|
|
||||||
|
Matrix3::new(cy * cz, cy * sz, -sy,
|
||||||
|
-cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy,
|
||||||
|
sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<A> From<Euler<A>> for Matrix4<<A as Angle>::Unitless> where
|
||||||
|
A: Angle + Into<Rad<<A as Angle>::Unitless>>,
|
||||||
|
{
|
||||||
|
fn from(src: Euler<A>) -> Matrix4<A::Unitless> {
|
||||||
|
// http://en.wikipedia.org/wiki/Rotation_matrix#General_rotations
|
||||||
|
let (sx, cx) = Rad::sin_cos(src.x.into());
|
||||||
|
let (sy, cy) = Rad::sin_cos(src.y.into());
|
||||||
|
let (sz, cz) = Rad::sin_cos(src.z.into());
|
||||||
|
|
||||||
|
Matrix4::new(cy * cz, cy * sz, -sy, A::Unitless::zero(),
|
||||||
|
-cx * sz + sx * sy * cz, cx * cz + sx * sy * sz, sx * cy, A::Unitless::zero(),
|
||||||
|
sx * sz + cx * sy * cz, -sx * cz + cx * sy * sz, cx * cy, A::Unitless::zero(),
|
||||||
|
A::Unitless::zero(), A::Unitless::zero(), A::Unitless::zero(), A::Unitless::one())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
impl<S: BaseFloat> Matrix4<S> {
|
impl<S: BaseFloat> Matrix4<S> {
|
||||||
/// Create a new matrix, providing values for each index.
|
/// Create a new matrix, providing values for each index.
|
||||||
#[inline]
|
#[inline]
|
||||||
|
|
|
@ -24,6 +24,7 @@ use structure::*;
|
||||||
|
|
||||||
use angle::Rad;
|
use angle::Rad;
|
||||||
use approx::ApproxEq;
|
use approx::ApproxEq;
|
||||||
|
use euler::Euler;
|
||||||
use matrix::{Matrix3, Matrix4};
|
use matrix::{Matrix3, Matrix4};
|
||||||
use num::BaseFloat;
|
use num::BaseFloat;
|
||||||
use point::Point3;
|
use point::Point3;
|
||||||
|
@ -117,44 +118,6 @@ impl<S: BaseFloat> Quaternion<S> {
|
||||||
(self * scale1 + other * scale2) * Rad::sin(theta).recip()
|
(self * scale1 + other * scale2) * Rad::sin(theta).recip()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Convert a Quaternion to Eular angles
|
|
||||||
/// This is a polar singularity aware conversion
|
|
||||||
///
|
|
||||||
/// Based on:
|
|
||||||
/// - [Maths - Conversion Quaternion to Euler]
|
|
||||||
/// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/)
|
|
||||||
pub fn to_euler(self) -> (Rad<S>, Rad<S>, Rad<S>) {
|
|
||||||
let sig: S = cast(0.499f64).unwrap();
|
|
||||||
let two: S = cast(2f64).unwrap();
|
|
||||||
let one: S = cast(1f64).unwrap();
|
|
||||||
|
|
||||||
let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z);
|
|
||||||
let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz);
|
|
||||||
|
|
||||||
let unit = sqx + sqy + sqz + sqw;
|
|
||||||
let test = qx * qy + qz * qw;
|
|
||||||
|
|
||||||
if test > sig * unit {
|
|
||||||
(
|
|
||||||
Rad::zero(),
|
|
||||||
Rad::turn_div_4(),
|
|
||||||
Rad::atan2(qx, qw) * two,
|
|
||||||
)
|
|
||||||
} else if test < -sig * unit {
|
|
||||||
(
|
|
||||||
Rad::zero(),
|
|
||||||
-Rad::turn_div_4(),
|
|
||||||
Rad::atan2(qx, qw) * two,
|
|
||||||
)
|
|
||||||
} else {
|
|
||||||
(
|
|
||||||
Rad::atan2(two * (qy * qw - qx * qz), one - two * (sqy + sqz)),
|
|
||||||
Rad::asin(two * (qx * qy + qz * qw)),
|
|
||||||
Rad::atan2(two * (qx * qw - qy * qz), one - two * (sqx + sqz)),
|
|
||||||
)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<S: BaseFloat> VectorSpace for Quaternion<S> {
|
impl<S: BaseFloat> VectorSpace for Quaternion<S> {
|
||||||
|
@ -173,6 +136,24 @@ impl<S: BaseFloat> InnerSpace for Quaternion<S> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
impl<A> From<Euler<A>> for Quaternion<<A as Angle>::Unitless> where
|
||||||
|
A: Angle + Into<Rad<<A as Angle>::Unitless>>,
|
||||||
|
{
|
||||||
|
fn from(src: Euler<A>) -> Quaternion<A::Unitless> {
|
||||||
|
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
|
||||||
|
|
||||||
|
let half = cast(0.5f64).unwrap();
|
||||||
|
let (s_x, c_x) = Rad::sin_cos(src.x.into() * half);
|
||||||
|
let (s_y, c_y) = Rad::sin_cos(src.y.into() * half);
|
||||||
|
let (s_z, c_z) = Rad::sin_cos(src.z.into() * half);
|
||||||
|
|
||||||
|
Quaternion::new(c_y * c_x * c_z - s_y * s_x * s_z,
|
||||||
|
s_y * s_x * c_z + c_y * c_x * s_z,
|
||||||
|
s_y * c_x * c_z + c_y * s_x * s_z,
|
||||||
|
c_y * s_x * c_z - s_y * c_x * s_z)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
impl_operator!(<S: BaseFloat> Neg for Quaternion<S> {
|
impl_operator!(<S: BaseFloat> Neg for Quaternion<S> {
|
||||||
fn neg(quat) -> Quaternion<S> {
|
fn neg(quat) -> Quaternion<S> {
|
||||||
Quaternion::from_sv(-quat.s, -quat.v)
|
Quaternion::from_sv(-quat.s, -quat.v)
|
||||||
|
@ -373,19 +354,6 @@ impl<S: BaseFloat> Rotation3<S> for Quaternion<S> {
|
||||||
let (s, c) = Rad::sin_cos(angle * cast(0.5f64).unwrap());
|
let (s, c) = Rad::sin_cos(angle * cast(0.5f64).unwrap());
|
||||||
Quaternion::from_sv(c, axis * s)
|
Quaternion::from_sv(c, axis * s)
|
||||||
}
|
}
|
||||||
|
|
||||||
/// - [Maths - Conversion Euler to Quaternion]
|
|
||||||
/// (http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm)
|
|
||||||
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Quaternion<S> {
|
|
||||||
let (s1, c1) = Rad::sin_cos(x * cast(0.5f64).unwrap());
|
|
||||||
let (s2, c2) = Rad::sin_cos(y * cast(0.5f64).unwrap());
|
|
||||||
let (s3, c3) = Rad::sin_cos(z * cast(0.5f64).unwrap());
|
|
||||||
|
|
||||||
Quaternion::new(c1 * c2 * c3 - s1 * s2 * s3,
|
|
||||||
s1 * s2 * c3 + c1 * c2 * s3,
|
|
||||||
s1 * c2 * c3 + c1 * s2 * s3,
|
|
||||||
c1 * s2 * c3 - s1 * c2 * s3)
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<S: BaseFloat> Into<[S; 4]> for Quaternion<S> {
|
impl<S: BaseFloat> Into<[S; 4]> for Quaternion<S> {
|
||||||
|
|
|
@ -19,6 +19,7 @@ use structure::*;
|
||||||
|
|
||||||
use angle::Rad;
|
use angle::Rad;
|
||||||
use approx::ApproxEq;
|
use approx::ApproxEq;
|
||||||
|
use euler::Euler;
|
||||||
use matrix::{Matrix2, Matrix3};
|
use matrix::{Matrix2, Matrix3};
|
||||||
use num::BaseFloat;
|
use num::BaseFloat;
|
||||||
use point::{Point2, Point3};
|
use point::{Point2, Point3};
|
||||||
|
@ -85,19 +86,11 @@ pub trait Rotation2<S: BaseFloat>: Rotation<Point2<S>>
|
||||||
pub trait Rotation3<S: BaseFloat>: Rotation<Point3<S>>
|
pub trait Rotation3<S: BaseFloat>: Rotation<Point3<S>>
|
||||||
+ Into<Matrix3<S>>
|
+ Into<Matrix3<S>>
|
||||||
+ Into<Basis3<S>>
|
+ Into<Basis3<S>>
|
||||||
+ Into<Quaternion<S>> {
|
+ Into<Quaternion<S>>
|
||||||
|
+ From<Euler<Rad<S>>> {
|
||||||
/// Create a rotation using an angle around a given axis.
|
/// Create a rotation using an angle around a given axis.
|
||||||
fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Self;
|
fn from_axis_angle(axis: Vector3<S>, angle: Rad<S>) -> Self;
|
||||||
|
|
||||||
/// Create a rotation from a set of euler angles.
|
|
||||||
///
|
|
||||||
/// # Parameters
|
|
||||||
///
|
|
||||||
/// - `x`: the angular rotation around the `x` axis (pitch).
|
|
||||||
/// - `y`: the angular rotation around the `y` axis (yaw).
|
|
||||||
/// - `z`: the angular rotation around the `z` axis (roll).
|
|
||||||
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Self;
|
|
||||||
|
|
||||||
/// Create a rotation from an angle around the `x` axis (pitch).
|
/// Create a rotation from an angle around the `x` axis (pitch).
|
||||||
#[inline]
|
#[inline]
|
||||||
fn from_angle_x(theta: Rad<S>) -> Self {
|
fn from_angle_x(theta: Rad<S>) -> Self {
|
||||||
|
@ -317,10 +310,6 @@ impl<S: BaseFloat> Rotation3<S> for Basis3<S> {
|
||||||
Basis3 { mat: Matrix3::from_axis_angle(axis, angle) }
|
Basis3 { mat: Matrix3::from_axis_angle(axis, angle) }
|
||||||
}
|
}
|
||||||
|
|
||||||
fn from_euler(x: Rad<S>, y: Rad<S>, z: Rad<S>) -> Basis3<S> {
|
|
||||||
Basis3 { mat: Matrix3::from_euler(x, y ,z) }
|
|
||||||
}
|
|
||||||
|
|
||||||
fn from_angle_x(theta: Rad<S>) -> Basis3<S> {
|
fn from_angle_x(theta: Rad<S>) -> Basis3<S> {
|
||||||
Basis3 { mat: Matrix3::from_angle_x(theta) }
|
Basis3 { mat: Matrix3::from_angle_x(theta) }
|
||||||
}
|
}
|
||||||
|
@ -334,6 +323,17 @@ impl<S: BaseFloat> Rotation3<S> for Basis3<S> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
impl<A: Angle> From<Euler<A>> for Basis3<<A as Angle>::Unitless> where
|
||||||
|
A: Into<Rad<<A as Angle>::Unitless>>,
|
||||||
|
{
|
||||||
|
/// Create a three-dimensional rotation matrix from a set of euler angles.
|
||||||
|
fn from(src: Euler<A>) -> Basis3<A::Unitless> {
|
||||||
|
Basis3 {
|
||||||
|
mat: Matrix3::from(src),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
impl<S: fmt::Debug> fmt::Debug for Basis3<S> {
|
impl<S: fmt::Debug> fmt::Debug for Basis3<S> {
|
||||||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||||||
try!(write!(f, "Basis3 "));
|
try!(write!(f, "Basis3 "));
|
||||||
|
|
|
@ -332,7 +332,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_x(pitch: Rad<f32>) {
|
fn check_from_axis_angle_x(pitch: Rad<f32>) {
|
||||||
let found = Matrix3::from_angle_x(pitch);
|
let found = Matrix3::from_angle_x(pitch);
|
||||||
let expected = Matrix3::from_euler(pitch, rad(0.0), rad(0.0));
|
let expected = Matrix3::from(Euler { x: pitch, y: rad(0.0), z: rad(0.0) });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -346,7 +346,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_y(yaw: Rad<f32>) {
|
fn check_from_axis_angle_y(yaw: Rad<f32>) {
|
||||||
let found = Matrix3::from_angle_y(yaw);
|
let found = Matrix3::from_angle_y(yaw);
|
||||||
let expected = Matrix3::from_euler(rad(0.0), yaw, rad(0.0));
|
let expected = Matrix3::from(Euler { x: rad(0.0), y: yaw, z: rad(0.0) });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -360,7 +360,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_z(roll: Rad<f32>) {
|
fn check_from_axis_angle_z(roll: Rad<f32>) {
|
||||||
let found = Matrix3::from_angle_z(roll);
|
let found = Matrix3::from_angle_z(roll);
|
||||||
let expected = Matrix3::from_euler(rad(0.0), rad(0.0), roll);
|
let expected = Matrix3::from(Euler { x: rad(0.0), y: rad(0.0), z: roll });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -375,7 +375,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_x(pitch: Rad<f32>) {
|
fn check_from_axis_angle_x(pitch: Rad<f32>) {
|
||||||
let found = Matrix3::from_axis_angle(Vector3::unit_x(), pitch);
|
let found = Matrix3::from_axis_angle(Vector3::unit_x(), pitch);
|
||||||
let expected = Matrix3::from_euler(pitch, rad(0.0), rad(0.0));
|
let expected = Matrix3::from(Euler { x: pitch, y: rad(0.0), z: rad(0.0) });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -389,7 +389,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_y(yaw: Rad<f32>) {
|
fn check_from_axis_angle_y(yaw: Rad<f32>) {
|
||||||
let found = Matrix3::from_axis_angle(Vector3::unit_y(), yaw);
|
let found = Matrix3::from_axis_angle(Vector3::unit_y(), yaw);
|
||||||
let expected = Matrix3::from_euler(rad(0.0), yaw, rad(0.0));
|
let expected = Matrix3::from(Euler { x: rad(0.0), y: yaw, z: rad(0.0) });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -403,7 +403,7 @@ pub mod matrix3 {
|
||||||
|
|
||||||
fn check_from_axis_angle_z(roll: Rad<f32>) {
|
fn check_from_axis_angle_z(roll: Rad<f32>) {
|
||||||
let found = Matrix3::from_axis_angle(Vector3::unit_z(), roll);
|
let found = Matrix3::from_axis_angle(Vector3::unit_z(), roll);
|
||||||
let expected = Matrix3::from_euler(rad(0.0), rad(0.0), roll);
|
let expected = Matrix3::from(Euler { x: rad(0.0), y: rad(0.0), z: roll });
|
||||||
assert_approx_eq_eps!(found, expected, 0.001);
|
assert_approx_eq_eps!(found, expected, 0.001);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -43,12 +43,12 @@ mod operators {
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn test_mul() {
|
fn test_mul() {
|
||||||
impl_test_mul!(2.0f32, Quaternion::from_euler(rad(1f32), rad(1f32), rad(1f32)));
|
impl_test_mul!(2.0f32, Quaternion::from(Euler { x: rad(1f32), y: rad(1f32), z: rad(1f32) }));
|
||||||
}
|
}
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn test_div() {
|
fn test_div() {
|
||||||
impl_test_div!(2.0f32, Quaternion::from_euler(rad(1f32), rad(1f32), rad(1f32)));
|
impl_test_div!(2.0f32, Quaternion::from(Euler { x: rad(1f32), y: rad(1f32), z: rad(1f32) }));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -57,28 +57,23 @@ mod to_from_euler {
|
||||||
|
|
||||||
use cgmath::*;
|
use cgmath::*;
|
||||||
|
|
||||||
fn check_euler(pitch: Rad<f32>, yaw: Rad<f32>, roll: Rad<f32>) {
|
fn check_euler(rotation: Euler<Rad<f32>>) {
|
||||||
let quat = Quaternion::from_euler(pitch, yaw, roll);
|
assert_approx_eq_eps!(Euler::from(Quaternion::from(rotation)), rotation, 0.001);
|
||||||
let (found_pitch, found_yaw, found_roll) = quat.to_euler();
|
|
||||||
|
|
||||||
assert_approx_eq_eps!(pitch, found_pitch, 0.001);
|
|
||||||
assert_approx_eq_eps!(yaw, found_yaw, 0.001);
|
|
||||||
assert_approx_eq_eps!(roll, found_roll, 0.001);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const HPI: f32 = f32::consts::FRAC_PI_2;
|
const HPI: f32 = f32::consts::FRAC_PI_2;
|
||||||
|
|
||||||
#[test] fn test_zero() { check_euler(rad(0f32), rad(0f32), rad(0f32)); }
|
#[test] fn test_zero() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad( 0f32) }); }
|
||||||
#[test] fn test_yaw_pos_1() { check_euler(rad(0f32), rad(1f32), rad(0f32)); }
|
#[test] fn test_yaw_pos_1() { check_euler(Euler { x: rad( 0f32), y: rad( 1f32), z: rad( 0f32) }); }
|
||||||
#[test] fn test_yaw_neg_1() { check_euler(rad(0f32), rad(-1f32), rad(0f32)); }
|
#[test] fn test_yaw_neg_1() { check_euler(Euler { x: rad( 0f32), y: rad(-1f32), z: rad( 0f32) }); }
|
||||||
#[test] fn test_pitch_pos_1() { check_euler(rad(1f32), rad(0f32), rad(0f32)); }
|
#[test] fn test_pitch_pos_1() { check_euler(Euler { x: rad( 1f32), y: rad( 0f32), z: rad( 0f32) }); }
|
||||||
#[test] fn test_pitch_neg_1() { check_euler(rad(-1f32), rad(0f32), rad(0f32)); }
|
#[test] fn test_pitch_neg_1() { check_euler(Euler { x: rad(-1f32), y: rad( 0f32), z: rad( 0f32) }); }
|
||||||
#[test] fn test_roll_pos_1() { check_euler(rad(0f32), rad(0f32), rad(1f32)); }
|
#[test] fn test_roll_pos_1() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad( 1f32) }); }
|
||||||
#[test] fn test_roll_neg_1() { check_euler(rad(0f32), rad(0f32), rad(-1f32)); }
|
#[test] fn test_roll_neg_1() { check_euler(Euler { x: rad( 0f32), y: rad( 0f32), z: rad(-1f32) }); }
|
||||||
#[test] fn test_pitch_yaw_roll_pos_1() { check_euler(rad(1f32), rad(1f32), rad(1f32)); }
|
#[test] fn test_pitch_yaw_roll_pos_1() { check_euler(Euler { x: rad( 1f32), y: rad( 1f32), z: rad( 1f32) }); }
|
||||||
#[test] fn test_pitch_yaw_roll_neg_1() { check_euler(rad(-1f32), rad(-1f32), rad(-1f32)); }
|
#[test] fn test_pitch_yaw_roll_neg_1() { check_euler(Euler { x: rad(-1f32), y: rad(-1f32), z: rad(-1f32) }); }
|
||||||
#[test] fn test_pitch_yaw_roll_pos_hp() { check_euler(rad(0f32), rad(HPI), rad(1f32)); }
|
#[test] fn test_pitch_yaw_roll_pos_hp() { check_euler(Euler { x: rad( 0f32), y: rad( HPI), z: rad( 1f32) }); }
|
||||||
#[test] fn test_pitch_yaw_roll_neg_hp() { check_euler(rad(0f32), rad(-HPI), rad(1f32)); }
|
#[test] fn test_pitch_yaw_roll_neg_hp() { check_euler(Euler { x: rad( 0f32), y: rad( -HPI), z: rad( 1f32) }); }
|
||||||
}
|
}
|
||||||
|
|
||||||
mod from {
|
mod from {
|
||||||
|
@ -86,7 +81,7 @@ mod from {
|
||||||
use cgmath::*;
|
use cgmath::*;
|
||||||
|
|
||||||
fn check_with_euler(x: Rad<f32>, y: Rad<f32>, z: Rad<f32>) {
|
fn check_with_euler(x: Rad<f32>, y: Rad<f32>, z: Rad<f32>) {
|
||||||
let matrix3 = Matrix3::from_euler(x, y, z);
|
let matrix3 = Matrix3::from(Euler { x: x, y: y, z: z });
|
||||||
let quaternion = Quaternion::from(matrix3);
|
let quaternion = Quaternion::from(matrix3);
|
||||||
let quaternion_matrix3 = Matrix3::from(quaternion);
|
let quaternion_matrix3 = Matrix3::from(quaternion);
|
||||||
assert_approx_eq!(matrix3, quaternion_matrix3);
|
assert_approx_eq!(matrix3, quaternion_matrix3);
|
||||||
|
|
Loading…
Reference in a new issue