From b7b1f6c9cbe42bb2e3d66b706c3a3bc50ed1539b Mon Sep 17 00:00:00 2001 From: Brendan Zabarauskas Date: Wed, 30 Sep 2015 18:05:20 +1000 Subject: [PATCH] Implement binary operators for quaternions --- src/quaternion.rs | 157 +++++++++++++++++++++------------------------- 1 file changed, 70 insertions(+), 87 deletions(-) diff --git a/src/quaternion.rs b/src/quaternion.rs index 4d3f2f2..ed8fd8a 100644 --- a/src/quaternion.rs +++ b/src/quaternion.rs @@ -68,79 +68,6 @@ impl Quaternion { Quaternion::from_sv(S::one(), Vector3::zero()) } - /// The result of multiplying the quaternion a scalar - #[inline] - pub fn mul_s(&self, value: S) -> Quaternion { - Quaternion::from_sv(self.s * value, self.v.mul_s(value)) - } - - /// The result of dividing the quaternion a scalar - #[inline] - pub fn div_s(&self, value: S) -> Quaternion { - Quaternion::from_sv(self.s / value, self.v.div_s(value)) - } - - /// The result of multiplying the quaternion by a vector - #[inline] - pub fn mul_v(&self, vec: &Vector3) -> Vector3 { - let tmp = self.v.cross(vec).add_v(&vec.mul_s(self.s.clone())); - self.v.cross(&tmp).mul_s(cast(2i8).unwrap()).add_v(vec) - } - - /// The sum of this quaternion and `other` - #[inline] - pub fn add_q(&self, other: &Quaternion) -> Quaternion { - Quaternion::from_sv(self.s + other.s, &self.v + &other.v) - } - - /// The difference between this quaternion and `other` - #[inline] - pub fn sub_q(&self, other: &Quaternion) -> Quaternion { - Quaternion::from_sv(self.s - other.s, &self.v - &other.v) - } - - /// The result of multipliplying the quaternion by `other` - pub fn mul_q(&self, other: &Quaternion) -> Quaternion { - Quaternion::new(self.s * other.s - self.v.x * other.v.x - self.v.y * other.v.y - self.v.z * other.v.z, - 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) - } - - /// Multiply this quaternion by a scalar, in-place. - #[inline] - pub fn mul_self_s(&mut self, s: S) { - self.s = self.s * s; - self.v.mul_self_s(s); - } - - /// Divide this quaternion by a scalar, in-place. - #[inline] - pub fn div_self_s(&mut self, s: S) { - self.s = self.s / s; - self.v.div_self_s(s); - } - - /// Add this quaternion by another, in-place. - #[inline] - pub fn add_self_q(&mut self, q: &Quaternion) { - self.s = self.s + q.s; - self.v.add_self_v(&q.v); - } - - /// Subtract another quaternion from this one, in-place. - #[inline] - pub fn sub_self_q(&mut self, q: &Quaternion) { - self.s = self.s - q.s; - self.v.sub_self_v(&q.v); - } - - /// Multiply this quaternion by another, in-place. - #[inline] - pub fn mul_self_q(&mut self, q: &Quaternion) { - *self = self.mul_q(q); - } - /// The dot product of the quaternion and `q`. #[inline] pub fn dot(&self, q: &Quaternion) -> S { @@ -176,12 +103,70 @@ impl Quaternion { /// Normalize this quaternion, returning the new quaternion. #[inline] pub fn normalize(&self) -> Quaternion { - self.mul_s(S::one() / self.magnitude()) + self * (S::one() / self.magnitude()) } /// Do a normalized linear interpolation with `other`, by `amount`. pub fn nlerp(&self, other: &Quaternion, amount: S) -> Quaternion { - self.mul_s(S::one() - amount).add_q(&other.mul_s(amount)).normalize() + (&(self * (S::one() - amount)) + &(other * amount)).normalize() + } +} + +impl<'a, S: BaseFloat> Mul for &'a Quaternion { + type Output = Quaternion; + + #[inline] + fn mul(self, value: S) -> Quaternion { + Quaternion::from_sv(self.s * value, &self.v * value) + } +} + +impl<'a, S: BaseFloat> Div for &'a Quaternion { + type Output = Quaternion; + + #[inline] + fn div(self, value: S) -> Quaternion { + Quaternion::from_sv(self.s / value, &self.v / value) + } +} + +impl<'a, 'b, S: BaseFloat> Mul<&'b Vector3> for &'a Quaternion { + type Output = Vector3; + + #[inline] + fn mul(self, vec: &'b Vector3) -> Vector3 { + let tmp = self.v.cross(vec).add_v(&vec.mul_s(self.s.clone())); + self.v.cross(&tmp).mul_s(cast(2i8).unwrap()).add_v(vec) + } +} + +impl<'a, 'b, S: BaseFloat> Add<&'b Quaternion> for &'a Quaternion { + type Output = Quaternion; + + #[inline] + fn add(self, other: &'b Quaternion) -> Quaternion { + Quaternion::from_sv(self.s + other.s, &self.v + &other.v) + } +} + +impl<'a, 'b, S: BaseFloat> Sub<&'b Quaternion> for &'a Quaternion { + type Output = Quaternion; + + #[inline] + fn sub(self, other: &'b Quaternion) -> Quaternion { + Quaternion::from_sv(self.s - other.s, &self.v - &other.v) + } +} + +impl<'a, 'b, S: BaseFloat> Mul<&'b Quaternion> for &'a Quaternion { + type Output = Quaternion; + + #[inline] + fn mul(self, other: &'b Quaternion) -> Quaternion { + Quaternion::new(self.s * other.s - self.v.x * other.v.x - self.v.y * other.v.y - self.v.z * other.v.z, + 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) } } @@ -233,9 +218,7 @@ impl Quaternion { let scale1 = sin(theta.mul_s(S::one() - amount)); let scale2 = sin(theta.mul_s(amount)); - self.mul_s(scale1) - .add_q(&other.mul_s(scale2)) - .mul_s(sin(theta).recip()) + &(&(self * scale1) + &(other * scale2)) * sin(theta).recip() } } @@ -251,10 +234,10 @@ impl Quaternion { let one: S = cast(1f64).unwrap(); let (qw, qx, qy, qz) = (self.s, self.v.x, self.v.y, self.v.z); - let (sqw, sqx, sqy, sqz) = (qw*qw, qx*qx, qy*qy, qz*qz); + let (sqw, sqx, sqy, sqz) = (qw * qw, qx * qx, qy * qy, qz * qz); let unit = sqx + sqy + sqz + sqw; - let test = qx*qy + qz*qw; + let test = qx * qy + qz * qw; if test > sig * unit { ( @@ -271,9 +254,9 @@ impl Quaternion { ) } else { ( - rad((two * (qy*qw - qx*qz)).atan2(one - two*(sqy + sqz))), - rad((two * (qx*qy + qz*qw)).asin()), - rad((two * (qx*qw - qy*qz)).atan2(one - two*(sqx + sqz))), + rad((two * (qy * qw - qx * qz)).atan2(one - two * (sqy + sqz))), + rad((two * (qx * qy + qz * qw)).asin()), + rad((two * (qx * qw - qy * qz)).atan2(one - two * (sqx + sqz))), ) } } @@ -373,16 +356,16 @@ impl Rotation, Point3> for Quaternion) -> Vector3 { self.mul_v(vec) } + fn rotate_vector(&self, vec: &Vector3) -> Vector3 { self * vec } #[inline] - fn concat(&self, other: &Quaternion) -> Quaternion { self.mul_q(other) } + fn concat(&self, other: &Quaternion) -> Quaternion { self * other } #[inline] - fn concat_self(&mut self, other: &Quaternion) { self.mul_self_q(other); } + fn concat_self(&mut self, other: &Quaternion) { *self = &*self * other; } #[inline] - fn invert(&self) -> Quaternion { self.conjugate().div_s(self.magnitude2()) } + fn invert(&self) -> Quaternion { &self.conjugate() / self.magnitude2() } #[inline] fn invert_self(&mut self) { *self = self.invert() }