lightmatrix/
quatr.rs

1use crate::matrix::*;
2#[cfg(test)]
3use crate::operator::Dot;
4use crate::quat::Quat;
5use crate::scalar::Scalar;
6use delegate::delegate;
7use num_traits::{float::FloatConst, real::Real, NumAssign};
8use std::ops::{Add, Mul, Sub};
9
10/// quaternion for rotation
11#[derive(Clone, Default, Debug)]
12pub struct QuatR<T: Real + NumAssign + Default + Clone + PartialEq>(pub(crate) Quat<T>);
13
14macro_rules! impl_quatr_partialeq_float {
15    ($type:ty) => {
16        impl PartialEq<QuatR<$type>> for QuatR<$type> {
17            fn eq(&self, other: &Self) -> bool {
18                (&self.0).eq(&other.0)
19            }
20        }
21    };
22}
23
24impl_quatr_partialeq_float!(f32);
25impl_quatr_partialeq_float!(f64);
26
27impl<T: Real + NumAssign + Default + Clone + PartialEq> From<Quat<T>> for QuatR<T> {
28    fn from(q: Quat<T>) -> Self {
29        Self(q)
30    }
31}
32
33impl<T: Real + Default + NumAssign> Add for &QuatR<T> {
34    type Output = QuatR<T>;
35    fn add(self, rhs: Self) -> Self::Output {
36        self.add(rhs)
37    }
38}
39
40impl<T: Real + Default + NumAssign> Add for QuatR<T> {
41    type Output = QuatR<T>;
42    fn add(self, rhs: Self) -> Self::Output {
43        (&self).add(&rhs)
44    }
45}
46
47impl<'a, T: Real + Default + NumAssign> Mul<&'a QuatR<T>> for &'a QuatR<T> {
48    type Output = QuatR<T>;
49    fn mul(self, rhs: Self) -> Self::Output {
50        self.mul(rhs)
51    }
52}
53
54impl<T: Real + Default + NumAssign> Mul<QuatR<T>> for QuatR<T> {
55    type Output = QuatR<T>;
56    fn mul(self, rhs: Self) -> Self::Output {
57        (&self).mul(&rhs)
58    }
59}
60
61impl<T: Real + Default + NumAssign> Mul<T> for &QuatR<T> {
62    type Output = QuatR<T>;
63    fn mul(self, rhs: T) -> Self::Output {
64        self.scale(rhs)
65    }
66}
67
68impl<T: Real + Default + NumAssign> Mul<T> for QuatR<T> {
69    type Output = QuatR<T>;
70    fn mul(self, rhs: T) -> Self::Output {
71        self.scale(rhs)
72    }
73}
74
75impl<T: Real + NumAssign + Copy + Default> Mul<&QuatR<T>> for Scalar<T> {
76    type Output = QuatR<T>;
77    fn mul(self, rhs: &QuatR<T>) -> Self::Output {
78        rhs * self.0
79    }
80}
81
82impl<T: Real + NumAssign + Copy + Default> Mul<QuatR<T>> for Scalar<T> {
83    type Output = QuatR<T>;
84    fn mul(self, rhs: QuatR<T>) -> Self::Output {
85        rhs * self.0
86    }
87}
88
89impl<T: Real + Default + NumAssign> Sub for &QuatR<T> {
90    type Output = QuatR<T>;
91    fn sub(self, rhs: Self) -> Self::Output {
92        self.minus(rhs)
93    }
94}
95
96impl<T: Real + Default + NumAssign> Sub for QuatR<T> {
97    type Output = QuatR<T>;
98    fn sub(self, rhs: Self) -> Self::Output {
99        (&self).minus(&rhs)
100    }
101}
102
103impl<T: Real + Default + NumAssign> QuatR<T> {
104    pub fn init(x: T, y: T, z: T, w: T) -> Self {
105        Self(Quat::init(x, y, z, w))
106    }
107    pub fn lerp(start: Quat<T>, end: Quat<T>, t: T) -> QuatR<T> {
108        Self(Quat::lerp(start, end, t))
109    }
110    pub fn slerp(start: Quat<T>, end: Quat<T>, t: T) -> QuatR<T> {
111        Self(Quat::slerp(start, end, t))
112    }
113    pub fn add(&self, other: &Self) -> Self {
114        Self(&self.0 + &other.0)
115    }
116    pub fn minus(&self, other: &Self) -> Self {
117        Self(&self.0 - &other.0)
118    }
119    pub fn mul(&self, other: &Self) -> Self {
120        Self(&self.0 * &other.0)
121    }
122    pub fn dot(&self, other: &Self) -> T {
123        self.0.dot(&other.0)
124    }
125    delegate! {
126        to self.0 {
127            pub fn x(&self)->T;
128            pub fn y(&self)->T;
129            pub fn z(&self)->T;
130            pub fn w(&self)->T;
131            pub fn x_mut(&mut self) -> & mut T;
132            pub fn y_mut(&mut self) -> & mut T;
133            pub fn z_mut(&mut self) -> & mut T;
134            pub fn w_mut(&mut self) -> & mut T;
135            pub fn norm_squared(&self) -> T;
136            pub fn norm(&self) -> T;
137            #[into]
138            pub fn normalize(&self) -> Self;
139            pub fn normalized(&mut self);
140            #[into]
141            pub fn ln(&self) -> Self;
142            #[into]
143            pub fn pow(&self, t:T) -> Self;
144            #[into]
145            pub fn negate(&self) -> Self;
146            #[into]
147            pub fn conjugate(&self) -> Self;
148            #[into]
149            pub fn scale(&self, s:T) -> Self;
150            pub fn scaled(&mut self, s: T);
151            #[into]
152            pub fn inverse(&self) -> Self;
153        }
154    }
155    #[allow(dead_code)]
156    pub fn init_auto_w(x: T, y: T, z: T) -> Self {
157        let w = T::one() - x * x - y * y - z * z;
158        if w < T::zero() {
159            QuatR(Quat(Matrix::from([[x, y, z, w]])))
160        } else {
161            QuatR(Quat(Matrix::from([[
162                x,
163                y,
164                z,
165                T::from(-1.).unwrap() * w.sqrt(),
166            ]])))
167        }
168    }
169    ///expects a proper rotation matrix as input
170    pub fn init_from_rotation(rot: Matrix<T, 3, 3>) -> Self {
171        let t = rot.trace();
172        if t > T::zero() {
173            let s = T::from(0.5).unwrap() / (t + T::one()).sqrt();
174
175            QuatR(Quat::init(
176                (rot[[2, 1]] - rot[[1, 2]]) * s,
177                (rot[[0, 2]] - rot[[2, 0]]) * s,
178                (rot[[1, 0]] - rot[[0, 1]]) * s,
179                T::one() / s * T::from(0.25).unwrap(),
180            ))
181        } else if rot[[0, 0]] > rot[[1, 1]] && rot[[0, 0]] > rot[[2, 2]] {
182            let s =
183                T::from(2.).unwrap() * (T::one() + rot[[0, 0]] - rot[[1, 1]] - rot[[2, 2]]).sqrt();
184
185            QuatR(Quat::init(
186                T::from(0.25).unwrap() * s,
187                (rot[[0, 1]] + rot[[1, 0]]) / s,
188                (rot[[0, 2]] + rot[[2, 0]]) / s,
189                (rot[[2, 1]] - rot[[1, 2]]) / s,
190            ))
191        } else if rot[[1, 1]] > rot[[2, 2]] {
192            let s =
193                T::from(2.).unwrap() * (T::one() + rot[[1, 1]] - rot[[0, 0]] - rot[[2, 2]]).sqrt();
194
195            QuatR(Quat::init(
196                (rot[[0, 1]] - rot[[1, 0]]) / s,
197                T::from(0.25).unwrap() * s,
198                (rot[[1, 2]] - rot[[2, 1]]) / s,
199                (rot[[0, 2]] - rot[[2, 0]]) / s,
200            ))
201        } else {
202            let s =
203                T::from(2.).unwrap() * (T::one() + rot[[2, 2]] - rot[[0, 0]] - rot[[1, 1]]).sqrt();
204
205            QuatR(Quat::init(
206                (rot[[0, 2]] - rot[[2, 0]]) / s,
207                (rot[[1, 2]] - rot[[2, 1]]) / s,
208                T::from(0.25).unwrap() * s,
209                (rot[[1, 0]] - rot[[0, 1]]) / s,
210            ))
211        }
212    }
213    /// returns a transformation matrix
214    #[allow(dead_code)]
215    pub fn to_matrix(&self) -> Matrix<T, 4, 4> {
216        //assumes unit quaternion
217        let a = self.normalize();
218        let two = T::from(2.).unwrap();
219
220        Matrix::from([
221            [
222                T::one() - two * (a.y() * a.y() + a.z() * a.z()), //first row
223                two * (a.x() * a.y() - a.z() * a.w()),
224                two * (a.x() * a.z() + a.y() * a.w()),
225                T::zero(),
226            ],
227            [
228                two * (a.x() * a.y() + a.z() * a.w()), //second row
229                T::one() - two * (a.x() * a.x() + a.z() * a.z()),
230                two * (a.y() * a.z() - a.x() * a.w()),
231                T::zero(),
232            ],
233            [
234                two * (a.x() * a.z() - a.y() * a.w()), //third row
235                two * (a.z() * a.y() + a.x() * a.w()),
236                T::one() - two * (a.x() * a.x() + a.y() * a.y()),
237                T::zero(),
238            ],
239            [
240                T::zero(), //last row
241                T::zero(),
242                T::zero(),
243                T::one(),
244            ],
245        ])
246    }
247    #[allow(dead_code)]
248    pub fn init_from_axis_angle_degree(axis: Matrix<T, 3, 1>, angle: T) -> Self
249    where
250        T: FloatConst,
251    {
252        Self::init_from_axis_angle_radian(axis, angle.to_radians())
253    }
254    #[allow(dead_code)]
255    pub fn init_from_axis_angle_radian(axis: Matrix<T, 3, 1>, angle: T) -> Self
256    where
257        T: FloatConst,
258    {
259        let two = T::from(2.).unwrap();
260        let radian = ((angle % (two * T::PI())) + two * T::PI()) % (two * T::PI());
261        let axis_adjust = axis.normalize_l2();
262        let sine_half = (radian / T::from(2.).unwrap()).sin();
263        QuatR(Quat::init(
264            axis_adjust[[0, 0]] * sine_half,
265            axis_adjust[[1, 0]] * sine_half,
266            axis_adjust[[2, 0]] * sine_half,
267            (radian / two).cos(),
268        ))
269    }
270    /// returns ([x,y,z], angle) where angle is in radians
271    #[allow(dead_code)]
272    pub fn to_axis_angle(&self) -> (Matrix<T, 3, 1>, T) {
273        let q = self.normalize();
274        let k = (T::one() - q.w() * q.w()).sqrt();
275        if k < T::epsilon() {
276            (
277                Matrix::from([[T::one()], [T::zero()], [T::zero()]]),
278                T::zero(),
279            )
280        } else {
281            let vec_x = q.x() / k;
282            let vec_y = q.y() / k;
283            let vec_z = q.z() / k;
284            (
285                Matrix::from([[vec_x], [vec_y], [vec_z]]),
286                T::from(2.).unwrap() * self.w().acos(),
287            )
288        }
289    }
290    ///rotation of a vector p, by a unit quaternion q:  q * p q', where q' is the conjugate
291    #[allow(dead_code)]
292    pub fn rotate_vector(&self, p: Matrix<T, 3, 1>) -> Matrix<T, 3, 1> {
293        let quat_p = QuatR(Quat::init(p[[0, 0]], p[[1, 0]], p[[2, 0]], T::zero()));
294        let temp2 = &(self * &quat_p) * &self.conjugate();
295        Matrix::from([[temp2.x()], [temp2.y()], [temp2.z()]])
296    }
297}
298
299#[test]
300fn test_quatr_convert_axis_angle_0() {
301    use more_asserts::assert_le;
302    use std::f64::consts::PI;
303    //convert axis angle to quaternion representation and back
304    let axis = Matrix::from([[1., 2., 3.]]).t();
305
306    let axis_normalize = axis.normalize_l2();
307    let q = QuatR::init_from_axis_angle_degree(axis, 90.);
308    let (a, angle) = q.to_axis_angle();
309    assert_eq!(a, axis_normalize);
310    assert_le!((angle / PI * 180. - 90.).abs(), 1e-9);
311}
312#[test]
313fn test_quatr_convert_axis_angle_1() {
314    use more_asserts::assert_le;
315    use std::f32::consts::PI;
316    //convert axis angle to quaternion representation and back
317    let axis = Matrix::from([[1., 2., 3.]]).t();
318
319    let axis_normalize = axis.normalize_l2();
320    let q = QuatR::init_from_axis_angle_degree(axis, 370.);
321    let (a, angle) = q.to_axis_angle();
322    // assert_eq!(a, axis_normalize);
323    assert_matrix_approx_eq_float(&a, &axis_normalize, 1e-5);
324    assert_le!((angle / PI * 180. - 10.).abs(), 1e-4);
325}
326#[test]
327fn test_quatr_convert_axis_angle_2() {
328    use std::f32::consts::PI;
329    //convert axis angle to quaternion representation and back
330    let axis = Matrix::from([[1., 2., 3.]]).t();
331
332    let axis_normalize = axis.normalize_l2();
333    let q = QuatR::init_from_axis_angle_degree(axis, -33.);
334    let (a, angle) = q.to_axis_angle();
335    assert!(
336        ((angle / PI * 180. - (360. - 33.)).abs() < 1e-9 && a == axis_normalize)
337            || ((angle / PI * 180. + (360. - 33.)).abs() < 1e-9 && (a * -1.) == (axis_normalize))
338    );
339}
340#[test]
341fn test_quatr_rotate_point() {
342    use more_asserts::assert_le;
343    use std::f32::consts::PI;
344    //compute rotation using quaternion
345    //rotate a vector using the rotation matrix and compare to rotation using quaternions
346    let p = Matrix::from([[1., 5., -3.]]).t();
347    let axis = Matrix::from([[1., 0., 0.]]).t();
348
349    let axis_normalize = axis.normalize_l2();
350    let q = QuatR::init_from_axis_angle_degree(axis, 90.);
351    let (a, angle) = q.to_axis_angle();
352    assert_eq!(a, axis_normalize);
353    assert_le!((angle / PI * 180. - 90.).abs(), 1e-5);
354
355    let rot = q.to_matrix();
356
357    let rot_expected = Matrix::from([
358        [1., 0., 0., 0.],
359        [0., 0., -1., 0.],
360        [0., 1., 0., 0.],
361        [0., 0., 0., 1.],
362    ]);
363
364    assert_eq!(rot, rot_expected);
365    // println!("rot: {:#?}", rot);
366    // println!("rot_expected: {:#?}", rot_expected);
367
368    let vec_rotated_expected =
369        rot_expected.dot(&Matrix::from([[p[[0, 0]], p[[1, 0]], p[[2, 0]], 1.]]).t());
370
371    let vec_rotated = q.rotate_vector(p);
372
373    assert_matrix_approx_eq_float(
374        &vec_rotated,
375        &Matrix::from([[
376            vec_rotated_expected[[0, 0]],
377            vec_rotated_expected[[1, 0]],
378            vec_rotated_expected[[2, 0]],
379        ]])
380        .t(),
381        1e-6,
382    );
383}
lightmatrix/quatr.rs

lightmatrix/
quatr.rs
