diff --git a/src/quaternion.rs b/src/quaternion.rs index 9a60652..08fdd46 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -29,7 +29,7 @@ pub trait Quaternion { pure fn mul_t(value: T) -> self; pure fn div_t(value: T) -> self; - // pure fn mul_v(other: &Vec3) -> Vec3; + pure fn mul_v(vec: &Vec3) -> Vec3; pure fn add_q(other: &self) -> self; pure fn sub_q(other: &self) -> self; @@ -62,7 +62,6 @@ pub trait ToQuat { pub struct Quat { w: T, x: T, y: T, z: T } pub mod Quat { - #[inline(always)] pub pure fn new(w: T, x: T, y: T, z: T) -> Quat { Quat { w: move w, x: move x, y: move y, z: move z } @@ -72,6 +71,12 @@ pub mod Quat { pub pure fn from_sv(s: T, v: Vec3) -> Quat { Quat::new(s, v.x, v.y, v.z) } + + #[inline(always)] + pub pure fn from_axis_angle(axis: Vec3, theta: T) -> Quat { + let half = radians(&theta) / cast(2); + from_sv(cos(&half), axis.mul_t(sin(&half))) + } #[inline(always)] pub pure fn zero() -> Quat { @@ -105,6 +110,13 @@ pub impl Quat: Quaternion self[2] / value, self[3] / value) } + + #[inline(always)] + pure fn mul_v(vec: &Vec3) -> Vec3 { + let base = Vec3{ x:self.x, y:self.y, z:self.z }; + let tmp = base.cross(vec).add_v(&vec.mul_t(self.w)); + base.cross(&tmp).mul_t(cast(2)).add_v(vec) + } #[inline(always)] pure fn add_q(other: &Quat) -> Quat {