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

maths_rs/
quat.rs
