From 00991e00f3d6e86fa663f12e31b4d10579a56c01 Mon Sep 17 00:00:00 2001 From: Brendan Zabarauskas Date: Tue, 3 Sep 2013 17:28:43 +1000 Subject: [PATCH] Copy quaternion method impls over from src-old, and add conversion traits --- src/cgmath/lib.rs | 6 +- src/cgmath/matrix.rs | 66 ++++++++++++ src/cgmath/quaternion.rs | 210 ++++++++++++++++++++++++++++++++++++++- src/cgmath/vector.rs | 5 + 4 files changed, 281 insertions(+), 6 deletions(-) diff --git a/src/cgmath/lib.rs b/src/cgmath/lib.rs index 708b86e..c683f21 100644 --- a/src/cgmath/lib.rs +++ b/src/cgmath/lib.rs @@ -33,7 +33,11 @@ pub mod projection; pub mod util { use std::num::one; - /// This is horrific. We really need better from-int support in std::num. + // These functions are horrific! We really need better from-int support + // in std::num. + #[inline] pub fn two() -> T { one::() + one::() } + #[inline] + pub fn half() -> T { one::() / two::() } } diff --git a/src/cgmath/matrix.rs b/src/cgmath/matrix.rs index fdc9aae..6a2096f 100644 --- a/src/cgmath/matrix.rs +++ b/src/cgmath/matrix.rs @@ -18,7 +18,9 @@ use std::num::{one, zero}; use array::*; +use quaternion::{Quat, ToQuat}; use vector::*; +use util::half; /// A 2 x 2, column major matrix #[deriving(Clone, Eq)] @@ -32,6 +34,11 @@ pub struct Mat3 { x: Vec3, y: Vec3, z: Vec3 } #[deriving(Clone, Eq)] pub struct Mat4 { x: Vec4, y: Vec4, z: Vec4, w: Vec4 } +// Conversion traits +pub trait ToMat2 { fn to_mat2(&self) -> Mat2; } +pub trait ToMat3 { fn to_mat3(&self) -> Mat3; } +pub trait ToMat4 { fn to_mat4(&self) -> Mat4; } + impl Mat2 { #[inline] pub fn new(c0r0: S, c0r1: S, @@ -95,6 +102,16 @@ impl Mat3 { } } +impl Mat3 { + pub fn look_at(dir: &Vec3, up: &Vec3) -> Mat3 { + let dir = dir.normalize(); + let side = dir.cross(&up.normalize()); + let up = side.cross(&dir).normalize(); + + Mat3::from_cols(up, side, dir) + } +} + impl Mat4 { #[inline] pub fn new(c0r0: S, c0r1: S, c0r2: S, c0r3: S, @@ -403,3 +420,52 @@ for Mat4 } } } + +impl ToQuat for Mat3 { + /// Convert the matrix to a quaternion + fn to_quat(&self) -> Quat { + // Implemented using a mix of ideas from jMonkeyEngine and Ken Shoemake's + // paper on Quaternions: http://www.cs.ucr.edu/~vbz/resources/Quatut.pdf + + let mut s; + let w; let x; let y; let z; + let trace = self.trace(); + + cond! ( + (trace >= zero::()) { + s = (one::() + trace).sqrt(); + w = half::() * s; + s = half::() / s; + x = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + } + ((*self.cr(0, 0) > *self.cr(1, 1)) + && (*self.cr(0, 0) > *self.cr(2, 2))) { + s = (half::() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2))).sqrt(); + w = half::() * s; + s = half::() / s; + x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + y = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + z = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + } + (*self.cr(1, 1) > *self.cr(2, 2)) { + s = (half::() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2))).sqrt(); + w = half::() * s; + s = half::() / s; + x = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + z = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + } + _ { + s = (half::() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1))).sqrt(); + w = half::() * s; + s = half::() / s; + x = (*self.cr(2, 0) - *self.cr(0, 2)) * s; + y = (*self.cr(1, 2) - *self.cr(2, 1)) * s; + z = (*self.cr(0, 1) - *self.cr(1, 0)) * s; + } + ) + Quat::new(w, x, y, z) + } +} diff --git a/src/cgmath/quaternion.rs b/src/cgmath/quaternion.rs index 63d1feb..592080a 100644 --- a/src/cgmath/quaternion.rs +++ b/src/cgmath/quaternion.rs @@ -13,23 +13,223 @@ // See the License for the specific language governing permissions and // limitations under the License. -use vector::Vec3; +use std::num::{zero, one, sqrt}; + +use util::two; +use matrix::{Mat3, ToMat3}; +use vector::{Vec3, Vector, EuclideanVector}; /// A quaternion in scalar/vector form #[deriving(Clone, Eq)] -pub struct Quat { s: T, v: Vec3 } +pub struct Quat { s: S, v: Vec3 } -impl Quat { +pub trait ToQuat { + fn to_quat(&self) -> Quat; +} + +impl Quat { /// Construct a new quaternion from one scalar component and three /// imaginary components #[inline] - pub fn new(w: T, xi: T, yj: T, zk: T) -> Quat { + pub fn new(w: S, xi: S, yj: S, zk: S) -> Quat { Quat::from_sv(w, Vec3::new(xi, yj, zk)) } /// Construct a new quaternion from a scalar and a vector #[inline] - pub fn from_sv(s: T, v: Vec3) -> Quat { + pub fn from_sv(s: S, v: Vec3) -> Quat { Quat { s: s, v: v } } + + #[inline] + pub fn look_at(dir: &Vec3, up: &Vec3) -> Quat { + Mat3::look_at(dir, up).to_quat() + } + + /// The additive identity, ie: `q = 0 + 0i + 0j + 0i` + #[inline] + pub fn zero() -> Quat { + Quat::new(zero(), zero(), zero(), zero()) + } + + /// The multiplicative identity, ie: `q = 1 + 0i + 0j + 0i` + #[inline] + pub fn identity() -> Quat { + Quat::from_sv(one::(), Vec3::zero()) + } + + /// The result of multiplying the quaternion a scalar + #[inline] + pub fn mul_s(&self, value: S) -> Quat { + Quat::from_sv(self.s * value, self.v.mul_s(value)) + } + + /// The result of dividing the quaternion a scalar + #[inline] + pub fn div_s(&self, value: S) -> Quat { + Quat::from_sv(self.s / value, self.v.div_s(value)) + } + + /// The result of multiplying the quaternion by a vector + #[inline] + pub fn mul_v(&self, vec: &Vec3) -> Vec3 { + let tmp = self.v.cross(vec).add_v(&vec.mul_s(self.s.clone())); + self.v.cross(&tmp).mul_s(two::()).add_v(vec) + } + + /// The sum of this quaternion and `other` + #[inline] + pub fn add_q(&self, other: &Quat) -> Quat { + Quat::new(self.s + other.s, + self.v.x + other.v.x, + self.v.y + other.v.y, + self.v.z + other.v.z) + } + + /// The sum of this quaternion and `other` + #[inline] + pub fn sub_q(&self, other: &Quat) -> Quat { + Quat::new(self.s - other.s, + self.v.x - other.v.x, + self.v.y - other.v.y, + self.v.z - other.v.z) + } + + /// The the result of multipliplying the quaternion by `other` + pub fn mul_q(&self, other: &Quat) -> Quat { + Quat::new(self.s * other.s - self.v.x * other.v.x - self.v.y * other.v.y - self.v.z * other.v.z, + self.s * other.v.x + self.v.x * other.s + self.v.y * other.v.z - self.v.z * other.v.y, + self.s * other.v.y + self.v.y * other.s + self.v.z * other.v.x - self.v.x * other.v.z, + self.s * other.v.z + self.v.z * other.s + self.v.x * other.v.y - self.v.y * other.v.x) + } + + /// The dot product of the quaternion and `other` + #[inline] + pub fn dot(&self, other: &Quat) -> S { + self.s * other.s + self.v.dot(&other.v) + } + + /// The conjugate of the quaternion + #[inline] + pub fn conjugate(&self) -> Quat { + Quat::from_sv(self.s.clone(), -self.v.clone()) + } + + /// The multiplicative inverse of the quaternion + #[inline] + pub fn inverse(&self) -> Quat { + self.conjugate().div_s(self.magnitude2()) + } + + /// The squared magnitude of the quaternion. This is useful for + /// magnitude comparisons where the exact magnitude does not need to be + /// calculated. + #[inline] + pub fn magnitude2(&self) -> S { + self.s * self.s + self.v.length2() + } + + /// The magnitude of the quaternion + /// + /// # Performance notes + /// + /// For instances where the exact magnitude of the quaternion does not need + /// to be known, for example for quaternion-quaternion magnitude comparisons, + /// it is advisable to use the `magnitude2` method instead. + #[inline] + pub fn magnitude(&self) -> S { + sqrt(self.magnitude2()) + } + + /// The normalized quaternion + #[inline] + pub fn normalize(&self) -> Quat { + self.mul_s(one::() / self.magnitude()) + } + + /// Normalised linear interpolation + /// + /// # Return value + /// + /// The intoperlated quaternion + pub fn nlerp(&self, other: &Quat, amount: S) -> Quat { + self.mul_s(one::() - amount).add_q(&other.mul_s(amount)).normalize() + } +} + +impl Quat { + /// Spherical Linear Intoperlation + /// + /// Perform a spherical linear interpolation between the quaternion and + /// `other`. Both quaternions should be normalized first. + /// + /// # Return value + /// + /// The intoperlated quaternion + /// + /// # Performance notes + /// + /// The `acos` operation used in `slerp` is an expensive operation, so unless + /// your quarternions a far away from each other it's generally more advisable + /// to use `nlerp` when you know your rotations are going to be small. + /// + /// - [Understanding Slerp, Then Not Using It] + /// (http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/) + /// - [Arcsynthesis OpenGL tutorial] + /// (http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Interpolation.html) + pub fn slerp(&self, other: &Quat, amount: S) -> Quat { + use std::num::cast; + + let dot = self.dot(other); + let dot_threshold = cast(0.9995); + + // if quaternions are close together use `nlerp` + if dot > dot_threshold { + self.nlerp(other, amount) + } else { + // stay within the domain of acos() + let robust_dot = dot.clamp(&-one::(), &one::()); + + let theta_0 = robust_dot.acos(); // the angle between the quaternions + let theta = theta_0 * amount; // the fraction of theta specified by `amount` + + let q = other.sub_q(&self.mul_s(robust_dot)) + .normalize(); + + self.mul_s(theta.cos()) + .add_q(&q.mul_s(theta.sin())) + } + } +} + +impl ToMat3 for Quat { + /// Convert the quaternion to a 3 x 3 rotation matrix + fn to_mat3(&self) -> Mat3 { + let x2 = self.v.x + self.v.x; + let y2 = self.v.y + self.v.y; + let z2 = self.v.z + self.v.z; + + let xx2 = x2 * self.v.x; + let xy2 = x2 * self.v.y; + let xz2 = x2 * self.v.z; + + let yy2 = y2 * self.v.y; + let yz2 = y2 * self.v.z; + let zz2 = z2 * self.v.z; + + let sy2 = y2 * self.s; + let sz2 = z2 * self.s; + let sx2 = x2 * self.s; + + Mat3::new(one::() - yy2 - zz2, xy2 + sz2, xz2 - sy2, + xy2 - sz2, one::() - xx2 - zz2, yz2 + sx2, + xz2 + sy2, yz2 - sx2, one::() - xx2 - yy2) + } +} + +impl Neg> for Quat { + #[inline] + fn neg(&self) -> Quat { + Quat::from_sv(-self.s, -self.v) + } } diff --git a/src/cgmath/vector.rs b/src/cgmath/vector.rs index fed143a..9baec6d 100644 --- a/src/cgmath/vector.rs +++ b/src/cgmath/vector.rs @@ -30,6 +30,11 @@ pub struct Vec3 { x: S, y: S, z: S } #[deriving(Eq, Clone, Zero)] pub struct Vec4 { x: S, y: S, z: S, w: S } +// Conversion traits +pub trait ToVec2 { fn to_vec2(&self) -> Vec2; } +pub trait ToVec3 { fn to_vec3(&self) -> Vec3; } +pub trait ToVec4 { fn to_vec4(&self) -> Vec4; } + // Utility macro for generating associated functions for the vectors macro_rules! vec( (impl $Self:ident <$S:ident> { $($field:ident),+ }) => (