2013-05-31 22:01:01 +00:00
|
|
|
// Copyright 2013 The Lmath Developers. For a full listing of the authors,
|
|
|
|
// refer to the AUTHORS 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.
|
2012-12-05 08:09:53 +00:00
|
|
|
|
2013-05-23 21:05:25 +00:00
|
|
|
use std::cast::transmute;
|
|
|
|
use std::cmp::ApproxEq;
|
|
|
|
use std::num::{Zero, One};
|
2012-11-15 02:23:39 +00:00
|
|
|
|
2013-04-02 05:12:13 +00:00
|
|
|
use mat::{Mat3, BaseMat3};
|
|
|
|
use vec::{Vec3, BaseVec3, AffineVec, NumVec, NumVec3};
|
2013-05-06 03:52:22 +00:00
|
|
|
|
|
|
|
use num::NumAssign;
|
2012-11-15 02:23:39 +00:00
|
|
|
|
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
|
|
|
|
*/
|
2013-03-28 10:56:38 +00:00
|
|
|
#[deriving(Eq)]
|
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
|
|
|
|
2013-05-07 15:00:06 +00:00
|
|
|
pub impl<T:Copy + Float + NumAssign> 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)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn new(w: T, xi: T, yj: T, zk: T) -> Quat<T> {
|
2013-04-02 05:12:13 +00:00
|
|
|
Quat::from_sv(w, BaseVec3::new(xi, yj, zk))
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +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)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_sv(s: T, v: Vec3<T>) -> Quat<T> {
|
2012-12-21 04:08:43 +00:00
|
|
|
Quat { s: s, v: v }
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-14 06:04:46 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The multiplicative identity, ie: `q = 1 + 0i + 0j + 0i`
|
|
|
|
*/
|
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn identity() -> Quat<T> {
|
2013-05-23 21:05:25 +00:00
|
|
|
Quat::new(One::one(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero())
|
2012-12-14 06:04:46 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-14 06:04:46 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The additive identity, ie: `q = 0 + 0i + 0j + 0i`
|
|
|
|
*/
|
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn zero() -> Quat<T> {
|
2013-05-23 21:05:25 +00:00
|
|
|
Quat::new(Zero::zero(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero())
|
2012-12-14 06:04:46 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_angle_x(radians: T) -> Quat<T> {
|
2013-04-02 00:25:05 +00:00
|
|
|
let _2 = num::cast(2);
|
2013-05-23 21:05:25 +00:00
|
|
|
Quat::new((radians / _2).cos(),
|
|
|
|
radians.sin(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero())
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_angle_y(radians: T) -> Quat<T> {
|
2013-04-02 00:25:05 +00:00
|
|
|
let _2 = num::cast(2);
|
2013-05-23 21:05:25 +00:00
|
|
|
Quat::new((radians / _2).cos(),
|
|
|
|
Zero::zero(),
|
|
|
|
radians.sin(),
|
|
|
|
Zero::zero())
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_angle_z(radians: T) -> Quat<T> {
|
2013-04-02 00:25:05 +00:00
|
|
|
let _2 = num::cast(2);
|
2013-05-23 21:05:25 +00:00
|
|
|
Quat::new((radians / _2).cos(),
|
|
|
|
Zero::zero(),
|
|
|
|
Zero::zero(),
|
|
|
|
radians.sin())
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_angle_xyz(radians_x: T, radians_y: T, radians_z: T) -> Quat<T> {
|
2013-01-29 09:26:48 +00:00
|
|
|
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Conversion
|
2013-04-02 00:25:05 +00:00
|
|
|
let _2 = num::cast(2);
|
2013-01-29 09:26:48 +00:00
|
|
|
let xdiv2 = radians_x / _2;
|
|
|
|
let ydiv2 = radians_y / _2;
|
|
|
|
let zdiv2 = radians_z / _2;
|
2013-05-06 03:52:22 +00:00
|
|
|
Quat::new(zdiv2.cos() * xdiv2.cos() * ydiv2.cos() + zdiv2.sin() * xdiv2.sin() * ydiv2.sin(),
|
|
|
|
zdiv2.sin() * xdiv2.cos() * ydiv2.cos() - zdiv2.cos() * xdiv2.sin() * ydiv2.sin(),
|
|
|
|
zdiv2.cos() * xdiv2.sin() * ydiv2.cos() + zdiv2.sin() * xdiv2.cos() * ydiv2.sin(),
|
|
|
|
zdiv2.cos() * xdiv2.cos() * ydiv2.sin() - zdiv2.sin() * xdiv2.sin() * ydiv2.cos())
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_angle_axis(radians: T, axis: &Vec3<T>) -> Quat<T> {
|
2013-04-02 00:25:05 +00:00
|
|
|
let half = radians / num::cast(2);
|
2013-05-06 03:52:22 +00:00
|
|
|
Quat::from_sv(half.cos(), axis.mul_t(half.sin()))
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn from_axes(x: Vec3<T>, y: Vec3<T>, z: Vec3<T>) -> Quat<T> {
|
2013-04-02 05:12:13 +00:00
|
|
|
let m: Mat3<T> = BaseMat3::from_axes(x, y, z); m.to_quat()
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-05-31 11:05:43 +00:00
|
|
|
#[inline(always)]
|
|
|
|
fn index<'a>(&'a self, i: uint) -> &'a T {
|
|
|
|
unsafe { &'a transmute::<&'a Quat<T>, &'a [T,..4]>(self)[i] }
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
|
|
|
fn index_mut<'a>(&'a mut self, i: uint) -> &'a mut T {
|
|
|
|
unsafe { &'a mut transmute::< &'a mut Quat<T>, &'a mut [T,..4]>(self)[i] }
|
|
|
|
}
|
|
|
|
|
2013-03-28 10:35:51 +00:00
|
|
|
fn get_angle_axis(&self) -> (T, Vec3<T>) {
|
2013-02-06 21:26:33 +00:00
|
|
|
fail!(~"Not yet implemented.")
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-01-29 09:26:48 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:37:25 +00:00
|
|
|
fn look_at(dir: &Vec3<T>, up: &Vec3<T>) -> Quat<T> {
|
2013-04-02 05:12:13 +00:00
|
|
|
let m: Mat3<T> = BaseMat3::look_at(dir, up); m.to_quat()
|
2013-01-29 09:26:48 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of multiplying the quaternion a scalar
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn mul_t(&self, value: T) -> Quat<T> {
|
2013-05-31 11:05:43 +00:00
|
|
|
Quat::new(*self.index(0) * value,
|
|
|
|
*self.index(1) * value,
|
|
|
|
*self.index(2) * value,
|
|
|
|
*self.index(3) * value)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of dividing the quaternion a scalar
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn div_t(&self, value: T) -> Quat<T> {
|
2013-05-31 11:05:43 +00:00
|
|
|
Quat::new(*self.index(0) / value,
|
|
|
|
*self.index(1) / value,
|
|
|
|
*self.index(2) / value,
|
|
|
|
*self.index(3) / value)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The result of multiplying the quaternion by a vector
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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));
|
2013-04-02 00:25:05 +00:00
|
|
|
self.v.cross(&tmp).mul_t(num::cast(2)).add_v(vec)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
2013-03-28 09:45:43 +00:00
|
|
|
* The sum of this quaternion and `other`
|
2012-12-28 03:47:34 +00:00
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn add_q(&self, other: &Quat<T>) -> Quat<T> {
|
2013-05-31 11:05:43 +00:00
|
|
|
Quat::new(*self.index(0) + *other.index(0),
|
|
|
|
*self.index(1) + *other.index(1),
|
|
|
|
*self.index(2) + *other.index(2),
|
|
|
|
*self.index(3) + *other.index(3))
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
2013-03-28 09:45:43 +00:00
|
|
|
* The sum of this quaternion and `other`
|
2012-12-28 03:47:34 +00:00
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn sub_q(&self, other: &Quat<T>) -> Quat<T> {
|
2013-05-31 11:05:43 +00:00
|
|
|
Quat::new(*self.index(0) - *other.index(0),
|
|
|
|
*self.index(1) - *other.index(1),
|
|
|
|
*self.index(2) - *other.index(2),
|
|
|
|
*self.index(3) - *other.index(3))
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The the result of multipliplying the quaternion by `other`
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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,
|
2013-03-28 09:45:43 +00:00
|
|
|
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
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The dot product of the quaternion and `other`
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The conjugate of the quaternion
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The multiplicative inverse of the quaternion
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The squared magnitude of the quaternion. This is useful for
|
|
|
|
* magnitude comparisons where the exact magnitude does not need to be
|
|
|
|
* calculated.
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* 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.
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn magnitude(&self) -> T {
|
2012-12-05 01:38:30 +00:00
|
|
|
self.magnitude2().sqrt()
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The normalized quaternion
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn normalize(&self) -> Quat<T> {
|
2013-05-23 21:05:25 +00:00
|
|
|
self.mul_t(One::one::<T>()/self.magnitude())
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* Normalised linear interpolation
|
|
|
|
*
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The intoperlated quaternion
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn nlerp(&self, other: &Quat<T>, amount: T) -> Quat<T> {
|
2013-05-23 21:05:25 +00:00
|
|
|
self.mul_t(One::one::<T>() - amount).add_q(&other.mul_t(amount)).normalize()
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-11-15 02:23:39 +00:00
|
|
|
/**
|
|
|
|
* Spherical Linear Intoperlation
|
|
|
|
*
|
2012-12-28 03:47:34 +00:00
|
|
|
* Perform a spherical linear interpolation between the quaternion and
|
|
|
|
* `other`. Both quaternions should be normalized first.
|
|
|
|
*
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* The intoperlated quaternion
|
2012-11-15 02:23:39 +00:00
|
|
|
*
|
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)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn slerp(&self, other: &Quat<T>, amount: T) -> Quat<T> {
|
2012-12-03 22:24:03 +00:00
|
|
|
let dot = self.dot(other);
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-04-02 00:25:05 +00:00
|
|
|
let dot_threshold = num::cast(0.9995);
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-08 02:59:37 +00:00
|
|
|
if dot > dot_threshold {
|
2013-05-23 21:05:25 +00:00
|
|
|
return self.nlerp(other, amount); // if quaternions are close together use `nlerp`
|
2012-12-08 02:59:37 +00:00
|
|
|
} else {
|
2013-05-23 21:05:25 +00:00
|
|
|
let robust_dot = dot.clamp(&-One::one::<T>(),
|
|
|
|
&One::one()); // stay within the domain of acos()
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-05-23 21:05:25 +00:00
|
|
|
let theta_0 = robust_dot.acos(); // the angle between the quaternions
|
|
|
|
let theta = theta_0 * amount; // the fraction of theta specified by `amount`
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-08 02:59:37 +00:00
|
|
|
let q = other.sub_q(&self.mul_t(robust_dot))
|
|
|
|
.normalize();
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-05-06 03:52:22 +00:00
|
|
|
return self.mul_t(theta.cos())
|
|
|
|
.add_q(&q.mul_t(theta.sin()));
|
2012-12-08 02:59:37 +00:00
|
|
|
}
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 06:41:21 +00:00
|
|
|
/**
|
|
|
|
* # Return value
|
|
|
|
*
|
|
|
|
* A pointer to the first component of the quaternion
|
|
|
|
*/
|
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn to_ptr(&self) -> *T {
|
2013-03-30 12:36:34 +00:00
|
|
|
unsafe { cast::transmute(self) }
|
2012-12-28 06:41:21 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2012-12-28 03:47:34 +00:00
|
|
|
/**
|
|
|
|
* Convert the quaternion to a 3 x 3 rotation matrix
|
|
|
|
*/
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
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;
|
2013-03-28 09:45:43 +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;
|
2013-03-28 09:45:43 +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;
|
2013-03-28 09:45:43 +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;
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-05-23 21:05:25 +00:00
|
|
|
let _1: T = One::one();
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-04-02 05:12:13 +00:00
|
|
|
BaseMat3::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
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-05-07 15:00:06 +00:00
|
|
|
impl<T:Copy + Float + NumAssign> Neg<Quat<T>> for Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-03-28 10:35:51 +00:00
|
|
|
fn neg(&self) -> Quat<T> {
|
2013-05-31 11:05:43 +00:00
|
|
|
Quat::new(-*self.index(0),
|
|
|
|
-*self.index(1),
|
|
|
|
-*self.index(2),
|
|
|
|
-*self.index(3))
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-05-31 11:05:43 +00:00
|
|
|
impl<T:Copy + Eq + Float + NumAssign> ApproxEq<T> for Quat<T> {
|
2012-11-15 02:23:39 +00:00
|
|
|
#[inline(always)]
|
2013-05-07 15:00:06 +00:00
|
|
|
fn approx_epsilon() -> T {
|
|
|
|
ApproxEq::approx_epsilon::<T,T>()
|
2013-02-09 22:42:06 +00:00
|
|
|
}
|
2013-03-28 09:45:43 +00:00
|
|
|
|
2013-02-09 22:42:06 +00:00
|
|
|
#[inline(always)]
|
2013-05-07 15:00:06 +00:00
|
|
|
fn approx_eq(&self, other: &Quat<T>) -> bool {
|
|
|
|
self.approx_eq_eps(other, &ApproxEq::approx_epsilon::<T,T>())
|
|
|
|
}
|
|
|
|
|
|
|
|
#[inline(always)]
|
|
|
|
fn approx_eq_eps(&self, other: &Quat<T>, epsilon: &T) -> bool {
|
2013-05-31 11:05:43 +00:00
|
|
|
self.index(0).approx_eq_eps(other.index(0), epsilon) &&
|
|
|
|
self.index(1).approx_eq_eps(other.index(1), epsilon) &&
|
|
|
|
self.index(2).approx_eq_eps(other.index(2), epsilon) &&
|
|
|
|
self.index(3).approx_eq_eps(other.index(3), epsilon)
|
2012-11-15 02:23:39 +00:00
|
|
|
}
|
2012-12-08 00:00:50 +00:00
|
|
|
}
|