use super::Mat4;
use super::Vec3;
use std::ops::Mul;
use std::ops::MulAssign;
#[derive(Debug, Copy, Clone, PartialEq)]
pub struct Quat {
pub x: f32,
pub y: f32,
pub z: f32,
pub w: f32,
}
impl Quat {
pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
Self { x, y, z, w }
}
pub fn euler_rotation(x: f32, y: f32, z: f32) -> Self {
let m = Mat4::euler_rotation(x, y, z);
Self::from(m)
}
pub fn axis_rotation(axis: impl Into<Vec3>, angle: f32) -> Self {
let m = Mat4::axis_rotation(axis, angle);
Self::from(m)
}
pub fn look_rotation(dir: impl Into<Vec3>, global_up: impl Into<Vec3>) -> Self {
let m = Mat4::look_rotation(dir, global_up);
Self::from(m)
}
pub fn local_up(self) -> Vec3 {
self.inverse() * Vec3::up()
}
pub fn local_forward(self) -> Vec3 {
self.inverse() * Vec3::forward()
}
pub fn local_right(self) -> Vec3 {
self.inverse() * Vec3::right()
}
pub fn inverse(self) -> Quat {
let mut result = self;
result.w = -result.w;
result
}
}
impl Default for Quat {
fn default() -> Self {
Self::new(0.0, 0.0, 0.0, 1.0)
}
}
impl Mul<Vec3> for Quat {
type Output = Vec3;
fn mul(self, rhs: Vec3) -> Vec3 {
let u = Vec3::new(self.x, self.y, self.z);
let s = self.w;
u * 2.0 * u.dot(rhs) + rhs * (s * s - u.dot(u)) + u.cross(rhs) * 2.0 * s
}
}
impl Mul<Self> for Quat {
type Output = Self;
fn mul(self, rhs: Self) -> Self {
let w = self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z;
let x = self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y;
let y = self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x;
let z = self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w;
Self::new(x, y, z, w)
}
}
impl MulAssign<Self> for Quat {
fn mul_assign(&mut self, rhs: Self) {
*self = *self * rhs;
}
}
impl From<[f32; 4]> for Quat {
fn from(a: [f32; 4]) -> Self {
Self::new(a[0], a[1], a[2], a[3])
}
}
impl From<Mat4> for Quat {
fn from(m: Mat4) -> Self {
let mut q = Self::default();
let trace = m.x.x + m.y.y + m.z.z;
if trace > 0.0 {
let s = 0.5 / (trace + 1.0).sqrt();
q.w = 0.25 / s;
q.x = (m.y.z - m.z.y) * s;
q.y = (m.z.x - m.x.z) * s;
q.z = (m.x.y - m.y.x) * s;
} else if m.x.x > m.y.y && m.x.x > m.z.z {
let s = 2.0 * (1.0 + m.x.x - m.y.y - m.z.z).sqrt();
q.w = (m.y.z - m.z.y) / s;
q.x = 0.25 * s;
q.y = (m.y.x + m.x.y) / s;
q.z = (m.z.x + m.x.z) / s;
} else if m.y.y > m.z.z {
let s = 2.0 * (1.0 + m.y.y - m.x.x - m.z.z).sqrt();
q.w = (m.z.x - m.x.z) / s;
q.x = (m.y.x + m.x.y) / s;
q.y = 0.25 * s;
q.z = (m.z.y + m.y.z) / s;
} else {
let s = 2.0 * (1.0 + m.z.z - m.x.x - m.y.y).sqrt();
q.w = (m.x.y - m.y.x) / s;
q.x = (m.z.x + m.x.z) / s;
q.y = (m.z.y + m.y.z) / s;
q.z = 0.25 * s;
}
q
}
}
#[cfg(test)]
mod test {
use super::Mat4;
use super::Quat;
use super::Vec3;
#[test]
fn default() {
assert_eq!(Quat::default(), Quat::new(0.0, 0.0, 0.0, 1.0));
}
#[test]
fn axis_rotation() {
let q = Quat::axis_rotation([1.0, 0.0, 0.0], 180.0);
let v = Vec3::new(1.0, 1.0, 1.0);
let r = q * v;
assert_eq_delta!(r.x, 1.0);
assert_eq_delta!(r.y, -1.0);
assert_eq_delta!(r.z, -1.0);
}
#[test]
fn look_rotation_x() {
let q = Quat::look_rotation([1.0, 0.0, 0.0], Vec3::up());
let r = q * Vec3::forward();
assert_eq_delta!(r.x, -1.0);
assert_eq_delta!(r.y, 0.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn look_rotation_y() {
let q = Quat::look_rotation([0.0, 1.0, 0.0], Vec3::forward());
let r = q * Vec3::forward();
assert_eq_delta!(r.x, 0.0);
assert_eq_delta!(r.y, 1.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn look_rotation_z() {
let q = Quat::look_rotation([0.0, 0.0, -1.0], Vec3::up());
let r = q * Vec3::forward();
assert_eq_delta!(r.x, 0.0);
assert_eq_delta!(r.y, 0.0);
assert_eq_delta!(r.z, -1.0);
}
#[test]
fn euler_rotation_x() {
let q = Quat::euler_rotation(90.0, 0.0, 0.0);
let v = Vec3::new(0.0, 0.0, 1.0);
let r = q * v;
assert_eq_delta!(r.x, 0.0);
assert_eq_delta!(r.y, -1.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn euler_rotation_y() {
let q = Quat::euler_rotation(0.0, 90.0, 0.0);
let v = Vec3::new(0.0, 0.0, 1.0);
let r = q * v;
assert_eq_delta!(r.x, 1.0);
assert_eq_delta!(r.y, 0.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn euler_rotation_z() {
let q = Quat::euler_rotation(0.0, 0.0, 90.0);
let v = Vec3::new(1.0, 0.0, 0.0);
let r = q * v;
assert_eq_delta!(r.x, 0.0);
assert_eq_delta!(r.y, 1.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn quat_to_mat_to_quat() {
let q1 = Quat::euler_rotation(0.0, 45.0, 45.0);
let q2 = Quat::from(Mat4::from(q1));
assert_eq_delta!(q1.x, q2.x);
assert_eq_delta!(q1.y, q2.y);
assert_eq_delta!(q1.z, q2.z);
assert_eq_delta!(q1.w, q2.w);
}
#[test]
fn mul_vector() {
let q = Quat::euler_rotation(0.0, 0.0, 90.0);
let v = Vec3::right();
let r = q * v;
assert_eq_delta!(r.x, 0.0);
assert_eq_delta!(r.y, 1.0);
assert_eq_delta!(r.z, 0.0);
}
#[test]
fn mul_self() {
let q1 = Quat::euler_rotation(0.0, 0.0, 180.0);
let q2 = Quat::euler_rotation(0.0, 0.0, 90.0) * Quat::euler_rotation(0.0, 0.0, 90.0);
assert_eq_delta!(q1.x, q2.x);
assert_eq_delta!(q1.y, q2.y);
assert_eq_delta!(q1.z, q2.z);
assert_eq_delta!(q1.w, q2.w);
}
}