use cgmath;
use std::{fmt, mem, ops};
use cgmath::{InnerSpace, Rotation3};
use *;
#[derive(Clone, Copy, Debug, PartialEq)]
#[allow(missing_docs)]
#[repr(C)]
pub struct Rotation {
pub x: f32,
pub y: f32,
pub z: f32,
pub s: f32,
}
impl fmt::Display for Rotation {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
write!(f, "({}, {}, {}; {})", self.x, self.y, self.z, self.s)
}
}
impl Rotation {
pub fn identity() -> Self {
Rotation { x: 0.0, y: 0.0, z: 0.0, s: 1.0 }
}
pub fn euler(angles: Vector) -> Self {
let roll = Rotation::new(vector!(0.0, 0.0, 1.0), angles.z);
let pitch = Rotation::new(vector!(1.0, 0.0, 0.0), angles.x);
let yaw = Rotation::new(vector!(0.0, 1.0, 0.0), angles.y);
roll * pitch * yaw
}
pub fn new(axis: Vector, angle: f32) -> Self {
let q = cgmath::Quaternion::from_axis_angle(
cgmath::Vector3::new(axis.x, axis.y, axis.z).normalize(),
cgmath::Rad(angle),
);
Rotation { x: q.v.x, y: q.v.y, z: q.v.z, s: q.s }
}
pub fn rotate(&self, vector: Vector) -> Vector {
use cgmath::Rotation;
let rotation = cgmath::Quaternion::new(self.s, self.x, self.y, self.z);
let point = cgmath::Point3::new(vector.x, vector.y, vector.z);
let result = rotation.rotate_point(point);
vector!(result.x, result.y, result.z)
}
pub fn then(self, rhs: Self) -> Self {
rhs * self
}
}
impl ops::Mul<f32> for Rotation {
type Output = Rotation;
fn mul(mut self, rhs: f32) -> Rotation {
self.s *= rhs;
self
}
}
impl ops::Mul<Rotation> for Rotation {
type Output = Rotation;
fn mul(self, rhs: Rotation) -> Rotation {
let a: &cgmath::Quaternion<f32> = self.as_ref().into();
let b: &cgmath::Quaternion<f32> = rhs.as_ref().into();
let q = a * b;
Rotation { x: q.v.x, y: q.v.y, z: q.v.z, s: q.s }
}
}
impl ops::MulAssign<Rotation> for Rotation {
fn mul_assign(&mut self, rhs: Rotation) {
*self = *self * rhs;
}
}
impl Default for Rotation {
fn default() -> Self {
Self::identity()
}
}
impl AsRef<[f32; 4]> for Rotation {
fn as_ref(&self) -> &[f32; 4] {
unsafe {
mem::transmute(self)
}
}
}
impl From<[f32; 4]> for Rotation {
fn from(q: [f32; 4]) -> Self {
Rotation { x: q[0], y: q[1], z: q[2], s: q[3] }
}
}
impl Into<[f32; 4]> for Rotation {
fn into(self) -> [f32; 4] {
[self.x, self.y, self.z, self.s]
}
}