lightmatrix/
quat.rs

1#[allow(unused_imports)]
2use std::ops::Div;
3#[allow(unused_imports)]
4use std::ops::Index;
5#[allow(unused_imports)]
6use std::ops::IndexMut;
7
8// use crate::matrix::Matrix;
9use crate::matrix::*;
10use crate::scalar::Scalar;
11
12use std::ops::{Add, Mul, Sub};
13
14use num_traits::{real::Real, NumAssign};
15
16#[derive(Clone, Debug)]
17pub struct Quat<T: Real + NumAssign + Default + Clone>(pub Matrix<T, 1, 4>);
18
19macro_rules! impl_quat_partialeq_float {
20    ($type:ty) => {
21        impl PartialEq<Quat<$type>> for Quat<$type> {
22            fn eq(&self, other: &Self) -> bool {
23                (&self.0).eq(&other.0)
24            }
25        }
26    };
27}
28
29impl_quat_partialeq_float!(f32);
30impl_quat_partialeq_float!(f64);
31
32impl<T: Real + NumAssign + Default> Default for Quat<T> {
33    fn default() -> Quat<T> {
34        Quat(Matrix::from([[T::zero(), T::zero(), T::zero(), T::one()]]))
35    }
36}
37
38impl<T: Real + Default + NumAssign> Quat<T> {
39    pub fn x(&self) -> T {
40        self.0[[0, 0]]
41    }
42    pub fn y(&self) -> T {
43        self.0[[0, 1]]
44    }
45    pub fn z(&self) -> T {
46        self.0[[0, 2]]
47    }
48    pub fn w(&self) -> T {
49        self.0[[0, 3]]
50    }
51    pub fn x_mut(&mut self) -> &mut T {
52        &mut self.0[[0, 0]]
53    }
54    pub fn y_mut(&mut self) -> &mut T {
55        &mut self.0[[0, 1]]
56    }
57    pub fn z_mut(&mut self) -> &mut T {
58        &mut self.0[[0, 2]]
59    }
60    pub fn w_mut(&mut self) -> &mut T {
61        &mut self.0[[0, 3]]
62    }
63    pub fn init(x: T, y: T, z: T, w: T) -> Self {
64        Quat(Matrix::from([[x, y, z, w]]))
65    }
66    // #[allow(dead_code)]
67    // pub fn reflection_in_plane(&self, p: Matrix<T, 3, 1>) -> Matrix<T, 3, 1> {
68    //     let quat_p = Quat::init(p[[0, 0]], p[[1, 0]], p[[2, 0]], T::zero());
69    //     let temp = self * &quat_p;
70    //     let temp2 = &temp * self;
71    //     Matrix::from([[temp2.x()], [temp2.y()], [temp2.z()]])
72    // }
73    // #[allow(dead_code)]
74    // pub fn parallel_component_of_plane(&self, p: Matrix<T, 3, 1>) -> Matrix<T, 3, 1> {
75    //     let quat_p = Quat::init(p[[0, 0]], p[[1, 0]], p[[2, 0]], T::zero());
76    //     let temp = self * &quat_p;
77    //     let temp2 = &temp * self;
78    //     let temp3 = &quat_p + &temp2;
79    //     let temp4 = &temp3 * T::from(0.5).unwrap();
80    //     Matrix::from([[temp4.x()], [temp4.y()], [temp4.z()]])
81    // }
82    // #[allow(dead_code)]
83    // pub fn orthogonal_component_of_plane(&self, p: Matrix<T, 3, 1>) -> Matrix<T, 3, 1> {
84    //     let quat_p = Quat::init(p[[0, 0]], p[[1, 0]], p[[2, 0]], T::zero());
85    //     let temp = self * &quat_p;
86    //     let temp2 = &temp * self;
87    //     let temp3 = &quat_p - &temp2;
88    //     let temp4 = &temp3 * T::from(0.5).unwrap();
89    //     Matrix::from([[temp4.x()], [temp4.y()], [temp4.z()]])
90    // }
91    pub fn add(&self, other: &Self) -> Self {
92        Quat::init(
93            self.x() + other.x(),
94            self.y() + other.y(),
95            self.z() + other.z(),
96            self.w() + other.w(),
97        )
98    }
99    pub fn minus(&self, other: &Self) -> Self {
100        Quat::init(
101            self.x() - other.x(),
102            self.y() - other.y(),
103            self.z() - other.z(),
104            self.w() - other.w(),
105        )
106    }
107    pub fn mul(&self, other: &Self) -> Self {
108        Quat::init(
109            self.w() * other.x() + self.x() * other.w() + self.y() * other.z()
110                - self.z() * other.y(),
111            self.w() * other.y() - self.x() * other.z()
112                + self.y() * other.w()
113                + self.z() * other.x(),
114            self.w() * other.z() + self.x() * other.y() - self.y() * other.x()
115                + self.z() * other.w(),
116            self.w() * other.w()
117                - self.x() * other.x()
118                - self.y() * other.y()
119                - self.z() * other.z(),
120        )
121    }
122    pub fn norm_squared(&self) -> T {
123        self.x() * self.x() + self.y() * self.y() + self.z() * self.z() + self.w() * self.w()
124    }
125    pub fn norm(&self) -> T {
126        self.norm_squared().sqrt()
127    }
128    pub fn normalize(&self) -> Self {
129        let l = self.norm();
130        if l > T::zero() || l < T::zero() {
131            Quat::init(self.x() / l, self.y() / l, self.z() / l, self.w() / l)
132        } else {
133            panic!("quat normalization unsuccessful.");
134        }
135    }
136    pub fn normalized(&mut self) {
137        let l = self.norm();
138        if l > T::zero() || l < T::zero() {
139            *self.x_mut() = self.x() / l;
140            *self.y_mut() = self.y() / l;
141            *self.z_mut() = self.z() / l;
142            *self.w_mut() = self.w() / l;
143        } else {
144            panic!("quat normalization unsuccessful.");
145        }
146    }
147    pub fn ln(&self) -> Self {
148        let l = self.norm();
149        let w_ln = self.w().ln();
150        //normalize x,y,z vector -> v/||v||
151        let vec_length = (self.x() * self.x() + self.y() * self.y() + self.z() * self.z()).sqrt();
152        assert!(vec_length != T::zero());
153        let vec_x = self.x() / vec_length;
154        let vec_y = self.y() / vec_length;
155        let vec_z = self.z() / vec_length;
156        //scale x,y,z by acos( w/l )
157        let s = (w_ln / l).acos();
158        Quat::init(vec_x * s, vec_y * s, vec_z * s, w_ln)
159    }
160    pub fn pow(&self, t: T) -> Self {
161        let vec_length = (self.x() * self.x() + self.y() * self.y() + self.z() * self.z()).sqrt();
162        assert!(vec_length != T::zero());
163        let vec_x = self.x() / vec_length;
164        let vec_y = self.y() / vec_length;
165        let vec_z = self.z() / vec_length;
166        let l = self.norm();
167        //original angle
168        let alpha = (self.w() / l).acos();
169        //new angle
170        let beta = t * alpha;
171        let coeff = l.powf(t);
172        Quat::init(
173            coeff * vec_x * beta.sin(),
174            coeff * vec_y * beta.sin(),
175            coeff * vec_z * beta.sin(),
176            coeff * beta.cos(),
177        )
178    }
179    pub fn negate(&self) -> Self {
180        Quat::init(-self.x(), -self.y(), -self.z(), -self.w())
181    }
182    pub fn conjugate(&self) -> Self {
183        Quat::init(-self.x(), -self.y(), -self.z(), self.w())
184    }
185    pub fn scale(&self, s: T) -> Self {
186        Quat::init(self.x() * s, self.y() * s, self.z() * s, self.w() * s)
187    }
188    pub fn scaled(&mut self, s: T) {
189        *self.x_mut() = self.x() * s;
190        *self.y_mut() = self.y() * s;
191        *self.z_mut() = self.z() * s;
192        *self.w_mut() = self.w() * s;
193    }
194    pub fn inverse(&self) -> Self {
195        let conj = self.conjugate();
196        let norm = conj.norm_squared();
197        assert!(norm != T::zero());
198        &conj * (T::one() / norm)
199    }
200    pub fn dot(&self, other: &Self) -> T {
201        self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w()
202    }
203    pub fn lerp(start: Quat<T>, end: Quat<T>, t: T) -> Self {
204        let clamp_upper = if t > T::one() { T::one() } else { t };
205        let clamp = if clamp_upper < T::zero() {
206            T::zero()
207        } else {
208            clamp_upper
209        };
210        Quat::init(
211            start.x() * (T::one() - clamp) + end.x() * clamp,
212            start.y() * (T::one() - clamp) + end.y() * clamp,
213            start.z() * (T::one() - clamp) + end.z() * clamp,
214            start.w() * (T::one() - clamp) + end.w() * clamp,
215        )
216    }
217    pub fn slerp(start: Quat<T>, end: Quat<T>, t: T) -> Self {
218        let t_clamp_upper = if t > T::one() { T::one() } else { t };
219        let t_clamp = if t_clamp_upper < T::zero() {
220            T::zero()
221        } else {
222            t_clamp_upper
223        };
224
225        let cos_omega =
226            start.w() * end.w() + start.x() * end.x() + start.y() * end.y() + start.z() * end.z();
227        let cos_omega_adjust = if cos_omega < T::zero() {
228            -cos_omega
229        } else {
230            cos_omega
231        };
232
233        let end_adjust = if cos_omega < T::zero() {
234            //inverted
235            Quat::init(-end.x(), -end.y(), -end.z(), -end.w())
236        } else {
237            Quat::init(end.x(), end.y(), end.z(), end.w())
238        };
239
240        let (k0, k1) = if cos_omega_adjust > T::one() - T::epsilon() {
241            (T::one() - t_clamp, t_clamp)
242        } else {
243            let sin_omega = (T::one() - cos_omega * cos_omega).sqrt();
244            let omega = sin_omega.atan2(cos_omega);
245            let inv_sin_omega = T::one() / sin_omega;
246            (
247                ((T::one() - t_clamp) * omega).sin() * inv_sin_omega,
248                (t_clamp * omega).sin() * inv_sin_omega,
249            )
250        };
251        Quat::init(
252            start.x() * k0 + end_adjust.x() * k1,
253            start.y() * k0 + end_adjust.y() * k1,
254            start.z() * k0 + end_adjust.z() * k1,
255            start.w() * k0 + end_adjust.w() * k1,
256        )
257    }
258}
259
260impl<T: Real + Default + NumAssign> Add for &Quat<T> {
261    type Output = Quat<T>;
262    fn add(self, rhs: Self) -> Self::Output {
263        self.add(rhs)
264    }
265}
266
267impl<T: Real + Default + NumAssign> Add for Quat<T> {
268    type Output = Quat<T>;
269    fn add(self, rhs: Self) -> Self::Output {
270        (&self).add(&rhs)
271    }
272}
273
274impl<'a, T: Real + Default + NumAssign> Mul<&'a Quat<T>> for &'a Quat<T> {
275    type Output = Quat<T>;
276    fn mul(self, rhs: Self) -> Self::Output {
277        self.mul(rhs)
278    }
279}
280
281impl<T: Real + Default + NumAssign> Mul<Quat<T>> for Quat<T> {
282    type Output = Quat<T>;
283    fn mul(self, rhs: Self) -> Self::Output {
284        (&self).mul(&rhs)
285    }
286}
287
288impl<T: Real + Default + NumAssign> Mul<T> for &Quat<T> {
289    type Output = Quat<T>;
290    fn mul(self, rhs: T) -> Self::Output {
291        self.scale(rhs)
292    }
293}
294
295impl<T: Real + Default + NumAssign> Mul<T> for Quat<T> {
296    type Output = Quat<T>;
297    fn mul(self, rhs: T) -> Self::Output {
298        self.scale(rhs)
299    }
300}
301
302impl<T: Real + Default + NumAssign> Mul<Scalar<T>> for Quat<T> {
303    type Output = Quat<T>;
304    fn mul(self, rhs: Scalar<T>) -> Self::Output {
305        self.scale(rhs.0)
306    }
307}
308
309impl<T: Real + Default + NumAssign> Mul<&Scalar<T>> for Quat<T> {
310    type Output = Quat<T>;
311    fn mul(self, rhs: &Scalar<T>) -> Self::Output {
312        self.scale(rhs.0)
313    }
314}
315
316impl<T: Real + Default + NumAssign> Mul<Scalar<T>> for &Quat<T> {
317    type Output = Quat<T>;
318    fn mul(self, rhs: Scalar<T>) -> Self::Output {
319        self.scale(rhs.0)
320    }
321}
322
323impl<T: Real + Default + NumAssign> Mul<&Scalar<T>> for &Quat<T> {
324    type Output = Quat<T>;
325    fn mul(self, rhs: &Scalar<T>) -> Self::Output {
326        self.scale(rhs.0)
327    }
328}
329
330impl<T: Real + NumAssign + Copy + Default> Mul<&Quat<T>> for Scalar<T> {
331    type Output = Quat<T>;
332    fn mul(self, rhs: &Quat<T>) -> Self::Output {
333        rhs * self.0
334    }
335}
336
337impl<T: Real + NumAssign + Copy + Default> Mul<Quat<T>> for Scalar<T> {
338    type Output = Quat<T>;
339    fn mul(self, rhs: Quat<T>) -> Self::Output {
340        rhs * self.0
341    }
342}
343
344impl<T: Real + Default + NumAssign> Sub for &Quat<T> {
345    type Output = Quat<T>;
346    fn sub(self, rhs: Self) -> Self::Output {
347        self.minus(rhs)
348    }
349}
350
351impl<T: Real + Default + NumAssign> Sub for Quat<T> {
352    type Output = Quat<T>;
353    fn sub(self, rhs: Self) -> Self::Output {
354        self.minus(&rhs)
355    }
356}
357
358#[test]
359fn convert_float() {
360    struct Test<T: Real>(std::marker::PhantomData<T>);
361    impl<T: Real> Test<T> {
362        pub fn give_me_a_number(&self) -> T {
363            T::from(0.55).unwrap()
364        }
365    }
366    let t = Test::<f32>(std::marker::PhantomData);
367    let num = t.give_me_a_number();
368    assert_eq!(0.55f32, num);
369
370    let t2 = Test::<f64>(std::marker::PhantomData);
371    let num2 = t2.give_me_a_number();
372    assert_eq!(0.55f64, num2);
373}
374
375#[cfg(test)]
376#[derive(Clone, Debug)]
377struct RandQuat<T: Real + NumAssign + Default>(pub Quat<T>);
378
379#[cfg(test)]
380impl<T: Real + Default + NumAssign + 'static> quickcheck::Arbitrary for RandQuat<T> {
381    fn arbitrary(g: &mut quickcheck::Gen) -> Self {
382        let selection = (-100..100).map(|x| T::from(x).unwrap()).collect::<Vec<_>>();
383
384        let q = Quat::init(
385            g.choose(&selection).unwrap().to_owned(),
386            g.choose(&selection).unwrap().to_owned(),
387            g.choose(&selection).unwrap().to_owned(),
388            g.choose(&selection).unwrap().to_owned(),
389        );
390
391        Self(q)
392    }
393}
394
395#[cfg(test)]
396#[quickcheck]
397fn quat_mul_scalar_by_quat(m: RandQuat<f64>) -> bool {
398    let q = m.0;
399    let q2 = Scalar(5.) * &q;
400    matrix_approx_eq_float(
401        &q2.0,
402        &Matrix::<f64, 1, 4>::from([[
403            5. * q.0[[0, 0]],
404            5. * q.0[[0, 1]],
405            5. * q.0[[0, 2]],
406            5. * q.0[[0, 3]],
407        ]]),
408        1e-5,
409    )
410}
411
412#[cfg(test)]
413#[quickcheck]
414fn quat_mul_quat_by_float(m: RandQuat<f64>) -> bool {
415    let q = m.0;
416    let q2 = &q * 5.;
417    matrix_approx_eq_float(
418        &q2.0,
419        &Matrix::<f64, 1, 4>::from([[
420            5. * q.0[[0, 0]],
421            5. * q.0[[0, 1]],
422            5. * q.0[[0, 2]],
423            5. * q.0[[0, 3]],
424        ]]),
425        1e-5,
426    )
427}
428
429#[cfg(test)]
430#[quickcheck]
431fn quat_mul_quat_by_scalar(m: RandQuat<f64>) -> bool {
432    let q = m.0;
433    let q2 = &q * &Scalar(5f64);
434    matrix_approx_eq_float(
435        &q2.0,
436        &Matrix::<f64, 1, 4>::from([[
437            5. * q.0[[0, 0]],
438            5. * q.0[[0, 1]],
439            5. * q.0[[0, 2]],
440            5. * q.0[[0, 3]],
441        ]]),
442        1e-5,
443    )
444}
445
446#[cfg(test)]
447#[quickcheck]
448fn quat_partial_eq_same(m: RandQuat<f64>) -> bool {
449    m.0.eq(&m.0)
450}
451
452#[cfg(test)]
453#[quickcheck]
454fn quat_partial_eq_different((mut m1, mut m2): (RandQuat<f64>, RandQuat<f64>)) -> bool {
455    m1.0 .0[[0, 0]] = 1.0;
456    m2.0 .0[[0, 0]] = 2.0;
457    !m1.0.eq(&m2.0)
458}
lightmatrix/quat.rs

lightmatrix/
quat.rs
