diff --git a/gdnative-core/src/core_types/geom/basis.rs b/gdnative-core/src/core_types/geom/basis.rs index 73507f2a5..4d94d7eea 100644 --- a/gdnative-core/src/core_types/geom/basis.rs +++ b/gdnative-core/src/core_types/geom/basis.rs @@ -1,5 +1,6 @@ use crate::core_types::{IsEqualApprox, Quat, Vector3}; use core::ops::Mul; +use glam::Mat3; /// A 3x3 matrix. #[repr(C)] @@ -86,6 +87,18 @@ impl Basis { b } + /// Constructs a pure rotation basis matrix from the given quaternion. + #[inline] + pub fn from_quat(quat: Quat) -> Self { + let basis = Mat3::from_quat(quat.glam()).to_cols_array_2d(); + let basis = [ + Vector3::new(basis[0][0], basis[1][0], basis[2][0]), + Vector3::new(basis[0][1], basis[1][1], basis[2][1]), + Vector3::new(basis[0][2], basis[1][2], basis[2][2]), + ]; + Basis::from_elements(basis) + } + /// Rotation matrix from axis and angle. /// /// See @@ -286,7 +299,7 @@ impl Basis { /// /// If `self` is not normalized. #[inline] - pub fn to_quat(self) -> Quat { + pub(crate) fn to_quat(&self) -> Quat { // Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S, // and returns the Euler angles corresponding to the rotation part, complementing get_scale(). // See the comment in get_scale() for further information. @@ -709,7 +722,7 @@ mod tests { fn to_quat() { let (b, _bn) = test_inputs(); - assert!(Quat::new(-0.167156, 0.677813, -0.043058, 0.714685).is_equal_approx(&b.to_quat())); + assert!(Quat::new(-0.167156, 0.677813, -0.043058, 0.714685).is_equal_approx(b.to_quat())); } #[test] diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index c4e36ffb9..46bbaa203 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -1,4 +1,5 @@ -use super::IsEqualApprox; +use super::{Basis, IsEqualApprox, Vector3, CMP_EPSILON}; +use std::ops::{Mul, Neg}; #[derive(Copy, Clone, Debug, PartialEq)] #[repr(C)] @@ -9,22 +10,274 @@ pub struct Quat { pub w: f32, } +/// Helper methods for `Quat`. +/// +/// See the official [`Godot documentation`](https://docs.godotengine.org/en/stable/classes/class_quat.html). impl Quat { + /// The identity quaternion, representing no rotation. Equivalent to an identity [`Basis`] matrix. + /// If a vector is transformed by an identity quaternion, it will not change. + pub const IDENTITY: Self = Self::new(0.0, 0.0, 0.0, 1.0); + + /// Constructs a quaternion defined by the given values. #[inline] - pub fn new(x: f32, y: f32, z: f32, w: f32) -> Self { + pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self { Self { x, y, z, w } } + /// Constructs a quaternion from the given [Basis] + #[inline] + pub fn from_basis(basis: &Basis) -> Self { + basis.to_quat() + } + + /// Constructs a quaternion that will perform a rotation specified by Euler angles (in the YXZ + /// convention: when decomposing, first Z, then X, and Y last), given in the vector format as + /// (X angle, Y angle, Z angle). + #[inline] + pub fn from_euler(euler: Vector3) -> Self { + Self::gd(glam::Quat::from_rotation_ypr(euler.y, euler.x, euler.z)) + } + + /// Constructs a quaternion that will rotate around the given axis by the specified angle. The + /// axis must be a normalized vector. + #[inline] + pub fn from_axis_angle(axis: Vector3, angle: f32) -> Self { + debug_assert!(axis.is_normalized(), "Axis is not normalized"); + Self::gd(glam::Quat::from_axis_angle(axis.glam().into(), angle)) + } + + /// Performs a cubic spherical interpolation between quaternions `pre_a`, this quaternion, `b`, + /// and `post_b`, by the given amount `t`. + #[inline] + pub fn cubic_slerp(self, b: Self, pre_a: Self, post_b: Self, t: f32) -> Self { + let t2 = (1.0 - t) * t * 2.0; + let sp = self.slerp(b, t); + let sq = pre_a.slerpni(post_b, t); + sp.slerpni(sq, t2) + } + + /// Returns the dot product of two quaternions. + #[inline] + pub fn dot(self, b: Self) -> f32 { + self.glam().dot(b.glam()) + } + + /// Returns Euler angles (in the YXZ convention: when decomposing, first Z, then X, and Y last) + /// corresponding to the rotation represented by the unit quaternion. Returned vector contains + /// the rotation angles in the format (X angle, Y angle, Z angle). + #[inline] + pub fn to_euler(self) -> Vector3 { + Basis::from_quat(self).to_euler() + } + + /// Returns the inverse of the quaternion. + #[inline] + pub fn inverse(self) -> Self { + Self::gd(self.glam().inverse()) + } + + /// Returns `true` if this quaternion and `quat` are approximately equal, by running + /// `is_equal_approx` on each component #[inline] - pub fn is_equal_approx(self, to: &Self) -> bool { + pub fn is_equal_approx(self, to: Self) -> bool { self.x.is_equal_approx(to.x) && self.y.is_equal_approx(to.y) && self.z.is_equal_approx(to.z) && self.w.is_equal_approx(to.w) } + /// Returns whether the quaternion is normalized or not. + #[inline] + pub fn is_normalized(self) -> bool { + self.glam().is_normalized() + } + + /// Returns the length of the quaternion. + #[inline] + pub fn length(self) -> f32 { + self.glam().length() + } + + /// Returns the length of the quaternion, squared. + #[inline] + pub fn length_squared(self) -> f32 { + self.glam().length_squared() + } + + /// Returns a copy of the quaternion, normalized to unit length. #[inline] - fn glam(self) -> glam::Quat { + pub fn normalized(self) -> Self { + Self::gd(self.glam().normalize()) + } + + /// Returns the result of the spherical linear interpolation between this quaternion and to by + /// amount weight. + /// + /// **Note:** Both quaternions must be normalized. + #[inline] + pub fn slerp(self, b: Self, t: f32) -> Self { + debug_assert!(self.is_normalized(), "Quaternion `self` is not normalized"); + debug_assert!(b.is_normalized(), "Quaternion `b` is not normalized"); + + // Copied from Godot's Quat::slerp as glam::lerp version diverges too much + + // calc cosine + let cosom = self.dot(b); + + // adjust signs (if necessary) + let (cosom, b) = if cosom < 0.0 { + (-cosom, -b) + } else { + (cosom, b) + }; + + // calculate coefficients + let scale = if (1.0 - cosom) > CMP_EPSILON as f32 { + // standard case (slerp) + let omega = cosom.acos(); + let sinom = omega.sin(); + (((1.0 - t) * omega).sin() / sinom, (t * omega).sin() / sinom) + } else { + // "from" and "to" quaternions are very close + // ... so we can do a linear interpolation + (1.0 - t, t) + }; + + // calculate final values + Self::new( + scale.0 * self.x + scale.1 * b.x, + scale.0 * self.y + scale.1 * b.y, + scale.0 * self.z + scale.1 * b.z, + scale.0 * self.w + scale.1 * b.w, + ) + } + + /// Returns the result of the spherical linear interpolation between this quaternion and `t` by + /// amount `t`, but without checking if the rotation path is not bigger than 90 degrees. + #[inline] + pub fn slerpni(self, b: Self, t: f32) -> Self { + debug_assert!(self.is_normalized(), "Quaternion `self` is not normalized"); + debug_assert!(b.is_normalized(), "Quaternion `b` is not normalized"); + + // Copied from Godot's Quat::slerpni as glam::slerp version diverges too much + + let dot = self.dot(b); + if dot.abs() > 0.9999 { + self + } else { + let theta = dot.acos(); + let sin_t = 1.0 / theta.sin(); + let new_f = (t * theta).sin() * sin_t; + let inv_f = ((1.0 - t) * theta).sin() * sin_t; + Self::new( + inv_f * self.x + new_f * b.x, + inv_f * self.y + new_f * b.y, + inv_f * self.z + new_f * b.z, + inv_f * self.w + new_f * b.w, + ) + } + } + + /// Returns a vector transformed (multiplied) by this quaternion. + #[inline] + pub fn xform(self, v: Vector3) -> Vector3 { + self * v + } + + #[inline] + pub(super) fn gd(quat: glam::Quat) -> Self { + Self::new(quat.x, quat.y, quat.z, quat.w) + } + + #[inline] + pub(super) fn glam(self) -> glam::Quat { glam::Quat::from_xyzw(self.x, self.y, self.z, self.w) } } + +impl Mul for Quat { + type Output = Vector3; + + #[inline] + fn mul(self, with: Vector3) -> Vector3 { + debug_assert!(self.is_normalized(), "Quaternion is not normalized"); + Vector3::gd(self.glam() * with.glam()) + } +} + +impl Neg for Quat { + type Output = Quat; + + #[inline] + fn neg(self) -> Self { + Self::gd(-self.glam()) + } +} + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn from_euler() { + let euler = Vector3::new(0.25, 5.24, 3.0); + let expect = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + assert!(Quat::from_euler(euler).is_equal_approx(expect)); + } + + #[test] + fn to_basis() { + let quat = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let expect = Basis::from_elements([ + Vector3::new(-0.528598, 0.140572, -0.837152), + Vector3::new(0.136732, -0.959216, -0.247404), + Vector3::new(-0.837788, -0.245243, 0.487819), + ]); + let basis = Basis::from_quat(quat); + assert!(basis.is_equal_approx(&expect)); + } + + #[test] + fn to_euler() { + let quat = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let expect = Vector3::new(0.25, -1.043185, 3.00001); + assert!(quat.to_euler().is_equal_approx(expect)); + } + + #[test] + fn mul() { + let quat = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let vec = Vector3::new(2.2, 0.8, 1.65); + let expect = Vector3::new(-2.43176, -0.874777, -1.234427); + assert!(expect.is_equal_approx(quat * vec)); + } + + #[test] + fn slerp() { + let q = Quat::new(-0.635115, -0.705592, 0.314052, 0.011812); + let p = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let t = 0.2; + let e = Quat::new(-0.638517, -0.620742, 0.454844, 0.009609); + assert!(e.is_equal_approx(q.slerp(p, t))); + } + + #[test] + fn slerpni() { + let q = Quat::new(-0.635115, -0.705592, 0.314052, 0.011812); + let p = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let t = 0.2; + let e = Quat::new(-0.535331, -0.836627, -0.114954, 0.016143); + assert!(e.is_equal_approx(q.slerpni(p, t))); + } + + #[test] + fn cubic_slerp() { + let a = Quat::new(-0.635115, -0.705592, 0.314052, 0.011812); + let b = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let c = Quat::new(-0.666276, 0.03859, 0.083527, -0.740007); + let d = Quat::new(-0.856633, -0.430228, -0.284017, 0.020464); + let t = 0.2; + let e = Quat::new(-0.768253, -0.490687, 0.341836, -0.22839); + assert!(e.is_equal_approx(a.cubic_slerp(b, c, d, t))); + } +} diff --git a/gdnative-core/src/core_types/vector3.rs b/gdnative-core/src/core_types/vector3.rs index 578abbfc9..3b4fe35f0 100644 --- a/gdnative-core/src/core_types/vector3.rs +++ b/gdnative-core/src/core_types/vector3.rs @@ -380,12 +380,12 @@ impl Vector3 { } #[inline] - fn glam(self) -> Vec3A { + pub(super) fn glam(self) -> Vec3A { Vec3A::new(self.x, self.y, self.z) } #[inline] - fn gd(from: Vec3A) -> Self { + pub(super) fn gd(from: Vec3A) -> Self { Self::new(from.x, from.y, from.z) } }