feo_math/rotation/
quaternion.rs

1//! Construct that represents a quaternion.
2//! 
3//! TODO
4//! 
5use std::{fmt, ops::{Add, Div, Mul, Neg, Rem, Sub}};
6
7use crate::{Construct, F32Fmt, One, /* Product ,*/ SignOps, Two, Zero, axes::{self, Axes}, imaginary::ImaginaryConstruct, linear_algebra::{
8        vector4::Vector4,
9        vector3::Vector3,
10        matrix3::Matrix3
11    }};
12
13use super::{Rotation, euler::Euler, rotor::Rotor};
14
15// Rules
16// 1   i   j   k (second)
17// i   -1  k   -j
18// j   -k  -1  i  
19// k    j  -i  -1
20// (first) ex: ij = k
21
22/// A Quaternion with the format q = (q.0)i + (q.1)j + (q.2)k + (q.3)
23/// Quaternions are usually stored 
24/// `q = r + ai + bj + ck`
25/// in this case however they are stored in the format 
26/// `q = ai + bj + ck + r`
27/// to emphasize the fact that you can essentially use the r value 
28/// as a scaler.
29#[derive(Clone, Copy)]
30pub struct Quaternion<T>(pub T, pub T, pub T, pub T);
31
32impl<T> PartialEq for Quaternion<T> 
33where T: Neg<Output = T> + PartialEq + Copy {
34    fn eq(&self, other: &Self) -> bool {
35        let neg = -*other;
36        (self.0 == other.0 && self.1 == other.1 && self.2 == other.2 && self.3 == other.3) ||
37        (self.0 == neg.0 && self.1 == neg.1 && self.2 == neg.2 && self.3 == neg.3) 
38    }
39}
40
41impl<T> Quaternion<T> {
42    /// Creates a new Quaternion with defined a, b, c, and r values.
43    /// # Attributes
44    /// * `a` - value multiplied by the imaginary number `i` in `q = ai + bj + ck + r`
45    /// * `b` - value multiplied by the imaginary number `j` in `q = ai + bj + ck + r`
46    /// * `c` - value multiplied by the imaginary number `k` in `q = ai + bj + ck + r`
47    /// * `r` - The real number denoted `r` in `q = ai + bj + ck + r`
48    pub fn new(a: T, b: T, c: T, r: T) -> Self {
49        Quaternion(a, b, c, r)
50    }
51
52    /// makes a quaternion from a Vector3 and a given real `r` value
53    /// # Arguments
54    /// * `vector` - A `Vector3` which maps out to (a, b, c)
55    /// * `real` - The r value of the Quaternion
56    pub fn new_vector_real(vector: Vector3<T>, real: T) -> Self {
57        Quaternion(vector.0, vector.1, vector.2, real)
58    }
59
60    /// creates a new quaternion that corresponds to a given rotation around a given axis
61    /// # Arguments
62    /// * `axis` - The `Vector3` rotation axis
63    /// * `angle` - the angle to rotate around that axis
64    pub fn new_axis_angle(axis: Vector3<T>, angle: T) -> Self
65    where T: Mul<T, Output = T> + Div<T, Output = T> + Add<T, Output = T> + F32Fmt + fmt::Debug + One + Two + Copy { // angle is in radians and axis is normalized
66        let normalized_axis = axis.normalize(None);
67        //println!("{:?} {:?}", normalized_axis, angle);
68        let half_angle = angle / T::TWO;
69        Quaternion(
70            T::sin_mul(half_angle, normalized_axis.0),
71            T::sin_mul(half_angle, normalized_axis.1),
72            T::sin_mul(half_angle, normalized_axis.2),
73            T::cos_mul(half_angle, T::ONE)
74        )
75    }
76
77    /// Returns the identity quaternion that is used for multiplying 
78    /// # Arguments
79    /// * `zero` - the T equivalent of zero
80    /// * `one` - the T equivalent of one
81    pub fn identity() -> Self
82    where T: Zero + One {
83        Quaternion::ONE // multiplication
84    }
85
86    /// Returns the identity quaternion that is used for adding
87    /// # Arguments
88    /// * `zero` - the T equivalent of zero
89    pub fn identity_add() -> Self
90    where T: Zero {
91        Quaternion::ZERO // addition
92    }
93
94    /// Returns the conjugate of the quaternion denoted `q*`
95    /// # Arguments
96    /// # Examples
97    /// ```ignore
98    /// todo!();
99    /// ```
100    pub fn conjugate(&self) -> Self
101    where T: Neg<Output = T> + Copy {
102        Quaternion(-self.0, -self.1, -self.2, self.3)
103    }
104
105    /// Returns the magnitude of the quaternion denoted `||q||`
106    /// # Arguments
107    /// # Examples
108    /// ```ignore
109    /// todo!();
110    /// ```
111    pub fn norm(&self) -> T
112    where T: Add<T, Output = T> + Mul<T, Output = T> + F32Fmt + Copy {
113        //println!("{} {}", (self.0 * self.0 + self.1 * self.1 + self.2 * self.2 + self.3 * self.3).into(), (self.0 * self.0 + self.1 * self.1 + self.2 * self.2 + self.3 * self.3).into().sqrt());
114        // conjugate
115        (self.0 * self.0 + self.1 * self.1 + self.2 * self.2 + self.3 * self.3).sqrt()
116    }
117
118    /// Returns the unit quaternion form of the quaternion denoted `Uq`
119    /// # Arguments
120    /// # Examples
121    /// ```ignore
122    /// todo!();
123    /// ```
124    pub fn unit_quaternion(&self) -> Self
125    where T: Add<T, Output = T> + Mul<T, Output = T> + Div<T, Output = T> + F32Fmt + Copy {
126        *self / self.norm()
127    }
128
129    /// Returns the reciprocal of a quaternion denoted `q⁻¹`
130    /// # Arguments
131    /// # Examples
132    /// ```ignore
133    /// todo!();
134    /// ```
135    pub fn reciprocal(&self) -> Self
136    where T: Add<T, Output = T> + Mul<T, Output = T> + Div<T, Output = T> + Neg<Output = T> + Copy + One {
137        self.conjugate() / (self.0 * self.0 + self.1 * self.1 + self.2 * self.2 + self.3 * self.3)
138    }
139    
140    pub fn slerp(self, to: Self, time_stamp: T::F32Fmt) -> Self 
141    where T: F32Fmt + One + Mul<T, Output = T> + Add<T, Output = T> + Sub<T, Output = T> + Neg<Output = T> + Copy + Div<T, Output = T>{
142        let self_conj = self.conjugate();
143        let cos_half_angle = (self_conj.3 * to.3 - self_conj.0 * to.0 - self_conj.1 * to.1 - self_conj.2 * to.2).intoF32Fmt();
144        if cos_half_angle.abs() == T::F32Fmt::ONE {
145            return self;
146        }
147        let half_angle = cos_half_angle.acos_mul(T::F32Fmt::ONE);
148
149        Self::fromF32Fmt(
150            (
151                self.intoF32Fmt() * ((T::F32Fmt::ONE - time_stamp) * half_angle).sin_mul(T::F32Fmt::ONE)
152                + to.intoF32Fmt() * (time_stamp * half_angle)
153            ) / (T::F32Fmt::ONE - cos_half_angle).sqrt()
154        )
155    }
156
157    pub fn sin(self) -> Self
158    where T: Copy + F32Fmt + One + Sub<T, Output = T> + Add<T, Output = T> + Mul<T, Output = T> + Neg<Output = T>{
159        // sin(a + bi + cj + dk)
160        // sin(a + bi)cos(cj + dk) + cos(a + bi)sin(cj + dk)
161        // (sin(a)cos(bi) + cos(a)sin(bi))(cos(cj)cos(dk) - sin(cj)sin(dk)) + (cos(a)cos(bi) - sin(a)sin(bi))(sin(cj)cos(dk) + cos(cj)sin(dk))
162        // (sin(a)cosh(b) + icos(a)sinh(b))(cosh(c)cosh(d) - jksinh(c)sinh(d)) + (cos(a)cosh(b) - isin(a)sinh(b))(jsinh(c)cosh(d) + kcosh(c)sinh(d))
163        // (sin(a)cosh(b) + icos(a)sinh(b))(cosh(c)cosh(d) - isinh(c)sinh(d)) + (cos(a)cosh(b) - isin(a)sinh(b))(jsinh(c)cosh(d) + kcosh(c)sinh(d))
164        //
165        // (sin(a)cosh(b)cosh(c)cosh(d) - sin(a)cosh(b)isinh(c)sinh(d) + icos(a)sinh(b)cosh(c)cosh(d) - icos(a)sinh(b)isinh(c)sinh(d))
166        // + (cos(a)cosh(b)jsinh(c)cosh(d) + cos(a)cosh(b)kcosh(c)sinh(d) - isin(a)sinh(b)jsinh(c)cosh(d) - isin(a)sinh(b)kcosh(c)sinh(d))
167        //
168        // (sin(a)cosh(b)cosh(c)cosh(d) - isin(a)cosh(b)sinh(c)sinh(d) + icos(a)sinh(b)cosh(c)cosh(d) - iicos(a)sinh(b)sinh(c)sinh(d))
169        // + (jcos(a)cosh(b)sinh(c)cosh(d) + kcos(a)cosh(b)cosh(c)sinh(d) - ijsin(a)sinh(b)sinh(c)cosh(d) - iksin(a)sinh(b)cosh(c)sinh(d))
170        //
171        // (sin(a)cosh(b)cosh(c)cosh(d) - isin(a)cosh(b)sinh(c)sinh(d) + icos(a)sinh(b)cosh(c)cosh(d) + cos(a)sinh(b)sinh(c)sinh(d))
172        // + (jcos(a)cosh(b)sinh(c)cosh(d) + kcos(a)cosh(b)cosh(c)sinh(d) - ksin(a)sinh(b)sinh(c)cosh(d) + jsin(a)sinh(b)cosh(c)sinh(d))
173        //
174        // ((sin(a)cosh(b)cosh(c)cosh(d) + cos(a)sinh(b)sinh(c)sinh(d)) - i(sin(a)cosh(b)sinh(c)sinh(d) + cos(a)sinh(b)cosh(c)cosh(d)))
175        // + (j(cos(a)cosh(b)sinh(c)cosh(d) + sin(a)sinh(b)cosh(c)sinh(d)) + k(cos(a)cosh(b)cosh(c)sinh(d) - sin(a)sinh(b)sinh(c)cosh(d)))
176        //
177        // (sin(a)cosh(b)cosh(c)cosh(d) + cos(a)sinh(b)sinh(c)sinh(d)) - i(sin(a)cosh(b)sinh(c)sinh(d) + cos(a)sinh(b)cosh(c)cosh(d))
178        // + j(cos(a)cosh(b)sinh(c)cosh(d) + sin(a)sinh(b)cosh(c)sinh(d)) + k(cos(a)cosh(b)cosh(c)sinh(d) - sin(a)sinh(b)sinh(c)cosh(d))
179        //
180        // (sin(a)cosh(b)cosh(c)cosh(d) + cos(a)sinh(b)sinh(c)sinh(d))
181        // - i(sin(a)cosh(b)sinh(c)sinh(d) + cos(a)sinh(b)cosh(c)cosh(d))
182        // + j(cos(a)cosh(b)sinh(c)cosh(d) + sin(a)sinh(b)cosh(c)sinh(d))
183        // + k(cos(a)cosh(b)cosh(c)sinh(d) - sin(a)sinh(b)sinh(c)cosh(d))
184        let a = self.3; // r
185        let b = self.0;
186        let c = self.1;
187        let d = self.2;
188        Quaternion::new(
189            a.sin_mul(b.cosh_mul(c.cosh_mul(d.cosh_mul(T::ONE)))) + a.cos_mul(b.sinh_mul(c.sinh_mul(d.sinh_mul(T::ONE)))),
190            -a.sin_mul(b.cosh_mul(c.sinh_mul(d.sinh_mul(T::ONE)))) + a.cos_mul(b.sinh_mul(c.cosh_mul(d.cosh_mul(T::ONE)))),
191            a.cos_mul(b.cosh_mul(c.sinh_mul(d.cosh_mul(T::ONE)))) + a.sin_mul(b.sinh_mul(c.cosh_mul(d.sinh_mul(T::ONE)))),
192            a.cos_mul(b.cosh_mul(c.cosh_mul(d.sinh_mul(T::ONE)))) - a.sin_mul(b.sinh_mul(c.sinh_mul(d.cosh_mul(T::ONE)))),
193        )
194    }
195
196    pub fn cos(self) -> Self{
197        //
198        todo!();
199    }
200
201    pub fn tan(self) -> Self{
202        todo!();
203    }
204} 
205
206impl Quaternion<f32> {
207    /// testing
208    pub fn camera_look_at_v1(pos: Vector3<f32>, look_at: Vector3<f32>) -> Self { // not done
209        let frd_camrel = (look_at - pos).normalize(None); 
210        let right_camrel = Vector3::cross_product(axes::NORMAL_Y_AXIS, frd_camrel).normalize(None); // axis 2
211        
212        Quaternion::from_axes(right_camrel, frd_camrel)
213    }
214
215    /// testing
216    pub fn from_axes(left: Vector3<f32>, frd: Vector3<f32>) -> Self { // merge 
217        let up = Vector3::cross_product( frd, left); //.normalize(None); // axis 2
218
219        // so the rotation matrix is:
220        // [  left ] this is row representation!
221        // [  up   ] each axis is a row in the 
222        // [  frd  ] corresponding rotation matrix
223
224        let derived_rot_mat = Matrix3::new( // rmb in col format
225            left.into(),
226            up.into(),
227            frd.into(),
228        ).transpose();
229        
230        derived_rot_mat.into()
231    }
232    
233    /// Camera Objects look along the -Z axis so the look_at function for a camera object
234    /// needs to be modified a little. flipping the pos and the look_at should do the trick.
235    /// ```text
236    ///    ^ Z
237    ///    |
238    ///    |
239    /// Y• C ------> X
240    ///   / \
241    ///  /   \
242    /// /     \
243    /// ```
244    /// # Arguments 
245    /// * `pos` - A `Vector3<f32>` representing the position of the camera
246    /// * `look_at` - A `Vector3<f32>` representing the position the camera is to look at
247    /// # Examples
248    /// ```ignore //rust
249    /// use crate::linear_algebra::Vector3;
250    /// let pos = Vector3::new(0.0, 0.0, 0.0);
251    /// let look_at = Vector3::new(1.0, 1.0, 1.0);
252    /// assert_eq!(camera_look_at(pos, look_at), todo!());
253    /// ```
254    pub fn camera_look_at(pos: Vector3<f32>, look_at: Vector3<f32>) -> Quaternion<f32> {
255        Quaternion::look_at_xy(look_at, pos)
256    }
257
258    pub fn f32_identity() -> Self{
259        Quaternion::new( 0.0_f32, 0.0_f32, 0.0_f32, 1.0_f32)
260    }
261}
262
263impl<T> Construct<T> for Quaternion<T> where T: Construct<T> {}
264impl<T> ImaginaryConstruct<T> for Quaternion<T> where T: Construct<T> {}
265impl<T> Rotation<T> for Quaternion<T> where T: Construct<T> {
266    /// # Examples
267    /// ```ignore //rust
268    /// use crate::linear_algebra::Vector3;
269    /// let pos = Vector3::new(0.0, 0.0, 0.0);
270    /// let look_at = Vector3::new(1.0, 1.0, 1.0);
271    /// assert_eq!(camera_look_at(pos, look_at), Quaternion::new());
272    /// ```
273    fn look_at_xy(pos: Vector3<T>, look_at: Vector3<T>) -> Self {
274        let frd_camrel = (look_at - pos).normalize(None);
275        let right_camrel = Vector3::cross_product(Self::Y_AXIS, frd_camrel).normalize(None);
276
277        let frd_no_y = Vector3::cross_product(right_camrel, Self::Y_AXIS).normalize(None);//. z;
278
279        //             0 when in line with each other
280        //            -y axis 
281        // -sqrt(2)/2 _|_ sqrt(2)/2
282        //          ___|___ frd 1
283        // -sqrt(2)/2 _|_ sqrt(2)/2
284        //             |
285        //             0 when exactly opposite
286        let self_x_axis_turn = T::asin_mul(
287            Vector3::dot_product(-Self::Y_AXIS, frd_camrel),
288            T::ONE
289        );
290
291        let self_y_axis_turn = T::atan2_mul(
292            Vector3::dot_product(Self::X_AXIS, frd_no_y),
293            Vector3::dot_product(Self::X_AXIS, right_camrel), 
294            T::ONE
295        );
296
297        Quaternion::new_axis_angle(right_camrel, self_x_axis_turn) *
298        Quaternion::new_axis_angle(Self::Y_AXIS, self_y_axis_turn)
299    }
300
301    fn look_at_xz(_pos: Vector3<T>, _look_at: Vector3<T>) -> Self {
302        todo!()
303    }
304
305    fn look_at_yz(_pos: Vector3<T>, _look_at: Vector3<T>) -> Self {
306        todo!()
307    }
308
309    fn look_at_lock(_pos: Vector3<T>, _look_at: Vector3<T>, _locked_axis: Vector3<T>) -> Self {
310        todo!()
311    }
312
313    /// # Examples
314    /// ```ignore //rust
315    /// use crate::linear_algebra::Vector3;
316    /// let pos = Vector3::new(0.0, 0.0, 0.0);
317    /// let look_at = Vector3::new(1.0, 1.0, 1.0);
318    /// assert_eq!(camera_look_at(pos, look_at), todo!());
319    /// ```
320    fn camera_look_at_xy(pos: Vector3<T>, look_at: Vector3<T>) -> Self { Quaternion::look_at_xy(look_at, pos) }
321
322    fn camera_look_at_xz(pos: Vector3<T>, look_at: Vector3<T>) -> Self { Quaternion::look_at_xz(look_at, pos) }
323
324    fn camera_look_at_yz(pos: Vector3<T>, look_at: Vector3<T>) -> Self { Quaternion::look_at_yz(look_at, pos) }
325
326    fn camera_look_at_lock(pos: Vector3<T>, look_at: Vector3<T>, locked_axis: Vector3<T>) -> Self { 
327        Quaternion::look_at_lock(look_at, pos, locked_axis)
328    }
329}
330impl<T> F32Fmt for Quaternion<T> 
331where T: F32Fmt + Copy + Mul<T, Output = T> + Add<T, Output = T> + Sub<T, Output = T> + Div<T, Output = T> + Neg<Output = T> + One { 
332    type F32Fmt = Quaternion<T::F32Fmt>;
333    #[inline]
334    fn intoF32Fmt(self) -> Self::F32Fmt {
335        Quaternion(self.0.intoF32Fmt(), self.1.intoF32Fmt(), self.2.intoF32Fmt(), self.3.intoF32Fmt())
336    } 
337    #[inline]
338    fn fromF32Fmt(f32_fmt: Self::F32Fmt) -> Self {
339        let vec = &f32_fmt;
340        Quaternion(T::fromF32Fmt(vec.0), T::fromF32Fmt(vec.1), T::fromF32Fmt(f32_fmt.2), T::fromF32Fmt(f32_fmt.3))
341    }
342 
343    fn sqrt(self) -> Self {
344        let v_part = Vector3(self.0, self.1, self.2);
345        let r_part = self.3;
346
347        let block0 = (self.norm() + r_part).f32_const_mul(0.5).sqrt();
348
349        Quaternion::new_vector_real(v_part.unit_vector() * block0, block0)
350    }
351
352    fn cbrt(self) -> Self {
353        todo!();
354    }
355
356    fn f32_const_mul(self, constant: f32) -> Self {
357        Quaternion(
358            self.0.f32_const_mul(constant), 
359            self.1.f32_const_mul(constant), 
360            self.2.f32_const_mul(constant), 
361            self.3.f32_const_mul(constant)
362        )
363    }
364
365    fn sin_mul(self, mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
366        self.sin() * mul_by
367    }
368
369    fn cos_mul(self, mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
370        self.cos() * mul_by
371    }
372
373    fn tan_mul(self, mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
374        self.tan() * mul_by
375    }
376
377    fn asin_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
378        todo!()
379    }
380
381    fn acos_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
382        todo!()
383    }
384
385    fn atan_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
386        todo!()
387    }
388
389    fn atan2_mul(self, _other: Self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
390        todo!()
391    }
392
393    fn sinh_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
394        todo!()
395    }
396
397    fn cosh_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
398        todo!()
399    }
400
401    fn tanh_mul(self, _mul_by: Self) -> Self where Self: Mul<Self, Output = Self> + Sized {
402        todo!()
403    }
404}
405
406impl<T> From<Matrix3<T>> for Quaternion<T> 
407where T: F32Fmt + Add<T, Output = T> + Sub<T, Output = T> + Div<T, Output = T> + Mul<T, Output = T> + fmt::Debug + Two + One + Zero + Copy {
408    fn from(mat3: Matrix3<T>) -> Self {
409        // get axis
410        let axis = mat3.eigen_vector(T::ONE); // nd
411
412        // get angle
413        // |\ 2
414        // |_\
415        //  1
416        
417        let trace = mat3.trace();
418        let angle = T::acos_mul((trace - T::ONE) / T::TWO, T::ONE);
419
420        Quaternion::new_axis_angle(axis, angle)
421    }
422}
423impl<T> From<Axes<T>> for Quaternion<T> 
424where T: Construct<T> + Copy {
425    fn from(a: Axes<T>) -> Self {
426        Into::<Matrix3<T>>::into(a).into()
427    }
428}
429impl<T> From<Euler<T>> for Quaternion<T> 
430where T: Construct<T> + Copy {
431    fn from(e: Euler<T>) -> Self {
432        Into::<Matrix3<T>>::into(e).into()
433    }
434}
435impl<T> From<Rotor<T>> for Quaternion<T> 
436where T: Construct<T> + Copy {
437    fn from(r: Rotor<T>) -> Self {
438        Into::<Matrix3<T>>::into(r).into()
439    }
440}
441
442impl<T> From<Quaternion<T>> for Matrix3<T> 
443where T: Add<T, Output = T> + Sub<T, Output = T> + Mul<T, Output = T> + Div<T, Output = T> + Neg<Output = T> + Copy + One + Two {
444    fn from(other: Quaternion<T>) -> Matrix3<T> {
445        let i = other.0; let i_squared = i * i;
446        let j = other.1; let j_squared = j * j;
447        let k = other.2; let k_squared = k * k;
448        let r = other.3; // distance from center 
449
450        // p'
451        
452        let two: T = T::TWO;
453        let one: T = T::ONE;
454        // let s: T = one / (i_squared + j_squared + k_squared + r * r);
455        // Matrix3::new(
456        //     [one - two * s * (j_squared + k_squared),               two * s * (i * j - k * r),               two * s * (i * k + j * r)],
457        //     [              two * s * (i * j + k * r), one - two * s * (i_squared + k_squared),               two * s * (j * k - i * r)],
458        //     [              two * s * (i * k - j * r),               two * s * (j * k + i * r), one - two * s * (i_squared + j_squared)],
459        // )
460
461        // let s: T = two / (i_squared + j_squared + k_squared + r * r);
462        // Matrix3::new(
463        //     [one - s * (j_squared + k_squared),               s * (i * j - k * r),               s * (i * k + j * r)],
464        //     [              s * (i * j + k * r), one - s * (i_squared + k_squared),               s * (j * k - i * r)],
465        //     [              s * (i * k - j * r),               s * (j * k + i * r), one - s * (i_squared + j_squared)],
466        // )
467
468
469        let two_s: T = two / (i_squared + j_squared + k_squared + r * r);
470
471        Matrix3::new(
472            [one - two_s * (j_squared + k_squared),               two_s * (i * j - k * r),               two_s * (i * k + j * r)],
473            [              two_s * (i * j + k * r), one - two_s * (i_squared + k_squared),               two_s * (j * k - i * r)],
474            [              two_s * (i * k - j * r),               two_s * (j * k + i * r), one - two_s * (i_squared + j_squared)],
475        )
476    }
477}
478impl<T> From<Quaternion<T>> for Vector4<T> where T: Copy { fn from(other: Quaternion<T>) -> Vector4<T> { Vector4(other.0, other.1, other.2, other.3) } } // rmv pointless
479impl<T> From<Quaternion<T>> for Vector3<T> where T: Copy { fn from(other: Quaternion<T>) -> Vector3<T> { Vector3(other.0, other.1, other.2) } } // just the vector/imaginary part
480impl<T> From<Quaternion<T>> for [T; 4] where T: Copy { fn from(other: Quaternion<T>) -> [T; 4] { [other.0, other.1, other.2, other.3] } }
481impl<T> fmt::Debug for Quaternion<T> where T: Copy + fmt::Debug {
482    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
483        write!(f, "{:?}i + {:?}j + {:?}k + {:?}", self.0, self.1, self.2, self.3)
484    }
485}
486
487impl<T> Zero for Quaternion<T> where T: Zero { const ZERO: Self = Quaternion(T::ZERO, T::ZERO, T::ZERO, T::ZERO); }
488impl<T> One for Quaternion<T> where T: One + Zero { const ONE: Self = Quaternion(T::ZERO, T::ZERO, T::ZERO, T::ONE); }
489impl<T> Two for Quaternion<T> where T: Two + Zero { const TWO: Self = Quaternion(T::ZERO, T::ZERO, T::ZERO, T::TWO); }
490
491
492impl<T> Add for Quaternion<T> 
493where T: Add<T, Output = T> + Copy {
494    type Output = Self;
495
496    fn add(self, rhs: Self) -> Self::Output {
497        Quaternion(self.0 + rhs.0, self.1 + rhs.1, self.2 + rhs.2, self.3 + rhs.3)
498    }
499}
500impl<T> Add<T> for Quaternion<T> 
501where T: Add<T, Output = T> + Copy {
502    type Output = Self;
503
504    fn add(self, rhs: T) -> Self::Output {
505        Quaternion(self.0 + rhs, self.1 + rhs, self.2 + rhs, self.3 + rhs)
506    }
507}
508
509impl<T> Sub for Quaternion<T> 
510where T: Sub<T, Output = T> + Copy {
511    type Output = Self;
512
513    fn sub(self, rhs: Self) -> Self::Output {
514        Quaternion(self.0 - rhs.0, self.1 - rhs.1, self.2 - rhs.2, self.3 - rhs.3)
515    }
516}
517impl<T> Sub<T> for Quaternion<T> 
518where T: Sub<T, Output = T> + Copy {
519    type Output = Self;
520
521    fn sub(self, rhs: T) -> Self::Output {
522        Quaternion(self.0 - rhs, self.1 - rhs, self.2 - rhs, self.3 - rhs)
523    }
524}
525
526impl<T> Mul for Quaternion<T> 
527where T: Sub<T, Output = T> + Mul<T, Output = T> + Add<T, Output = T> + Copy {
528    type Output = Self;
529
530    // hamilton product
531    fn mul(self, rhs: Self) -> Self::Output {
532        // rhs = rhs.3 + rhs.0i+ rhs.1j + rhs.2k
533        // self = self.3 + self.0i + self.1j + self.2k
534        //
535        // Rules
536        // 1   i   j   k (second)
537        // i   -1  k   -j
538        // j   -k  -1  i  
539        // k    j  -i  -1
540        // (first) ex: ij = k
541        //
542        
543        let r = self.3 * rhs.3 - self.0 * rhs.0 - self.1 * rhs.1 - self.2 * rhs.2;
544        let i = self.3 * rhs.0 + self.0 * rhs.3 + self.1 * rhs.2 - self.2 * rhs.1;
545        let j = self.3 * rhs.1 - self.0 * rhs.2 + self.1 * rhs.3 + self.2 * rhs.0;
546        let k = self.3 * rhs.2 + self.0 * rhs.1 - self.1 * rhs.0 + self.2 * rhs.3;
547
548        Quaternion(i, j, k, r)
549        
550        // let v1: Vector3<T> = self.into();
551        // let v2: Vector3<T> = rhs.into();
552
553        // let r1 = self.3;
554        // let r2 = rhs.3;
555
556        // let vector_part: Vector3<T> = v2 * r1 + v1 * r2 + Vector3::cross_product(v1, v2);
557        // Quaternion(vector_part.0, vector_part.1, vector_part.2, r1 * r2 - Vector3::dot_product(v1, v2))
558    }
559}
560impl<T> Mul<T> for Quaternion<T> 
561where T: Mul<T, Output = T> + Copy {
562    type Output = Self;
563
564    fn mul(self, rhs: T) -> Self::Output {
565        Quaternion(self.0 * rhs, self.1 * rhs, self.2 * rhs, self.3 * rhs)
566    }
567}
568
569impl<T> Div for Quaternion<T>
570where T: Copy + Neg<Output = T> + Sub<T, Output = T> + Add<T, Output = T> + Div<T, Output = T> {
571    type Output = Self;
572
573    #[allow(clippy::many_single_char_names)]
574    fn div(self, rhs: Self) -> Self::Output {
575        // q1 = a + bi + cj + dk
576        // q2 = e + fi + gj + hk
577        
578        // q1/q2 = a/e + a/fi + a/gj + a/hk
579        //       + bi/e + bi/fi + bi/gj + bi/hk
580        //       + cj/e + cj/fi + cj/gj + cj/hk
581        //       + dk/e + dk/fi + dk/gj + dk/hk
582        
583        // q1/q2 = a/e + a/fi + a/gj + a/hk
584        //       + bi/e + b/f + bi/gj + bi/hk
585        //       + cj/e + cj/fi + c/g + cj/hk
586        //       + dk/e + dk/fi + dk/gj + d/h
587        
588        // q1/q2 = a/e + ai/fii + aj/gjj + ak/hkk
589        //       + bi/e + b/f + bij/gjj + bik/hkk
590        //       + cj/e + cji/fii + c/g + cjk/hkk
591        //       + dk/e + dki/fii + dkj/gjj + d/h
592        
593        // q1/q2 = a/e - ai/f - aj/g - ak/h
594        //       + bi/e + b/f - bij/g - bik/h
595        //       + cj/e - cji/f + c/g - cjk/h
596        //       + dk/e - dki/f - dkj/g + d/h
597        
598        // q1/q2 = a/e - ai/f - aj/g - ak/h
599        //       + bi/e + b/f - bk/g + bj/h
600        //       + cj/e + ck/f + c/g - ci/h
601        //       + dk/e - dj/f + di/g + d/h
602        
603        // q1/q2 = a/e - (a/f)i - (a/g)j - (a/h)k
604        //       + (b/e)i + b/f - (b/g)k + (b/h)j
605        //       + (c/e)j + (c/f)k + c/g - (c/h)i
606        //       + (d/e)k - (d/f)j + (d/g)i + d/h
607        
608        // q1/q2 = (a/e + b/f + c/g + d/h) 
609        //       + (-a/f + b/e - c/h + d/g)i
610        //       + (-a/g + b/h + c/e - d/f)j 
611        //       + (-a/h - b/g + c/f + d/e)k
612        
613        let Quaternion(a, b, c, d) = self;
614        let Quaternion(e, f, g, h) = rhs;
615
616        Quaternion(
617            -a/f + b/e - c/h + d/g,
618            -a/g + b/h + c/e - d/f,
619            -a/h - b/g + c/f + d/e,
620            a/e + b/f + c/g + d/h
621        )
622    }
623}
624impl<T> Div<T> for Quaternion<T> 
625where T: Div<T, Output = T> + Copy {
626    type Output = Self;
627
628    fn div(self, rhs: T) -> Self::Output {
629        Quaternion(self.0 / rhs, self.1 / rhs, self.2 / rhs, self.3 / rhs)
630    }
631}
632
633impl<T> Rem for Quaternion<T> 
634where T: Copy + Neg<Output = T> + Sub<T, Output = T> + Add<T, Output = T> + Rem<T, Output = T> {
635    type Output = Self;
636
637    /// TODO Test
638    #[allow(clippy::many_single_char_names)]
639    fn rem(self, rhs: Self) -> Self::Output {
640        // same as div
641
642        let Quaternion(a, b, c, d) = self;
643        let Quaternion(e, f, g, h) = rhs;
644
645        Quaternion(
646            -a%f + b%e - c%h + d%g,
647            -a%g + b%h + c%e - d%f,
648            -a%h - b%g + c%f + d%e,
649            a%e + b%f + c%g + d%h
650        )
651    }
652}
653
654impl<T> Rem<T> for Quaternion<T> 
655where T: Rem<T, Output = T> + Copy {
656    type Output = Self;
657
658    fn rem(self, rhs: T) -> Self::Output {
659        Quaternion(self.0 % rhs, self.1 % rhs, self.2 % rhs, self.3 % rhs)
660    }
661}
662
663impl<T> Neg for Quaternion<T> 
664where T: Neg<Output = T> + Copy {
665    type Output = Self;
666
667    fn neg(self) -> Self::Output {
668        Quaternion(-self.0, -self.1, -self.2, -self.3)
669    }
670}
671
672impl<T> SignOps for Quaternion<T> 
673where T: SignOps + Add<T, Output = T> {
674    fn ptcopysign(self, sign: Self) -> Self {
675        Quaternion(
676            self.0.ptcopysign(sign.0), 
677            self.1.ptcopysign(sign.1), 
678            self.2.ptcopysign(sign.2), 
679            self.3.ptcopysign(sign.3)
680        )
681    }
682    
683    // Does not really make sense
684    fn ptsignum(self) -> i8 {
685        (self.0 + self.1 + self.2 + self.3).ptsignum()
686    }
687
688    fn abs(self) -> Self {
689        Quaternion(
690            self.0.abs(), 
691            self.1.abs(), 
692            self.2.abs(), 
693            self.3.abs()
694        )
695    }
696}
feo_math/rotation/quaternion.rs

feo_math/rotation/
quaternion.rs
