godot_core/builtin/
quaternion.rs

1/*
2 * Copyright (c) godot-rust; Bromeon and contributors.
3 * This Source Code Form is subject to the terms of the Mozilla Public
4 * License, v. 2.0. If a copy of the MPL was not distributed with this
5 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
6 */
7
8use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
9
10use godot_ffi as sys;
11use sys::{ffi_methods, ExtVariantType, GodotFfi};
12
13use crate::builtin::math::{ApproxEq, FloatExt, GlamConv, GlamType};
14use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, RealConv, Vector3};
15
16/// Unit quaternion to represent 3D rotations.
17///
18/// See also [`Quaternion`](https://docs.godotengine.org/en/stable/classes/class_quaternion.html) in the Godot documentation.
19///
20/// # Godot docs
21///
22/// [`Quaternion` (stable)](https://docs.godotengine.org/en/stable/classes/class_quaternion.html)
23#[derive(Copy, Clone, PartialEq, Debug)]
24#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
25#[repr(C)]
26pub struct Quaternion {
27    pub x: real,
28    pub y: real,
29    pub z: real,
30    pub w: real,
31}
32
33impl Quaternion {
34    /// The identity quaternion, representing no rotation. This has the same rotation as [`Basis::IDENTITY`].
35    ///
36    /// If a [`Vector3`] is rotated (multiplied) by this quaternion, it does not change.
37    pub const IDENTITY: Self = Self {
38        x: 0.0,
39        y: 0.0,
40        z: 0.0,
41        w: 1.0,
42    };
43
44    pub fn new(x: real, y: real, z: real, w: real) -> Self {
45        Self { x, y, z, w }
46    }
47
48    /// Creates a quaternion from a Vector3 and an angle.
49    ///
50    /// # Panics
51    /// If the vector3 is not normalized.
52    pub fn from_axis_angle(axis: Vector3, angle: real) -> Self {
53        assert!(
54            axis.is_normalized(),
55            "Quaternion axis {axis:?} is not normalized."
56        );
57        let d = axis.length();
58        let sin_angle = (angle * 0.5).sin();
59        let cos_angle = (angle * 0.5).cos();
60        let s = sin_angle / d;
61        let x = axis.x * s;
62        let y = axis.y * s;
63        let z = axis.z * s;
64        let w = cos_angle;
65        Self::new(x, y, z, w)
66    }
67
68    /// Constructs a Quaternion representing the shortest arc between `arc_from` and `arc_to`.
69    ///
70    /// These can be imagined as two points intersecting a unit sphere's surface, with a radius of 1.0.
71    ///
72    // This is an undocumented assumption of Godot's as well.
73    /// The inputs must be unit vectors.
74    ///
75    /// For near-singular cases (`arc_from`≈`arc_to` or `arc_from`≈-`arc_to`) the current implementation is only accurate to about
76    /// 0.001, or better if `double-precision` is enabled.
77    ///
78    /// *Godot equivalent: `Quaternion(arc_from: Vector3, arc_to: Vector3)`*
79    pub fn from_rotation_arc(arc_from: Vector3, arc_to: Vector3) -> Self {
80        debug_assert!(
81            arc_from.is_normalized(),
82            "input 1 (`arc_from`) in `Quaternion::from_rotation_arc` must be a unit vector"
83        );
84        debug_assert!(
85            arc_to.is_normalized(),
86            "input 2 (`arc_to`) in `Quaternion::from_rotation_arc` must be a unit vector"
87        );
88        <Self as GlamConv>::Glam::from_rotation_arc(arc_from.to_glam(), arc_to.to_glam()).to_front()
89    }
90
91    pub fn angle_to(self, to: Self) -> real {
92        self.glam2(&to, RQuat::angle_between)
93    }
94
95    pub fn dot(self, with: Self) -> real {
96        self.glam2(&with, RQuat::dot)
97    }
98
99    pub fn exp(self) -> Self {
100        let mut v = Vector3::new(self.x, self.y, self.z);
101        let theta = v.length();
102        v = v.normalized();
103
104        if theta < real::CMP_EPSILON || !v.is_normalized() {
105            Self::default()
106        } else {
107            Self::from_axis_angle(v, theta)
108        }
109    }
110
111    pub fn from_euler(euler: Vector3) -> Self {
112        let half_a1 = euler.y * 0.5;
113        let half_a2 = euler.x * 0.5;
114        let half_a3 = euler.z * 0.5;
115        let cos_a1 = half_a1.cos();
116        let sin_a1 = half_a1.sin();
117        let cos_a2 = half_a2.cos();
118        let sin_a2 = half_a2.sin();
119        let cos_a3 = half_a3.cos();
120        let sin_a3 = half_a3.sin();
121
122        Self::new(
123            sin_a1 * cos_a2 * sin_a3 + cos_a1 * sin_a2 * cos_a3,
124            sin_a1 * cos_a2 * cos_a3 - cos_a1 * sin_a2 * sin_a3,
125            -sin_a1 * sin_a2 * cos_a3 + cos_a1 * cos_a2 * sin_a3,
126            sin_a1 * sin_a2 * sin_a3 + cos_a1 * cos_a2 * cos_a3,
127        )
128    }
129
130    pub fn get_angle(self) -> real {
131        2.0 * self.w.acos()
132    }
133
134    pub fn get_axis(self) -> Vector3 {
135        let Self { x, y, z, w } = self;
136        let axis = Vector3::new(x, y, z);
137
138        if self.w.abs() > 1.0 - real::CMP_EPSILON {
139            axis
140        } else {
141            let r = 1.0 / (1.0 - w * w).sqrt();
142            r * axis
143        }
144    }
145
146    /// Returns the rotation of the matrix in euler angles, with the order `YXZ`.
147    ///
148    /// See [`get_euler_with()`](Self::get_euler_with) for custom angle orders.
149    pub fn get_euler(self) -> Vector3 {
150        self.get_euler_with(EulerOrder::YXZ)
151    }
152
153    /// Returns the rotation of the matrix in euler angles.
154    ///
155    /// The order of the angles are given by `order`. To use the default order `YXZ`, see [`get_euler()`](Self::get_euler).
156    ///
157    /// _Godot equivalent: `Quaternion.get_euler()`_
158    pub fn get_euler_with(self, order: EulerOrder) -> Vector3 {
159        Basis::from_quaternion(self).get_euler_with(order)
160    }
161
162    pub fn inverse(self) -> Self {
163        Self::new(-self.x, -self.y, -self.z, self.w)
164    }
165
166    pub fn is_finite(self) -> bool {
167        self.x.is_finite() && self.y.is_finite() && self.z.is_finite() && self.w.is_finite()
168    }
169
170    pub fn is_normalized(self) -> bool {
171        self.length_squared().approx_eq(&1.0)
172    }
173
174    pub fn length(self) -> real {
175        self.length_squared().sqrt()
176    }
177
178    pub fn length_squared(self) -> real {
179        self.dot(self)
180    }
181
182    pub fn log(self) -> Self {
183        let v = self.get_axis() * self.get_angle();
184        Quaternion::new(v.x, v.y, v.z, 0.0)
185    }
186
187    /// # Panics
188    /// If the quaternion has length of 0.
189    pub fn normalized(self) -> Self {
190        let length = self.length();
191        assert!(!length.approx_eq(&0.0), "Quaternion has length 0");
192        self / length
193    }
194
195    /// # Panics
196    /// If either quaternion is not normalized.
197    pub fn slerp(self, to: Self, weight: real) -> Self {
198        let normalized_inputs = self.ensure_normalized(&[&to]);
199        assert!(normalized_inputs, "Slerp requires normalized quaternions");
200
201        self.as_inner().slerp(to, weight.as_f64())
202    }
203
204    /// # Panics
205    /// If either quaternion is not normalized.
206    pub fn slerpni(self, to: Self, weight: real) -> Self {
207        let normalized_inputs = self.ensure_normalized(&[&to]);
208        assert!(normalized_inputs, "Slerpni requires normalized quaternions");
209
210        self.as_inner().slerpni(to, weight.as_f64())
211    }
212
213    /// # Panics
214    /// If any quaternions are not normalized.
215    pub fn spherical_cubic_interpolate(
216        self,
217        b: Self,
218        pre_a: Self,
219        post_b: Self,
220        weight: real,
221    ) -> Self {
222        let normalized_inputs = self.ensure_normalized(&[&b, &pre_a, &post_b]);
223        assert!(
224            normalized_inputs,
225            "Spherical cubic interpolation requires normalized quaternions"
226        );
227
228        self.as_inner()
229            .spherical_cubic_interpolate(b, pre_a, post_b, weight.as_f64())
230    }
231
232    /// # Panics
233    /// If any quaternions are not normalized.
234    #[allow(clippy::too_many_arguments)]
235    pub fn spherical_cubic_interpolate_in_time(
236        self,
237        b: Self,
238        pre_a: Self,
239        post_b: Self,
240        weight: real,
241        b_t: real,
242        pre_a_t: real,
243        post_b_t: real,
244    ) -> Self {
245        let normalized_inputs = self.ensure_normalized(&[&b, &pre_a, &post_b]);
246        assert!(
247            normalized_inputs,
248            "Spherical cubic interpolation in time requires normalized quaternions"
249        );
250
251        self.as_inner().spherical_cubic_interpolate_in_time(
252            b,
253            pre_a,
254            post_b,
255            weight.as_f64(),
256            b_t.as_f64(),
257            pre_a_t.as_f64(),
258            post_b_t.as_f64(),
259        )
260    }
261
262    #[doc(hidden)]
263    pub fn as_inner(&self) -> inner::InnerQuaternion<'_> {
264        inner::InnerQuaternion::from_outer(self)
265    }
266
267    #[doc(hidden)]
268    fn ensure_normalized(&self, quats: &[&Quaternion]) -> bool {
269        quats.iter().all(|v| v.is_normalized()) && self.is_normalized()
270    }
271}
272
273impl Add for Quaternion {
274    type Output = Self;
275
276    fn add(self, other: Self) -> Self {
277        Self::new(
278            self.x + other.x,
279            self.y + other.y,
280            self.z + other.z,
281            self.w + other.w,
282        )
283    }
284}
285
286impl AddAssign for Quaternion {
287    fn add_assign(&mut self, other: Self) {
288        *self = *self + other
289    }
290}
291
292impl Sub for Quaternion {
293    type Output = Self;
294
295    fn sub(self, other: Self) -> Self {
296        Self::new(
297            self.x - other.x,
298            self.y - other.y,
299            self.z - other.z,
300            self.w - other.w,
301        )
302    }
303}
304
305impl SubAssign for Quaternion {
306    fn sub_assign(&mut self, other: Self) {
307        *self = *self - other
308    }
309}
310
311impl Mul<Quaternion> for Quaternion {
312    type Output = Self;
313
314    fn mul(self, other: Quaternion) -> Self {
315        // TODO use super::glam?
316
317        let x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y;
318        let y = self.w * other.y + self.y * other.w + self.z * other.x - self.x * other.z;
319        let z = self.w * other.z + self.z * other.w + self.x * other.y - self.y * other.x;
320        let w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z;
321
322        Self::new(x, y, z, w)
323    }
324}
325
326impl Mul<Vector3> for Quaternion {
327    type Output = Vector3;
328
329    /// Applies the quaternion's rotation to the 3D point represented by the vector.
330    ///
331    /// # Panics
332    /// If the quaternion is not normalized.
333    fn mul(self, rhs: Vector3) -> Self::Output {
334        Vector3::from_glam(self.to_glam().mul_vec3(rhs.to_glam()))
335    }
336}
337
338// SAFETY:
339// This type is represented as `Self` in Godot, so `*mut Self` is sound.
340unsafe impl GodotFfi for Quaternion {
341    const VARIANT_TYPE: ExtVariantType = ExtVariantType::Concrete(sys::VariantType::QUATERNION);
342
343    ffi_methods! { type sys::GDExtensionTypePtr = *mut Self; .. }
344}
345
346crate::meta::impl_godot_as_self!(Quaternion: ByValue);
347
348impl std::fmt::Display for Quaternion {
349    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
350        self.to_glam().fmt(f)
351    }
352}
353
354impl Default for Quaternion {
355    fn default() -> Self {
356        Self::new(0.0, 0.0, 0.0, 1.0)
357    }
358}
359
360impl ApproxEq for Quaternion {
361    fn approx_eq(&self, other: &Self) -> bool {
362        self.x.approx_eq(&other.x)
363            && self.y.approx_eq(&other.y)
364            && self.z.approx_eq(&other.z)
365            && self.w.approx_eq(&other.w)
366    }
367}
368
369impl GlamConv for Quaternion {
370    type Glam = RQuat;
371}
372
373impl GlamType for RQuat {
374    type Mapped = Quaternion;
375
376    fn to_front(&self) -> Self::Mapped {
377        Quaternion::new(self.x, self.y, self.z, self.w)
378    }
379
380    fn from_front(mapped: &Self::Mapped) -> Self {
381        RQuat::from_xyzw(mapped.x, mapped.y, mapped.z, mapped.w)
382    }
383}
384
385impl MulAssign<Quaternion> for Quaternion {
386    fn mul_assign(&mut self, other: Quaternion) {
387        *self = *self * other
388    }
389}
390
391impl Mul<real> for Quaternion {
392    type Output = Self;
393
394    fn mul(self, other: real) -> Self {
395        Quaternion::new(
396            self.x * other,
397            self.y * other,
398            self.z * other,
399            self.w * other,
400        )
401    }
402}
403
404impl Mul<Quaternion> for real {
405    type Output = Quaternion;
406
407    fn mul(self, other: Quaternion) -> Quaternion {
408        other * self
409    }
410}
411
412impl MulAssign<real> for Quaternion {
413    fn mul_assign(&mut self, other: real) {
414        *self = *self * other
415    }
416}
417
418impl Div<real> for Quaternion {
419    type Output = Self;
420
421    fn div(self, other: real) -> Self {
422        Self::new(
423            self.x / other,
424            self.y / other,
425            self.z / other,
426            self.w / other,
427        )
428    }
429}
430
431impl DivAssign<real> for Quaternion {
432    fn div_assign(&mut self, other: real) {
433        *self = *self / other
434    }
435}
436
437impl Neg for Quaternion {
438    type Output = Self;
439
440    fn neg(self) -> Self {
441        Self::new(-self.x, -self.y, -self.z, -self.w)
442    }
443}
444
445#[cfg(test)] #[cfg_attr(published_docs, doc(cfg(test)))]
446mod test {
447    #[cfg(feature = "serde")] #[cfg_attr(published_docs, doc(cfg(feature = "serde")))]
448    #[test]
449    fn serde_roundtrip() {
450        let quaternion = super::Quaternion::new(1.0, 1.0, 1.0, 1.0);
451        let expected_json = "{\"x\":1.0,\"y\":1.0,\"z\":1.0,\"w\":1.0}";
452
453        crate::builtin::test_utils::roundtrip(&quaternion, expected_json);
454    }
455}
godot_core/builtin/quaternion.rs

godot_core/builtin/
quaternion.rs
