nova_math/
quaternion.rs

1/*
2 *
3 * This file is a part of NovaEngine
4 * https://gitlab.com/MindSpunk/NovaEngine
5 *
6 *
7 * MIT License
8 *
9 * Copyright (c) 2018 Nathan Voglsam
10 *
11 * Permission is hereby granted, free of charge, to any person obtaining a copy
12 * of this software and associated documentation files (the "Software"), to deal
13 * in the Software without restriction, including without limitation the rights
14 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15 * copies of the Software, and to permit persons to whom the Software is
16 * furnished to do so, subject to the following conditions:
17 *
18 * The above copyright notice and this permission notice shall be included in all
19 * copies or substantial portions of the Software.
20 *
21 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27 * SOFTWARE.
28 */
29
30use core::ops::{Mul, MulAssign};
31
32use crate::matrix::TMat4x4;
33use crate::traits::CrossProduct;
34use crate::traits::DotProduct;
35use crate::traits::Inverse;
36use crate::traits::InverseAssign;
37use crate::traits::Length;
38use crate::traits::Normalize;
39use crate::traits::NormalizeAssign;
40use crate::traits::Real;
41use crate::vector::TVec3;
42
43#[repr(align(16))]
44#[derive(Copy, Clone, Debug)]
45pub struct TQuat<T: Real> {
46    data: [T; 4],
47}
48
49impl<T: Real> TQuat<T> {
50    ///
51    ///
52    ///
53    #[inline]
54    pub fn new(scalar: T, vector: TVec3<T>) -> Self {
55        let data = [vector[0], vector[1], vector[2], scalar];
56        TQuat { data }
57    }
58
59    ///
60    ///
61    ///
62    #[inline]
63    pub fn identity() -> Self {
64        Self::new(T::one(), [T::zero(), T::zero(), T::zero()].into())
65    }
66
67    ///
68    ///
69    ///
70    #[inline]
71    pub fn from_euler_angles(angles: TVec3<T>) -> Self {
72        let two = T::one() + T::one();
73        let half_angles: TVec3<T> = angles / two;
74        let sin_x = half_angles[0].sin();
75        let cos_x = half_angles[0].cos();
76        let sin_y = half_angles[1].sin();
77        let cos_y = half_angles[1].cos();
78        let sin_z = half_angles[2].sin();
79        let cos_z = half_angles[2].cos();
80        let vector = TVec3::<T>::new(
81            sin_x * cos_y * cos_z - cos_x * sin_y * sin_z,
82            cos_x * sin_y * cos_z + sin_x * cos_y * sin_z,
83            cos_x * cos_y * sin_z - sin_x * sin_y * cos_z,
84        );
85        let scalar = cos_x * cos_y * cos_z + sin_x * sin_y * sin_z;
86        Self::new(scalar, vector)
87    }
88
89    #[inline]
90    pub fn from_angle_axis(angle: T, axis: TVec3<T>) -> TQuat<T> {
91        let two = T::one() + T::one();
92
93        let half_angle: T = angle / two;
94
95        TQuat::<T>::new(half_angle.cos(), axis.normalize() * half_angle.sin())
96    }
97
98    ///
99    ///
100    ///
101    #[inline]
102    pub fn as_angle_axis(&self) -> (T, TVec3<T>) {
103        let axis = if self.scalar() > T::zero() {
104            self.vector()
105        } else {
106            -self.vector()
107        };
108        let axis = axis.normalize();
109        let two = T::one() + T::one();
110        let angle = two
111            * T::atan2(
112                axis.length(),
113                if self.scalar() > T::zero() {
114                    self.scalar()
115                } else {
116                    -self.scalar()
117                },
118            );
119        (angle, axis)
120    }
121
122    ///
123    ///
124    ///
125    pub fn conjugate(&self) -> TQuat<T> {
126        TQuat::<T>::new(self.scalar(), -self.vector())
127    }
128
129    ///
130    ///
131    ///
132    #[inline]
133    pub fn scalar(&self) -> T {
134        self.data[3]
135    }
136
137    ///
138    ///
139    ///
140    #[inline]
141    pub fn scalar_mut(&mut self) -> &mut T {
142        &mut self.data[3]
143    }
144
145    ///
146    ///
147    ///
148    #[inline]
149    pub fn vector(&self) -> TVec3<T> {
150        TVec3::<T>::new(self.data[0], self.data[1], self.data[2])
151    }
152}
153
154impl<T: Real> Mul<TQuat<T>> for TQuat<T> {
155    type Output = TQuat<T>;
156
157    #[inline]
158    fn mul(mut self, rhs: TQuat<T>) -> Self::Output {
159        self.mul_assign(rhs);
160        self
161    }
162}
163
164impl<T: Real> MulAssign<TQuat<T>> for TQuat<T> {
165    #[inline]
166    fn mul_assign(&mut self, rhs: TQuat<T>) {
167        let lhs_scalar = self.scalar();
168        let rhs_scalar = rhs.scalar();
169        let lhs_vector = self.vector();
170        let rhs_vector = rhs.vector();
171        self.data[3] = (rhs_scalar * lhs_scalar) - (lhs_vector.dot(&rhs_vector));
172        let vector =
173            (rhs_vector * lhs_scalar) + (lhs_vector * rhs_scalar) + lhs_vector.cross(&rhs_vector);
174        self.data[0] = vector[0];
175        self.data[1] = vector[1];
176        self.data[2] = vector[2];
177    }
178}
179
180impl<T: Real> Mul<TVec3<T>> for TQuat<T> {
181    type Output = TVec3<T>;
182
183    #[inline]
184    #[allow(clippy::suspicious_arithmetic_impl)]
185    fn mul(self, rhs: TVec3<T>) -> Self::Output {
186        let two = T::one() + T::one();
187        let t = self.vector().cross(&rhs) * two;
188        rhs + (t * self.scalar()) + self.vector().cross(&t)
189    }
190}
191
192impl<T: Real> Inverse for TQuat<T> {
193    #[inline]
194    fn inverse(mut self) -> Self {
195        self.inverse_assign();
196        self
197    }
198}
199
200impl<T: Real> InverseAssign for TQuat<T> {
201    #[inline]
202    fn inverse_assign(&mut self) {
203        self.data[0] = -self.data[0];
204        self.data[1] = -self.data[1];
205        self.data[2] = -self.data[2];
206    }
207}
208
209impl<T: Real> Normalize for TQuat<T> {
210    #[inline]
211    fn normalize(mut self) -> Self {
212        self.normalize_assign();
213        self
214    }
215}
216
217impl<T: Real> NormalizeAssign for TQuat<T> {
218    #[inline]
219    fn normalize_assign(&mut self) {
220        let len: T = (self.scalar() * self.scalar() + self.vector().dot(&self.vector())).sqrt();
221        let len: T = T::one() / len;
222        *self.scalar_mut() *= len;
223        let vector = self.vector() * len;
224        self.data[0] = vector[0];
225        self.data[1] = vector[1];
226        self.data[2] = vector[2];
227    }
228}
229
230impl<T: Real> Into<TMat4x4<T>> for TQuat<T> {
231    fn into(self) -> TMat4x4<T> {
232        let mut result = TMat4x4::<T>::identity();
233
234        let two = T::one() + T::one();
235
236        let qxx = self.data[0] * self.data[0];
237        let qyy = self.data[1] * self.data[1];
238        let qzz = self.data[2] * self.data[2];
239        let qxz = self.data[0] * self.data[2];
240        let qxy = self.data[0] * self.data[1];
241        let qyz = self.data[1] * self.data[2];
242        let qwx = self.data[3] * self.data[0];
243        let qwy = self.data[3] * self.data[1];
244        let qwz = self.data[3] * self.data[2];
245
246        result[(0, 0)] = T::one() - two * (qyy + qzz);
247        result[(1, 0)] = two * (qxy + qwz);
248        result[(2, 0)] = two * (qxz - qwy);
249
250        result[(0, 1)] = two * (qxy - qwz);
251        result[(1, 1)] = T::one() - two * (qxx + qzz);
252        result[(2, 1)] = two * (qyz + qwx);
253
254        result[(0, 2)] = two * (qxz + qwy);
255        result[(1, 2)] = two * (qyz - qwx);
256        result[(2, 2)] = T::one() - two * (qxx + qyy);
257
258        result
259    }
260}
nova_math/quaternion.rs

nova_math/
quaternion.rs
