maths_rs/
quat.rs

1use std::ops::Div;
2use std::ops::DivAssign;
3use std::ops::Mul;
4use std::ops::MulAssign;
5use std::ops::Neg;
6use std::ops::Add;
7use std::ops::AddAssign;
8use std::ops::Sub;
9use std::ops::SubAssign;
10use std::ops::Deref;
11use std::ops::DerefMut;
12
13use std::fmt::Display;
14use std::fmt::Formatter;
15
16use crate::num::*;
17use crate::vec::*;
18use crate::mat::*;
19use crate::swizz::*;
20use crate::dot;
21use crate::cross;
22
23#[cfg_attr(feature="serde", derive(serde::Serialize, serde::Deserialize))]
24#[cfg_attr(feature="hash", derive(Hash))]
25#[derive(Debug, Copy, Clone, PartialEq, Eq)]
26#[repr(C)]
27pub struct Quat<T> {
28    pub x: T,
29    pub y: T,
30    pub z: T,
31    pub w: T
32}
33
34impl<T> Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
35    /// constructs an identity quaternion which means no rotation and if multiplied with another quat it has no effect
36    pub fn identity() -> Self {
37        Self {
38            x: T::zero(),
39            y: T::zero(),
40            z: T::zero(),
41            w: T::one()
42        }
43    }
44
45    pub fn new(x: T, y: T, z: T, w: T) -> Self {
46        Self {
47            x,
48            y,
49            z,
50            w
51        }
52    }
53
54    /// construct a quaternion from 3 euler angles `x`, `y` and `z` in radians
55    pub fn from_euler_angles(x: T, y: T, z: T) -> Self {
56        let half_z = T::point_five() * z;
57        let half_x = T::point_five() * x;
58        let half_y = T::point_five() * y;
59
60        let cos_z_2 = T::cos(half_z);
61        let cos_y_2 = T::cos(half_y);
62        let cos_x_2 = T::cos(half_x);
63
64        let sin_z_2 = T::sin(half_z);
65        let sin_y_2 = T::sin(half_y);
66        let sin_x_2 = T::sin(half_x);
67
68        Self::normalize( Quat {
69            w: cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2,
70            x: cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2,
71            y: cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2,
72            z: sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2,
73        })
74    }
75
76    /// constructs quaternion from `axis` and `angle` representation where `angle` is in radians
77    pub fn from_axis_angle(axis: Vec3<T>, angle: T) -> Self {
78        let half_angle = angle * T::point_five();
79        Self::normalize( Quat {
80            w: T::cos(half_angle),
81            x: axis.x * T::sin(half_angle),
82            y: axis.y * T::sin(half_angle),
83            z: axis.z * T::sin(half_angle)
84        })
85    }
86
87    /// constructs quaternion from matrix `m`
88    pub fn from_matrix(m: Mat3<T>) -> Self {
89        // https://math.stackexchange.com/questions/893984/conversion-of-rotation-matrix-to-quaternion
90        let m00 = m.m[0];
91        let m01 = m.m[3];
92        let m02 = m.m[6];
93        let m10 = m.m[1];
94        let m11 = m.m[4];
95        let m12 = m.m[7];
96        let m20 = m.m[2];
97        let m21 = m.m[5];
98        let m22 = m.m[8];
99
100        let t0 = T::zero();
101        let t1 = T::one();
102
103        let (x, y, z, w, t) = if m22 < t0 {
104            if m00 > m11 {
105                let x = t1 + m00 -m11 -m22;
106                let y = m01 + m10;
107                let z = m20 + m02;
108                let w = m12 - m21;
109                (x, y, z, w, x)
110            }
111            else {
112                let x = m01 + m10;
113                let y = t1 -m00 + m11 -m22;
114                let z = m12 + m21;
115                let w = m20 - m02;
116                (x, y, z, w, y)
117
118            }
119        }
120        else if m00 < -m11 {
121            let x = m20+m02;
122            let y = m12+m21;
123            let z = t1 -m00 -m11 + m22;
124            let w = m01-m10;
125            (x, y, z, w, z)
126        }
127        else {
128            let x = m12-m21;
129            let y = m20-m02;
130            let z = m01-m10;
131            let w = t1 + m00 + m11 + m22;
132            (x, y, z, w, w)
133        };
134
135        let sq = T::point_five() / T::sqrt(t);
136
137        Quat {
138            x: x * sq,
139            y: y * sq,
140            z: z * sq,
141            w: w * sq
142        }
143    }
144
145    /// returns euler angles in radians as tuple `(x, y, z)` from quaternion
146    pub fn to_euler_angles(self) -> (T, T, T) {
147        let t2 = T::two();
148        let t1 = T::one();
149
150        // roll (x-axis rotation)
151        let sinr = t2 * (self.w * self.x + self.y * self.z);
152        let cosr = t1 - t2 * (self.x * self.x + self.y * self.y);
153        let x = T::atan2(sinr, cosr);
154
155        // pitch (y-axis rotation)
156        let sinp = t2 * (self.w * self.y - self.z * self.x);
157        let y = if T::abs(sinp) >= T::one() {
158            // use 90 degrees if out of range
159            T::copysign(T::pi() / t2, sinp)
160        }
161        else {
162            T::asin(sinp)
163        };
164
165        // yaw (z-axis rotation)
166        let siny = t2 * (self.w * self.z + self.x * self.y);
167        let cosy = t1 - t2 * (self.y * self.y + self.z * self.z);
168        let z = T::atan2(siny, cosy);
169
170        (x, y, z)
171    }
172
173    /// returns a 3x3 rotation matrix
174    pub fn get_matrix(self) -> Mat3<T> {
175        let t1 = T::one();
176        let t2 = T::two();
177        Mat3::new(
178            // row 1
179            t1 - t2 * self.y * self.y - t2 * self.z * self.z,
180            t2 * self.x * self.y - t2 * self.z * self.w,
181            t2 * self.x * self.z + t2 * self.y * self.w,
182            // row 2
183            t2 * self.x * self.y + t2 * self.z * self.w,
184            t1 - t2 * self.x * self.x - t2 * self.z * self.z,
185            t2 * self.y * self.z - t2 * self.x * self.w,
186            // row 3
187            t2 * self.x * self.z - t2 * self.y * self.w,
188            t2 * self.y * self.z + t2 * self.x * self.w,
189            t1 - t2 * self.x * self.x - t2 * self.y * self.y
190        )
191    }
192
193    /// returns a slice `T` of the quaternion
194    pub fn as_slice(&self) -> &[T] {
195        // SAFETY: The struct is #[repr(C)] with 4 fields of the same type T (x, y, z, w) laid
196        // out contiguously. `self.x` is the first field, so the pointer is valid for 4 reads
197        // of T and the lifetime is bounded by `&self`.
198        unsafe {
199            std::slice::from_raw_parts(&self.x, 4)
200        }
201    }
202
203    /// returns a mutable slice `T` of the quaternion
204    pub fn as_mut_slice(&mut self) -> &mut [T] {
205        // SAFETY: The struct is #[repr(C)] with 4 fields of the same type T (x, y, z, w) laid
206        // out contiguously. `self.x` is the first field, so the pointer is valid for 4 writes
207        // of T. Exclusive access is guaranteed by `&mut self`.
208        unsafe {
209            std::slice::from_raw_parts_mut(&mut self.x, 4)
210        }
211    }
212
213    /// returns a slice of `u8` bytes for the quaternion
214    pub fn as_u8_slice(&self) -> &[u8] {
215        // SAFETY: Any initialized memory can be viewed as bytes. The struct is #[repr(C)]
216        // and T: Float ensures all bytes are initialized. The cast to *const u8 is valid
217        // since u8 has alignment 1, and the length is the exact byte size of the struct.
218        unsafe {
219            std::slice::from_raw_parts((&self.x as *const T) as *const u8, std::mem::size_of::<Quat<T>>())
220        }
221    }
222
223    /// returns a quaternion which reverses the axis of rotation of self
224    pub fn reverse(self) -> Self {
225        Quat {
226            x: -self.x,
227            y: -self.y,
228            z: -self.z,
229            w: self.w
230        }
231    }
232
233    /// returns a quaternion which is the inverse of self
234    pub fn inverse(self) -> Self {
235        self.reverse() / Self::mag2(self)
236    }
237}
238
239impl<T> Add<Self> for Quat<T> where T: Float {
240    type Output = Self;
241    fn add(self, other: Self) -> Self {
242        Quat {
243            x: self.x + other.x,
244            y: self.y + other.y,
245            z: self.z + other.z,
246            w: self.w + other.w,
247        }
248    }
249}
250
251impl<T> AddAssign<Self> for Quat<T> where T: Float {
252    fn add_assign(&mut self, other: Self) {
253        self.x += other.x;
254        self.y += other.y;
255        self.z += other.z;
256        self.w += other.w;
257    }
258}
259
260impl<T> Sub<Self> for Quat<T> where T: Float {
261    type Output = Self;
262    fn sub(self, other: Self) -> Self {
263        Quat {
264            x: self.x - other.x,
265            y: self.y - other.y,
266            z: self.z - other.z,
267            w: self.w - other.w,
268        }
269    }
270}
271
272impl<T> SubAssign<Self> for Quat<T> where T: Float {
273    fn sub_assign(&mut self, other: Self) {
274        self.x -= other.x;
275        self.y -= other.y;
276        self.z -= other.z;
277        self.w -= other.w;
278    }
279}
280
281impl<T> Div<T> for Quat<T> where T: Number {
282    type Output = Self;
283    fn div(self, other: T) -> Self {
284        Quat {
285            x: self.x / other,
286            y: self.y / other,
287            z: self.z / other,
288            w: self.w / other,
289        }
290    }
291}
292
293impl<T> DivAssign<T> for Quat<T> where T: Number {
294    fn div_assign(&mut self, other: T) {
295        self.x /= other;
296        self.y /= other;
297        self.z /= other;
298        self.w /= other;
299    }
300}
301
302impl<T> Mul<T> for Quat<T> where T: Number {
303    type Output = Self;
304    fn mul(self, other: T) -> Self {
305        Quat {
306            x: self.x * other,
307            y: self.y * other,
308            z: self.z * other,
309            w: self.w * other,
310        }
311    }
312}
313
314impl<T> MulAssign<T> for Quat<T> where T: Number {
315    fn mul_assign(&mut self, other: T) {
316        self.x *= other;
317        self.y *= other;
318        self.z *= other;
319        self.w *= other;
320    }
321}
322
323// value * value
324impl<T> Mul<Self> for Quat<T> where T: Number {
325    type Output = Self;
326    fn mul(self, other: Self) -> Self {
327        Quat {
328            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
329            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
330            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
331            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
332        }
333    }
334}
335
336// ref * value
337impl<T> Mul<Quat<T>> for &Quat<T> where T: Number {
338    type Output = Quat<T>;
339    fn mul(self, other: Quat<T>) -> Quat<T> {
340        Quat {
341            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
342            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
343            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
344            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
345        }
346    }
347}
348
349// value * ref
350impl<T> Mul<&Self> for Quat<T> where T: Number {
351    type Output = Self;
352    fn mul(self, other: &Self) -> Self {
353        Quat {
354            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
355            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
356            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
357            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
358        }
359    }
360}
361
362// ref * ref
363impl<T> Mul<Self> for &Quat<T> where T: Number {
364    type Output = Quat<T>;
365    fn mul(self, other: Self) -> Quat<T> {
366        Quat {
367            w: self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z,
368            x: self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
369            y: self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
370            z: self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w
371        }
372    }
373}
374
375impl<T> MulAssign<Self> for Quat<T> where T: Number {
376    fn mul_assign(&mut self, other: Self) {
377        *self = Self::mul(*self, other);
378    }
379}
380
381impl<T> Neg for Quat<T> where T: Float {
382    type Output = Self;
383    fn neg(self) -> Self::Output {
384        Quat {
385            x: -self.x,
386            y: -self.y,
387            z: -self.z,
388            w: -self.w,
389        }
390    }
391}
392
393/// mul with vec3 assumes quat is normalized
394impl<T> Mul<Vec3<T>> for Quat<T> where T: Number {
395    type Output = Vec3<T>;
396    fn mul(self, other: Vec3<T>) -> Vec3<T> {
397        // https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
398        let u = Vec3::new(self.x, self.y, self.z);
399        let v = other;
400        let s = self.w;
401
402        let r0 = u * T::two() * dot(u, v);
403        let r1 = v * (s*s - dot(u, u));
404        let r2 = cross(u, v) * T::two() * s;
405
406        r0 + r1 + r2
407    }
408}
409
410// ref * value
411impl<T> Mul<Vec3<T>> for &Quat<T> where T: Number {
412    type Output = Vec3<T>;
413    fn mul(self, other: Vec3<T>) -> Vec3<T> {
414        (*self).mul(other)
415    }
416}
417
418// value * ref
419impl<T> Mul<&Vec3<T>> for Quat<T> where T: Number {
420    type Output = Vec3<T>;
421    fn mul(self, other: &Vec3<T>) -> Vec3<T> {
422        self.mul(*other)
423    }
424}
425
426// ref * ref
427impl<T> Mul<&Vec3<T>> for &Quat<T> where T: Number {
428    type Output = Vec3<T>;
429    fn mul(self, other: &Vec3<T>) -> Vec3<T> {
430        (*self).mul(*other)
431    }
432}
433
434/// default to identity quaternion
435impl<T> Default for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
436    fn default() -> Self {
437        Self::identity()
438    }
439}
440
441impl<T> Dot<T> for Quat<T> where T: Float + FloatOps<T> {
442    fn dot(a: Self, b: Self) -> T {
443        a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
444    }
445}
446
447impl<T> Magnitude<T> for Quat<T> where T: Float + FloatOps<T> {
448    fn length(a: Self) -> T {
449        T::sqrt(Self::dot(a, a))
450    }
451
452    fn mag(a: Self) -> T {
453        T::sqrt(Self::dot(a, a))
454    }
455
456    fn mag2(a: Self) -> T {
457        Self::dot(a, a)
458    }
459
460    fn normalize(a: Self) -> Self {
461        let m = Self::mag(a);
462        a / m
463    }
464}
465
466/// returns a quaternion spherically interpolated between `e0` and `e1` by percentage `t`
467impl<T> Slerp<T> for Quat<T> where T: Float + FloatOps<T> + NumberOps<T> + From<T> {
468    fn slerp(e0: Self, e1: Self, t: T) -> Self {
469        let q1 = e0;
470        // reverse the sign of e1 if q1.q2 < 0.
471        let q2 = if Self::dot(q1, e1) < T::zero() {
472            -e1
473        }
474        else {
475            e1
476        };
477
478        let theta = T::acos(Self::dot(q1, q2));
479        let k_epsilon = T::small_epsilon();
480        let (m1, m2) = if theta > k_epsilon {
481            (
482                T::sin((T::one() - t) * theta) / T::sin(theta),
483                T::sin(t * theta) / T::sin(theta)
484            )
485        }
486        else {
487            (
488                T::one() - t,
489                t
490            )
491        };
492
493        Quat {
494            w: m1 * q1.w + m2 * q2.w,
495            x: m1 * q1.x + m2 * q2.x,
496            y: m1 * q1.y + m2 * q2.y,
497            z: m1 * q1.z + m2 * q2.z
498        }
499    }
500}
501
502/// returns a quaternion linearly interpolated between `e0` and `e1` by percentage `t`
503impl<T> Lerp<T> for Quat<T> where T: Float + FloatOps<T> + NumberOps<T> {
504    fn lerp(e0: Self, e1: Self, t: T) -> Self {
505        e0 * (T::one() - t) + e1 * t
506    }
507}
508
509/// returns a quaternion linearly interpolated between `e0` and `e1` by percentage `t` normalising the return value
510impl<T> Nlerp<T> for Quat<T> where T: Float + FloatOps<T> + NumberOps<T> {
511    fn nlerp(e0: Self, e1: Self, t: T) -> Self {
512        Self::normalize(e0 * (T::one() - t) + e1 * t)
513    }
514}
515
516/// constructs from a `Vec3`, assuming the `Vec3` is poulated with euler angles `(x, y, z)` where the angles are in radians
517impl<T> From<Vec3<T>> for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
518    fn from(v: Vec3<T>) -> Self {
519        Quat::from_euler_angles(v.x, v.y, v.z)
520    }
521}
522
523/// constructs from a `Vec4`, assuming the `Vec4` is an `axis` - `angle` representation. `xyz = axis, w = radian angle`
524impl<T> From<Vec4<T>> for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
525    fn from(v: Vec4<T>) -> Self {
526        Quat::from_axis_angle(v.xyz(), v.w)
527    }
528}
529
530/// constructs a quaternion from a 3x3 rotation matrix
531impl<T> From<Mat3<T>> for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
532    fn from(m: Mat3<T>) -> Self {
533        Quat::from_matrix(m)
534    }
535}
536
537/// dereference to a slice of `T`
538impl<T> Deref for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
539    type Target = [T];
540    fn deref(&self) -> &[T] {
541        self.as_slice()
542    }
543}
544
545/// mutably dereference to a slice of `T`
546impl<T> DerefMut for Quat<T> where T: Float + FloatOps<T> + SignedNumberOps<T> {
547    fn deref_mut(&mut self) -> &mut [T] {
548        self.as_mut_slice()
549    }
550}
551
552/// displays like [1.0, 2.0, 3.0, 4.0]
553impl<T> Display for Quat<T> where T: Display {
554    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
555        write!(f, "[{}, {}, {}, {}]", self.x, self.y, self.z, self.w)
556    }
557}
maths_rs/quat.rs

maths_rs/
quat.rs
