diff --git a/src/structs/quat.rs b/src/structs/quat.rs new file mode 100644 index 00000000..b08ff0eb --- /dev/null +++ b/src/structs/quat.rs @@ -0,0 +1,75 @@ +//! Quaternion definition. + +/// A quaternion. +/// +/// A single unit quaternion can represent a 3d rotation while a pair of unit quaternions can +/// represent a 4d rotation. +pub struct Quat { + w: N + i: N, + j: N, + k: N, +} + +// FIXME: find a better name +type QuatPair = (Quat, Quat) + +impl Quat { + pub fn new(w: N, x: N, y: N, z: N) -> Quat { + Quat { + w: w, + i: i, + j: j, + k: k + } + } +} + +impl> Add, Quat> for Quat { + fn add(&self, other: &Quat) -> Quat { + Quat::new( + self.w + other.w, + self.i + other.i, + self.j + other.j, + self.k + other.k) + } +} + +impl Mul, Quat> for Quat { + fn mul(&self, other: &Quat) -> Quat { + Quat::new( + self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z, + self.a * other.b - self.b * other.a - self.c * other.d - self.d * other.c, + self.a * other.c - self.b * other.d - self.c * other.a - self.d * other.b, + self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z, + ) + } +} + +impl Rotate> for Quat { + #[inline] + fn rotate(&self, v: &Vec3) -> Vec3 { + *self * *v_quat * self.inv() + } + + #[inline] + fn inv_rotate(&self, v: &Vec3) -> Vec3 { + -self * *v + } +} + +impl Rotate> for (QuatPair, QuatPair) { + #[inline] + fn rotate(&self, v: &Vec4) -> Vec4 { + let (ref l, ref r) = *self; + + *l * *v * *r + } + + #[inline] + fn inv_rotate(&self, v: &Vec4) -> Vec4 { + let (ref l, ref r) = *self; + + (-r) * **v * (-l) + } +}