From 90bdc5e690260a261680292b3ea78dca1fe3db73 Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Sun, 4 Apr 2021 15:58:23 +0200 Subject: [PATCH 1/6] Implement Godot methods for Quat There are still some tests lacking & `get_euler` has a large error for some reason. --- gdnative-core/src/core_types/geom/basis.rs | 2 +- gdnative-core/src/core_types/quat.rs | 235 ++++++++++++++++++++- gdnative-core/src/core_types/vector3.rs | 4 +- 3 files changed, 235 insertions(+), 6 deletions(-) diff --git a/gdnative-core/src/core_types/geom/basis.rs b/gdnative-core/src/core_types/geom/basis.rs index d053dc0b3..a66cdb0e8 100644 --- a/gdnative-core/src/core_types/geom/basis.rs +++ b/gdnative-core/src/core_types/geom/basis.rs @@ -709,7 +709,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..c994c6db8 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -1,4 +1,7 @@ -use super::IsEqualApprox; +use super::{Basis, IsEqualApprox, Vector3, CMP_EPSILON}; +use core::f32::consts::FRAC_PI_2; +use core::ops::Mul; +use glam::Mat3; #[derive(Copy, Clone, Debug, PartialEq)] #[repr(C)] @@ -9,22 +12,248 @@ 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 { + 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 get_euler(self) -> Vector3 { + let basis = Mat3::from_quat(self.glam()); + let elm = basis.to_cols_array_2d(); + // Copied from Basis::get_euler_yxz + let m12 = elm[1][2]; + if m12 < 1.0 - CMP_EPSILON as f32 { + if m12 > CMP_EPSILON as f32 - 1.0 { + #[allow(clippy::float_cmp)] // Godot also used direct comparison + if elm[1][0] == 0.0 + && elm[0][1] == 0.0 + && elm[0][2] == 0.0 + && elm[2][0] == 0.0 + && elm[0][0] == 1.0 + { + Vector3::new((-m12).atan2(elm[1][1]), 0.0, 0.0) + } else { + Vector3::new( + (-m12).asin(), + elm[0][2].atan2(elm[2][2]), + elm[1][0].atan2(elm[1][1]), + ) + } + } else { + Vector3::new(FRAC_PI_2, elm[0][1].atan2(elm[0][0]), 0.0) + } + } else { + Vector3::new(-FRAC_PI_2, (-elm[0][1]).atan2(elm[0][0]), 0.0) + } + } + + /// 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] + pub fn normalized(self) -> Self { + Self::gd(self.glam().normalize()) + } + + /// Sets the quaternion to a rotation which rotates around axis by the specified angle, in + /// radians. The axis must be a normalized vector. + #[inline] + pub fn set_axis_angle(&mut self, axis: Vector3, angle: f32) { + *self = Self::from_axis_angle(axis, angle) + } + + /// Sets the quaternion to 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 set_euler(&mut self, euler: Vector3) { + *self = Self::from_euler(euler); + } + + /// 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"); + Self::gd(self.glam().slerp(b.glam(), t)) + } + + /// 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"); + 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] + fn gd(quat: glam::Quat) -> Self { + Self::new(quat.x, quat.y, quat.z, quat.w) + } + #[inline] 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()) + } +} + +#[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 = 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]), + ]; + let basis = Basis::from_elements(basis); + assert!(basis.is_equal_approx(&expect)); + } + + #[test] + fn get_euler() { + let quat = Quat::new(0.485489, 0.142796, -0.862501, 0.001113); + let expect = Vector3::new(0.25, -1.043185, 3.00001); + dbg!(quat.get_euler()); + assert!(quat.get_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)); + } +} 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) } } From cf0da51220019ce54fbe348342db7fe318ea1762 Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Sun, 4 Apr 2021 17:57:25 +0200 Subject: [PATCH 2/6] Incorporate feedback * s/core::/std:: * Add documentation to IDENTITY constant * s/get_euler/to_euler * Use Basis::to_euler for to_euler instead. Incidentally, this fixes the precision issue in to_the euler test. * Remove set_axis_angle and set_euler * Make Basis::to_quat private. The implementation could perhaps be moved to Quat::from_basis instead? --- gdnative-core/src/core_types/geom/basis.rs | 2 +- gdnative-core/src/core_types/quat.rs | 62 +++++----------------- 2 files changed, 15 insertions(+), 49 deletions(-) diff --git a/gdnative-core/src/core_types/geom/basis.rs b/gdnative-core/src/core_types/geom/basis.rs index a66cdb0e8..85451ac8f 100644 --- a/gdnative-core/src/core_types/geom/basis.rs +++ b/gdnative-core/src/core_types/geom/basis.rs @@ -286,7 +286,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. diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index c994c6db8..be0e0136c 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -1,7 +1,6 @@ -use super::{Basis, IsEqualApprox, Vector3, CMP_EPSILON}; -use core::f32::consts::FRAC_PI_2; -use core::ops::Mul; +use super::{Basis, IsEqualApprox, Vector3}; use glam::Mat3; +use std::ops::Mul; #[derive(Copy, Clone, Debug, PartialEq)] #[repr(C)] @@ -16,6 +15,8 @@ pub struct 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. @@ -66,34 +67,15 @@ impl Quat { /// 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 get_euler(self) -> Vector3 { + pub fn to_euler(self) -> Vector3 { let basis = Mat3::from_quat(self.glam()); - let elm = basis.to_cols_array_2d(); - // Copied from Basis::get_euler_yxz - let m12 = elm[1][2]; - if m12 < 1.0 - CMP_EPSILON as f32 { - if m12 > CMP_EPSILON as f32 - 1.0 { - #[allow(clippy::float_cmp)] // Godot also used direct comparison - if elm[1][0] == 0.0 - && elm[0][1] == 0.0 - && elm[0][2] == 0.0 - && elm[2][0] == 0.0 - && elm[0][0] == 1.0 - { - Vector3::new((-m12).atan2(elm[1][1]), 0.0, 0.0) - } else { - Vector3::new( - (-m12).asin(), - elm[0][2].atan2(elm[2][2]), - elm[1][0].atan2(elm[1][1]), - ) - } - } else { - Vector3::new(FRAC_PI_2, elm[0][1].atan2(elm[0][0]), 0.0) - } - } else { - Vector3::new(-FRAC_PI_2, (-elm[0][1]).atan2(elm[0][0]), 0.0) - } + let basis = basis.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).to_euler() } /// Returns the inverse of the quaternion. @@ -136,21 +118,6 @@ impl Quat { Self::gd(self.glam().normalize()) } - /// Sets the quaternion to a rotation which rotates around axis by the specified angle, in - /// radians. The axis must be a normalized vector. - #[inline] - pub fn set_axis_angle(&mut self, axis: Vector3, angle: f32) { - *self = Self::from_axis_angle(axis, angle) - } - - /// Sets the quaternion to 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 set_euler(&mut self, euler: Vector3) { - *self = Self::from_euler(euler); - } - /// Returns the result of the spherical linear interpolation between this quaternion and to by /// amount weight. /// @@ -242,11 +209,10 @@ mod test { } #[test] - fn get_euler() { + 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); - dbg!(quat.get_euler()); - assert!(quat.get_euler().is_equal_approx(expect)); + assert!(quat.to_euler().is_equal_approx(expect)); } #[test] From 2ca1cb783576b3ab369e0df570f2d56ffe882bbf Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Sun, 4 Apr 2021 18:04:56 +0200 Subject: [PATCH 3/6] Add Basis::from_quat --- gdnative-core/src/core_types/geom/basis.rs | 13 +++++++++++++ gdnative-core/src/core_types/quat.rs | 22 ++++------------------ 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/gdnative-core/src/core_types/geom/basis.rs b/gdnative-core/src/core_types/geom/basis.rs index 85451ac8f..a642adfdd 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 diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index be0e0136c..da850baf9 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -1,5 +1,4 @@ use super::{Basis, IsEqualApprox, Vector3}; -use glam::Mat3; use std::ops::Mul; #[derive(Copy, Clone, Debug, PartialEq)] @@ -68,14 +67,7 @@ impl Quat { /// the rotation angles in the format (X angle, Y angle, Z angle). #[inline] pub fn to_euler(self) -> Vector3 { - let basis = Mat3::from_quat(self.glam()); - let basis = basis.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).to_euler() + Basis::from_quat(self).to_euler() } /// Returns the inverse of the quaternion. @@ -159,12 +151,12 @@ impl Quat { } #[inline] - fn gd(quat: glam::Quat) -> Self { + pub(super) fn gd(quat: glam::Quat) -> Self { Self::new(quat.x, quat.y, quat.z, quat.w) } #[inline] - fn glam(self) -> glam::Quat { + pub(super) fn glam(self) -> glam::Quat { glam::Quat::from_xyzw(self.x, self.y, self.z, self.w) } } @@ -198,13 +190,7 @@ mod test { Vector3::new(0.136732, -0.959216, -0.247404), Vector3::new(-0.837788, -0.245243, 0.487819), ]); - 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]), - ]; - let basis = Basis::from_elements(basis); + let basis = Basis::from_quat(quat); assert!(basis.is_equal_approx(&expect)); } From 703eab3290f2b879688672de70a45276047d2fa5 Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Sun, 4 Apr 2021 18:42:53 +0200 Subject: [PATCH 4/6] Add `slerp` and `slerpni` tests The slerp test fails because the result diverges a lot. Using `slerp` instead of `lerp` causes the result to be completely different. --- gdnative-core/src/core_types/quat.rs | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index da850baf9..aeee69d30 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -118,7 +118,7 @@ impl Quat { 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"); - Self::gd(self.glam().slerp(b.glam(), t)) + Self::gd(self.glam().lerp(b.glam(), t)) } /// Returns the result of the spherical linear interpolation between this quaternion and `t` by @@ -208,4 +208,23 @@ mod test { 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); + dbg!(q.slerp(p, t), e); + 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))); + } } From bbbd2873b31b43532cdda3d7159bf345b4dc4174 Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Mon, 5 Apr 2021 20:34:50 +0200 Subject: [PATCH 5/6] Copy Godot's Quat::slerp --- gdnative-core/src/core_types/quat.rs | 49 ++++++++++++++++++++++++++-- 1 file changed, 46 insertions(+), 3 deletions(-) diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index aeee69d30..b06b411b2 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -1,5 +1,5 @@ -use super::{Basis, IsEqualApprox, Vector3}; -use std::ops::Mul; +use super::{Basis, IsEqualApprox, Vector3, CMP_EPSILON}; +use std::ops::{Mul, Neg}; #[derive(Copy, Clone, Debug, PartialEq)] #[repr(C)] @@ -118,7 +118,38 @@ impl Quat { 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"); - Self::gd(self.glam().lerp(b.glam(), t)) + + // 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 @@ -127,6 +158,9 @@ impl Quat { 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 @@ -171,6 +205,15 @@ impl Mul for Quat { } } +impl Neg for Quat { + type Output = Quat; + + #[inline] + fn neg(self) -> Self { + Self::gd(-self.glam()) + } +} + #[cfg(test)] mod test { use super::*; From 8263c05f71a158773ef4264e801af0de599ee9e7 Mon Sep 17 00:00:00 2001 From: David Hoppenbrouwers Date: Mon, 5 Apr 2021 20:39:30 +0200 Subject: [PATCH 6/6] Add test for cubic_slerp --- gdnative-core/src/core_types/quat.rs | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/gdnative-core/src/core_types/quat.rs b/gdnative-core/src/core_types/quat.rs index b06b411b2..46bbaa203 100644 --- a/gdnative-core/src/core_types/quat.rs +++ b/gdnative-core/src/core_types/quat.rs @@ -258,7 +258,6 @@ mod test { 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); - dbg!(q.slerp(p, t), e); assert!(e.is_equal_approx(q.slerp(p, t))); } @@ -270,4 +269,15 @@ mod test { 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))); + } }