Copy quaternion method impls over from src-old, and add conversion traits

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: Num>() -> T { one::<T>() + one::<T>() }
+    #[inline]
+    pub fn half<T: Real>() -> T { one::<T>() / two::<T>() }
 }

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<S> { x: Vec3<S>, y: Vec3<S>, z: Vec3<S> }
 #[deriving(Clone, Eq)]
 pub struct Mat4<S> { x: Vec4<S>, y: Vec4<S>, z: Vec4<S>, w: Vec4<S> }
 
+// Conversion traits
+pub trait ToMat2<S: Clone + Num> { fn to_mat2(&self) -> Mat2<S>; }
+pub trait ToMat3<S: Clone + Num> { fn to_mat3(&self) -> Mat3<S>; }
+pub trait ToMat4<S: Clone + Num> { fn to_mat4(&self) -> Mat4<S>; }
+
 impl<S: Clone + Num> Mat2<S> {
     #[inline]
     pub fn new(c0r0: S, c0r1: S,
@@ -95,6 +102,16 @@ impl<S: Clone + Num> Mat3<S> {
     }
 }
 
+impl<S: Clone + Float> Mat3<S> {
+    pub fn look_at(dir: &Vec3<S>, up: &Vec3<S>) -> Mat3<S> {
+        let dir  = dir.normalize();
+        let side = dir.cross(&up.normalize());
+        let up   = side.cross(&dir).normalize();
+
+        Mat3::from_cols(up, side, dir)
+    }
+}
+
 impl<S: Clone + Num> Mat4<S> {
     #[inline]
     pub fn new(c0r0: S, c0r1: S, c0r2: S, c0r3: S,
@@ -403,3 +420,52 @@ for Mat4<S>
         }
     }
 }
+
+impl<S:Clone + Float> ToQuat<S> for Mat3<S> {
+    /// Convert the matrix to a quaternion
+    fn to_quat(&self) -> Quat<S> {
+        // 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>()) {
+                s = (one::<S>() + trace).sqrt();
+                w = half::<S>() * s;
+                s = half::<S>() / 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::<S>() + (*self.cr(0, 0) - *self.cr(1, 1) - *self.cr(2, 2))).sqrt();
+                w = half::<S>() * s;
+                s = half::<S>() / 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::<S>() + (*self.cr(1, 1) - *self.cr(0, 0) - *self.cr(2, 2))).sqrt();
+                w = half::<S>() * s;
+                s = half::<S>() / 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::<S>() + (*self.cr(2, 2) - *self.cr(0, 0) - *self.cr(1, 1))).sqrt();
+                w = half::<S>() * s;
+                s = half::<S>() / 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)
+    }
+}

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<T> { s: T, v: Vec3<T> }
+pub struct Quat<S> { s: S, v: Vec3<S> }
 
-impl<T: Clone + Num> Quat<T> {
+pub trait ToQuat<S: Clone + Float> {
+    fn to_quat(&self) -> Quat<S>;
+}
+
+impl<S: Clone + Float> Quat<S> {
     /// 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<T> {
+    pub fn new(w: S, xi: S, yj: S, zk: S) -> Quat<S> {
         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<T>) -> Quat<T> {
+    pub fn from_sv(s: S, v: Vec3<S>) -> Quat<S> {
         Quat { s: s, v: v }
     }
+
+    #[inline]
+    pub fn look_at(dir: &Vec3<S>, up: &Vec3<S>) -> Quat<S> {
+        Mat3::look_at(dir, up).to_quat()
+    }
+
+    /// The additive identity, ie: `q = 0 + 0i + 0j + 0i`
+    #[inline]
+    pub fn zero() -> Quat<S> {
+        Quat::new(zero(), zero(), zero(), zero())
+    }
+
+    /// The multiplicative identity, ie: `q = 1 + 0i + 0j + 0i`
+    #[inline]
+    pub fn identity() -> Quat<S> {
+        Quat::from_sv(one::<S>(), Vec3::zero())
+    }
+
+    /// The result of multiplying the quaternion a scalar
+    #[inline]
+    pub fn mul_s(&self, value: S) -> Quat<S> {
+        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<S> {
+        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<S>) -> Vec3<S>  {
+        let tmp = self.v.cross(vec).add_v(&vec.mul_s(self.s.clone()));
+        self.v.cross(&tmp).mul_s(two::<S>()).add_v(vec)
+    }
+
+    /// The sum of this quaternion and `other`
+    #[inline]
+    pub fn add_q(&self, other: &Quat<S>) -> Quat<S> {
+        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<S>) -> Quat<S> {
+        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<S>) -> Quat<S> {
+        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>) -> S {
+        self.s * other.s + self.v.dot(&other.v)
+    }
+
+    /// The conjugate of the quaternion
+    #[inline]
+    pub fn conjugate(&self) -> Quat<S> {
+        Quat::from_sv(self.s.clone(), -self.v.clone())
+    }
+
+    /// The multiplicative inverse of the quaternion
+    #[inline]
+    pub fn inverse(&self) -> Quat<S> {
+        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<S> {
+        self.mul_s(one::<S>() / self.magnitude())
+    }
+
+    /// Normalised linear interpolation
+    ///
+    /// # Return value
+    ///
+    /// The intoperlated quaternion
+    pub fn nlerp(&self, other: &Quat<S>, amount: S) -> Quat<S> {
+        self.mul_s(one::<S>() - amount).add_q(&other.mul_s(amount)).normalize()
+    }
+}
+
+impl<S: Clone + Float> Quat<S> {
+    /// 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<S>, amount: S) -> Quat<S> {
+        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::<S>(), &one::<S>());
+
+            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<S: Clone + Float> ToMat3<S> for Quat<S> {
+    /// Convert the quaternion to a 3 x 3 rotation matrix
+    fn to_mat3(&self) -> Mat3<S> {
+        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::<S>() - yy2 - zz2, xy2 + sz2, xz2 - sy2,
+                  xy2 - sz2, one::<S>() - xx2 - zz2, yz2 + sx2,
+                  xz2 + sy2, yz2 - sx2, one::<S>() - xx2 - yy2)
+    }
+}
+
+impl<S: Clone + Float> Neg<Quat<S>> for Quat<S> {
+    #[inline]
+    fn neg(&self) -> Quat<S> {
+        Quat::from_sv(-self.s, -self.v)
+    }
 }

src/cgmath/vector.rs

@@ -30,6 +30,11 @@ pub struct Vec3<S> { x: S, y: S, z: S }
 #[deriving(Eq, Clone, Zero)]
 pub struct Vec4<S> { x: S, y: S, z: S, w: S }
 
+// Conversion traits
+pub trait ToVec2<S: Clone + Num> { fn to_vec2(&self) -> Vec2<S>; }
+pub trait ToVec3<S: Clone + Num> { fn to_vec3(&self) -> Vec3<S>; }
+pub trait ToVec4<S: Clone + Num> { fn to_vec4(&self) -> Vec4<S>; }
+
 // Utility macro for generating associated functions for the vectors
 macro_rules! vec(
     (impl $Self:ident <$S:ident> { $($field:ident),+ }) => (