2012-12-05 08:09:53 +00:00
|
|
|
/**
|
|
|
|
* > Every morning in the early part of October 1843, on my coming down to
|
|
|
|
* breakfast, your brother William Edward and yourself used to ask me: "Well,
|
|
|
|
* Papa, can you multiply triples?" Whereto I was always obliged to reply,
|
|
|
|
* with a sad shake of the head, "No, I can only add and subtract them."
|
|
|
|
*
|
|
|
|
* Sir William Hamilton
|
|
|
|
*/
|
|
|
|
|
2012-11-15 02:23:39 +00:00
|
|
|
use core::cast::transmute;
|
2012-11-22 01:09:04 +00:00
|
|
|
use core::cmp::{Eq, Ord};
|
2012-11-22 00:38:39 +00:00
|
|
|
use core::ptr::to_unsafe_ptr;
|
2012-11-25 12:05:47 +00:00
|
|
|
use core::sys::size_of;
|
2012-11-15 02:23:39 +00:00
|
|
|
use core::vec::raw::buf_as_slice;
|
|
|
|
|
|
|
|
use std::cmp::FuzzyEq;
|
|
|
|
|
2012-12-08 02:59:10 +00:00
|
|
|
use angle::Angle;
|
2012-12-01 04:55:45 +00:00
|
|
|
use dim::{Dimensional, ToPtr};
|
2012-11-26 06:48:46 +00:00
|
|
|
use funs::common::*;
|
|
|
|
use funs::exponential::*;
|
|
|
|
use funs::triganomic::*;
|
2012-11-15 02:23:39 +00:00
|
|
|
use mat::{Mat3, Mat4};
|
2012-12-03 05:39:32 +00:00
|
|
|
use num::kinds::{Float, Number};
|
2012-11-15 02:23:39 +00:00
|
|
|
use vec::Vec3;
|
|
|
|
|
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
|
|
|
* The base quaternion trait
|
2012-12-05 01:51:18 +00:00
|
|
|
*
|
|
|
|
* # Type parameters
|
|
|
|
*
|
|
|
|
* * `T` - The type of the components. Should be a floating point type.
|
2012-12-05 08:09:53 +00:00
|
|
|
* * `V3` - The 3-dimensional vector type that will containin the imaginary
|
|
|
|
* components of the quaternion.
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-12-05 01:51:18 +00:00
|
|
|
pub trait Quaternion<T,V3>: Dimensional<T>, ToPtr<T>, Eq, Neg<self> {
|
2012-12-08 02:59:10 +00:00
|
|
|
static pure fn from_axis_angle<A:Angle<T>>(axis: &Vec3<T>, theta: A) -> self;
|
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The multiplicative identity, ie: `q = 1 + 0i + 0j + 0i`
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
|
|
|
static pure fn identity() -> self;
|
2012-11-20 06:57:32 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The additive identity, ie: `q = 0 + 0i + 0j + 0i`
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
|
|
|
static pure fn zero() -> self;
|
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of multiplying the quaternion a scalar
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn mul_t(&self, value: T) -> self;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of dividing the quaternion a scalar
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn div_t(&self, value: T) -> self;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of multiplying the quaternion by a vector
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-12-05 01:51:18 +00:00
|
|
|
pure fn mul_v(&self, vec: &V3) -> V3;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The sum of this quaternion and `other`
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn add_q(&self, other: &self) -> self;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The sum of this quaternion and `other`
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn sub_q(&self, other: &self) -> self;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The the result of multipliplying the quaternion by `other`
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn mul_q(&self, other: &self) -> self;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
2012-12-03 22:31:26 +00:00
|
|
|
* The dot product of the quaternion and `other`
|
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn dot(&self, other: &self) -> T;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-05 01:38:30 +00:00
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The conjugate of the quaternion
|
2012-12-05 01:38:30 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn conjugate(&self) -> self;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The multiplicative inverse of the quaternion
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn inverse(&self) -> self;
|
2012-12-05 01:38:30 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The squared magnitude of the quaternion. This is useful for
|
2012-12-05 01:38:30 +00:00
|
|
|
* magnitude comparisons where the exact magnitude does not need to be
|
|
|
|
* calculated.
|
|
|
|
*/
|
|
|
|
pure fn magnitude2(&self) -> T;
|
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The magnitude of the quaternion
|
2012-12-05 01:38:30 +00:00
|
|
|
*
|
|
|
|
* # Performance notes
|
|
|
|
*
|
2012-12-05 08:09:53 +00:00
|
|
|
* For instances where the exact magnitude of the quaternion does not need
|
|
|
|
* to be known, for example for quaternion-quaternion magnitude comparisons,
|
2012-12-05 01:38:30 +00:00
|
|
|
* it is advisable to use the `magnitude2` method instead.
|
|
|
|
*/
|
|
|
|
pure fn magnitude(&self) -> T;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The normalized quaternion
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn normalize(&self) -> self;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
|
|
|
* Normalised linear interpolation
|
2012-12-05 08:09:53 +00:00
|
|
|
*
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The intoperlated quaternion
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn nlerp(&self, other: &self, amount: T) -> self;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
|
|
|
* Perform a spherical linear interpolation between the quaternion and
|
2012-12-05 01:38:30 +00:00
|
|
|
* `other`.
|
|
|
|
*
|
2012-12-05 08:09:53 +00:00
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The intoperlated quaternion
|
|
|
|
*
|
2012-12-05 01:38:30 +00:00
|
|
|
* # Performance notes
|
|
|
|
*
|
|
|
|
* This is more accurate than `nlerp` but is also more
|
2012-12-03 22:31:26 +00:00
|
|
|
* computationally intensive.
|
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn slerp(&self, other: &self, amount: T) -> self;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
|
|
|
* Convert the quaternion to a 3 x 3 rotation matrix
|
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn to_mat3(&self) -> Mat3<T>;
|
2012-12-03 22:31:26 +00:00
|
|
|
|
|
|
|
/**
|
|
|
|
* Convert the quaternion to a 4 x 4 transformation matrix
|
|
|
|
*/
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn to_mat4(&self) -> Mat4<T>;
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
pub trait ToQuat<T> {
|
2012-12-05 01:38:30 +00:00
|
|
|
/**
|
|
|
|
* Convert `self` to a quaternion
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
pure fn to_Quat() -> Quat<T>;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
2012-12-05 01:38:30 +00:00
|
|
|
/**
|
|
|
|
* A quaternion in scalar/vector form
|
|
|
|
*
|
2012-12-05 01:51:18 +00:00
|
|
|
* # Type parameters
|
|
|
|
*
|
|
|
|
* * `T` - The type of the components. Should be a floating point type.
|
|
|
|
*
|
2012-12-05 01:38:30 +00:00
|
|
|
* # Fields
|
|
|
|
*
|
|
|
|
* * `s` - the scalar component
|
|
|
|
* * `v` - a vector containing the three imaginary components
|
|
|
|
*/
|
2012-11-21 04:01:21 +00:00
|
|
|
pub struct Quat<T> { s: T, v: Vec3<T> }
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-11-21 04:01:21 +00:00
|
|
|
pub impl<T> Quat<T> {
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
|
|
|
* Construct the quaternion from one scalar component and three
|
|
|
|
* imaginary components
|
2012-12-05 01:38:30 +00:00
|
|
|
*
|
|
|
|
* # Arguments
|
|
|
|
*
|
|
|
|
* * `w` - the scalar component
|
|
|
|
* * `xi` - the fist imaginary component
|
|
|
|
* * `yj` - the second imaginary component
|
|
|
|
* * `zk` - the third imaginary component
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2012-12-05 01:38:30 +00:00
|
|
|
static pure fn new(w: T, xi: T, yj: T, zk: T) -> Quat<T> {
|
|
|
|
Quat::from_sv(move w, move Vec3::new(move xi, move yj, move zk))
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
2012-12-03 22:31:26 +00:00
|
|
|
/**
|
|
|
|
* Construct the quaternion from a scalar and a vector
|
2012-12-05 01:38:30 +00:00
|
|
|
*
|
|
|
|
* # Arguments
|
|
|
|
*
|
|
|
|
* * `s` - the scalar component
|
|
|
|
* * `v` - a vector containing the three imaginary components
|
2012-12-03 22:31:26 +00:00
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2012-11-21 04:01:21 +00:00
|
|
|
static pure fn from_sv(s: T, v: Vec3<T>) -> Quat<T> {
|
|
|
|
Quat { s: move s, v: move v }
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-12-01 04:55:45 +00:00
|
|
|
pub impl<T> Quat<T>: Dimensional<T> {
|
2012-12-01 04:19:21 +00:00
|
|
|
#[inline(always)]
|
|
|
|
static pure fn dim() -> uint { 4 }
|
|
|
|
|
|
|
|
#[inline(always)]
|
|
|
|
static pure fn size_of() -> uint { size_of::<Quat<T>>() }
|
|
|
|
}
|
|
|
|
|
|
|
|
pub impl<T:Copy> Quat<T>: Index<uint, T> {
|
|
|
|
#[inline(always)]
|
|
|
|
pure fn index(i: uint) -> T {
|
2012-12-04 07:50:15 +00:00
|
|
|
unsafe { do buf_as_slice(self.to_ptr(), 4) |slice| { slice[i] } }
|
2012-12-01 04:19:21 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-12-01 04:55:45 +00:00
|
|
|
pub impl<T:Copy> Quat<T>: ToPtr<T> {
|
|
|
|
#[inline(always)]
|
|
|
|
pure fn to_ptr(&self) -> *T {
|
2012-12-04 07:50:15 +00:00
|
|
|
unsafe {
|
|
|
|
transmute::<*Quat<T>, *T>(
|
|
|
|
to_unsafe_ptr(&*self)
|
|
|
|
)
|
|
|
|
}
|
2012-12-01 04:55:45 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-12-05 01:51:18 +00:00
|
|
|
pub impl<T:Copy Float Exp Extent InvTrig> Quat<T>: Quaternion<T, Vec3<T>> {
|
2012-12-08 02:59:10 +00:00
|
|
|
#[inline(always)]
|
|
|
|
static pure fn from_axis_angle<A:Angle<T>>(axis: &Vec3<T>, theta: A) -> Quat<T> {
|
|
|
|
let half = theta.to_radians() / Number::from(2);
|
|
|
|
Quat::from_sv(cos(&half), axis.mul_t(sin(&half)))
|
|
|
|
}
|
|
|
|
|
2012-11-20 06:57:32 +00:00
|
|
|
#[inline(always)]
|
|
|
|
static pure fn identity() -> Quat<T> {
|
2012-12-03 06:19:53 +00:00
|
|
|
Quat::new(Number::from(1),
|
|
|
|
Number::from(0),
|
|
|
|
Number::from(0),
|
|
|
|
Number::from(0))
|
2012-11-20 06:57:32 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
|
|
|
static pure fn zero() -> Quat<T> {
|
2012-12-03 06:19:53 +00:00
|
|
|
Quat::new(Number::from(0),
|
|
|
|
Number::from(0),
|
|
|
|
Number::from(0),
|
|
|
|
Number::from(0))
|
2012-11-20 06:57:32 +00:00
|
|
|
}
|
|
|
|
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn mul_t(&self, value: T) -> Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
Quat::new(self[0] * value,
|
|
|
|
self[1] * value,
|
|
|
|
self[2] * value,
|
|
|
|
self[3] * value)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn div_t(&self, value: T) -> Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
Quat::new(self[0] / value,
|
|
|
|
self[1] / value,
|
|
|
|
self[2] / value,
|
|
|
|
self[3] / value)
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn mul_v(&self, vec: &Vec3<T>) -> Vec3<T> {
|
2012-11-21 04:01:21 +00:00
|
|
|
let tmp = self.v.cross(vec).add_v(&vec.mul_t(self.s));
|
2012-12-03 06:19:53 +00:00
|
|
|
self.v.cross(&tmp).mul_t(Number::from(2)).add_v(vec)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn add_q(&self, other: &Quat<T>) -> Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
Quat::new(self[0] + other[0],
|
|
|
|
self[1] + other[1],
|
|
|
|
self[2] + other[2],
|
|
|
|
self[3] + other[3])
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn sub_q(&self, other: &Quat<T>) -> Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
Quat::new(self[0] - other[0],
|
|
|
|
self[1] - other[1],
|
|
|
|
self[2] - other[2],
|
|
|
|
self[3] - other[3])
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn mul_q(&self, other: &Quat<T>) -> Quat<T> {
|
2012-11-21 04:01:21 +00:00
|
|
|
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)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn dot(&self, other: &Quat<T>) -> T {
|
2012-11-21 04:01:21 +00:00
|
|
|
self.s * other.s + self.v.dot(&other.v)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn conjugate(&self) -> Quat<T> {
|
2012-11-21 04:01:21 +00:00
|
|
|
Quat::from_sv(self.s, -self.v)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn inverse(&self) -> Quat<T> {
|
2012-12-05 01:38:30 +00:00
|
|
|
self.conjugate().div_t(self.magnitude2())
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-12-05 01:38:30 +00:00
|
|
|
pure fn magnitude2(&self) -> T {
|
2012-11-21 04:01:21 +00:00
|
|
|
self.s * self.s + self.v.length2()
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-12-05 01:38:30 +00:00
|
|
|
pure fn magnitude(&self) -> T {
|
|
|
|
self.magnitude2().sqrt()
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn normalize(&self) -> Quat<T> {
|
2012-12-03 06:19:53 +00:00
|
|
|
let mut n: T = Number::from(1);
|
2012-12-05 01:38:30 +00:00
|
|
|
n /= self.magnitude();
|
2012-11-15 02:23:39 +00:00
|
|
|
return self.mul_t(n);
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn nlerp(&self, other: &Quat<T>, amount: T) -> Quat<T> {
|
2012-12-03 06:19:53 +00:00
|
|
|
let _1: T = Number::from(1);
|
2012-11-15 02:23:39 +00:00
|
|
|
self.mul_t(_1 - amount).add_q(&other.mul_t(amount)).normalize()
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* Spherical Linear Intoperlation
|
|
|
|
*
|
|
|
|
* Both quaternions should be normalized first, or else strange things will
|
|
|
|
* will happen...
|
|
|
|
*
|
2012-12-05 01:38:30 +00:00
|
|
|
* # Performance notes
|
2012-11-15 02:23:39 +00:00
|
|
|
*
|
2012-12-05 01:38:30 +00:00
|
|
|
* 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)
|
2012-11-15 02:23:39 +00:00
|
|
|
*/
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn slerp(&self, other: &Quat<T>, amount: T) -> Quat<T> {
|
2012-12-03 22:24:03 +00:00
|
|
|
let dot = self.dot(other);
|
2012-11-15 02:23:39 +00:00
|
|
|
|
|
|
|
// if quaternions are close together use `nlerp`
|
2012-12-03 06:19:53 +00:00
|
|
|
let dot_threshold = Number::from(0.9995);
|
2012-11-15 02:23:39 +00:00
|
|
|
if dot > dot_threshold { return self.nlerp(other, amount) }
|
|
|
|
|
2012-12-03 06:19:53 +00:00
|
|
|
let robust_dot = dot.clamp(&-Number::from(1), &Number::from(1)); // stay within the domain of acos()
|
2012-11-15 02:23:39 +00:00
|
|
|
let theta_0 = acos(&robust_dot); // the angle between the quaternions
|
|
|
|
let theta = theta_0 * amount; // the fraction of theta specified by `amount`
|
|
|
|
|
|
|
|
let q = other.sub_q(&self.mul_t(robust_dot))
|
|
|
|
.normalize();
|
|
|
|
|
|
|
|
self.mul_t(cos(&theta)).add_q(&q.mul_t(sin(&theta)))
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn to_mat3(&self) -> Mat3<T> {
|
2012-11-21 04:01:21 +00:00
|
|
|
let x2 = self.v.x + self.v.x;
|
|
|
|
let y2 = self.v.y + self.v.y;
|
|
|
|
let z2 = self.v.z + self.v.z;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-11-21 04:01:21 +00:00
|
|
|
let xx2 = x2 * self.v.x;
|
|
|
|
let xy2 = x2 * self.v.y;
|
|
|
|
let xz2 = x2 * self.v.z;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-11-21 04:01:21 +00:00
|
|
|
let yy2 = y2 * self.v.y;
|
|
|
|
let yz2 = y2 * self.v.z;
|
|
|
|
let zz2 = z2 * self.v.z;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-11-21 04:01:21 +00:00
|
|
|
let sy2 = y2 * self.s;
|
|
|
|
let sz2 = z2 * self.s;
|
|
|
|
let sx2 = x2 * self.s;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-03 22:24:03 +00:00
|
|
|
let _1: T = Number::from(1);
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2012-12-07 04:16:28 +00:00
|
|
|
Mat3::new(_1 - yy2 - zz2, xy2 + sz2, xz2 - sy2,
|
|
|
|
xy2 - sz2, _1 - xx2 - zz2, yz2 + sx2,
|
|
|
|
xz2 + sy2, yz2 - sx2, _1 - xx2 - yy2)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-30 03:13:20 +00:00
|
|
|
pure fn to_mat4(&self) -> Mat4<T> {
|
2012-11-22 00:36:33 +00:00
|
|
|
self.to_mat3().to_mat4()
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-12-01 04:19:21 +00:00
|
|
|
pub impl<T:Copy Num> Quat<T>: Neg<Quat<T>> {
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2012-12-01 04:19:21 +00:00
|
|
|
pure fn neg(&self) -> Quat<T> {
|
|
|
|
Quat::new(-self[0], -self[1], -self[2], -self[3])
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2012-11-29 11:30:40 +00:00
|
|
|
pub impl<T:Copy Eq> Quat<T>: Eq {
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2012-11-29 03:14:42 +00:00
|
|
|
pure fn eq(&self, other: &Quat<T>) -> bool {
|
2012-11-29 11:30:40 +00:00
|
|
|
self[0] == other[0] &&
|
|
|
|
self[1] == other[1] &&
|
|
|
|
self[2] == other[2] &&
|
|
|
|
self[3] == other[3]
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
2012-11-29 03:14:42 +00:00
|
|
|
pure fn ne(&self, other: &Quat<T>) -> bool {
|
|
|
|
!(self == other)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
pub impl<T:Copy FuzzyEq> Quat<T>: FuzzyEq {
|
|
|
|
#[inline(always)]
|
|
|
|
pure fn fuzzy_eq(other: &Quat<T>) -> bool {
|
|
|
|
self[0].fuzzy_eq(&other[0]) &&
|
|
|
|
self[1].fuzzy_eq(&other[1]) &&
|
|
|
|
self[2].fuzzy_eq(&other[2]) &&
|
|
|
|
self[3].fuzzy_eq(&other[3])
|
|
|
|
}
|
2012-12-05 01:38:30 +00:00
|
|
|
}
|