diff --git a/godot-core/src/builtin/quaternion.rs b/godot-core/src/builtin/quaternion.rs index 446bf8148..fe5ddc23a 100644 --- a/godot-core/src/builtin/quaternion.rs +++ b/godot-core/src/builtin/quaternion.rs @@ -9,7 +9,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; use crate::builtin::math::{ApproxEq, FloatExt, GlamConv, GlamType}; -use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, Vector3}; +use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, RealConv, Vector3}; use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign}; @@ -30,10 +30,14 @@ impl Quaternion { Self { x, y, z, w } } + /// Creates a Quaternion from a Vector3 and an angle. + /// + /// # Panics + /// If the Vector3 is not normalized. pub fn from_angle_axis(axis: Vector3, angle: real) -> Self { let d = axis.length(); - if d == 0.0 { - Self::new(0.0, 0.0, 0.0, 0.0) + if d != 1.0 { + panic!("Attempted to create a Quaternion from a Vector3 that was not normalized."); } else { let sin_angle = (angle * 0.5).sin(); let cos_angle = (angle * 0.5).cos(); @@ -130,8 +134,16 @@ impl Quaternion { Quaternion::new(v.x, v.y, v.z, 0.0) } + /// Normalizes the Quaternion. + /// + /// # Panics + /// If the Quaternion has length of 0. pub fn normalized(self) -> Self { - self / self.length() + let length = self.length(); + if length == 0.0 { + panic!("Attempted to normalize a Quaternion with 0 length."); + } + self / length } pub fn slerp(self, to: Self, weight: real) -> Self { @@ -174,21 +186,73 @@ impl Quaternion { inv_factor * self + new_factor * to } - // pub fn spherical_cubic_interpolate(self, b: Self, pre_a: Self, post_b: Self, weight: real) -> Self {} - // TODO: Implement godot's function in Rust - /* - pub fn spherical_cubic_interpolate_in_time( - self, - b: Self, - pre_a: Self, - post_b: Self, - weight: real, - b_t: real, - pre_a_t: real, - post_b_t: real, - ) -> Self { + /// Performs spherical cubic interpolation. + /// + /// # Panics + /// If the provided Quaternions are not normalized. + pub fn spherical_cubic_interpolate( + self, + b: Self, + pre_a: Self, + post_b: Self, + weight: real, + ) -> Self { + let interpolated = + self.as_inner() + .spherical_cubic_interpolate(b, pre_a, post_b, weight.as_f64()); + // Godot returns default if you give it Quaternions that are not normalized. This means we can check for default + // Then check if we should panic. + if interpolated == Quaternion::default() + && (!b.is_normalized() + || !pre_a.is_normalized() + || !post_b.is_normalized() + || !self.is_normalized()) + { + panic!( + "Attempted spherical cubic interpolation on Quaternions that are not normalized." + ); + } + interpolated + } + + /// Performs spherical cubic interpolation in time. + /// + /// # Panics + /// If the provided Quaternions are not normalized. + #[allow(clippy::too_many_arguments)] + pub fn spherical_cubic_interpolate_in_time( + self, + b: Self, + pre_a: Self, + post_b: Self, + weight: real, + b_t: real, + pre_a_t: real, + post_b_t: real, + ) -> Self { + let interpolated = self.as_inner().spherical_cubic_interpolate_in_time( + b, + pre_a, + post_b, + weight.as_f64(), + b_t.as_f64(), + pre_a_t.as_f64(), + post_b_t.as_f64(), + ); + // Godot returns default if you give it Quaternions that are not normalized. This means we can check for default + // Then check if we should panic. + if interpolated == Quaternion::default() + && (!b.is_normalized() + || !pre_a.is_normalized() + || !post_b.is_normalized() + || !self.is_normalized()) + { + panic!( + "Attempted spherical cubic interpolation in time on Quaternions that are not normalized." + ); } - */ + interpolated + } #[doc(hidden)] pub fn as_inner(&self) -> inner::InnerQuaternion { diff --git a/itest/rust/src/builtin_tests/geometry/quaternion_test.rs b/itest/rust/src/builtin_tests/geometry/quaternion_test.rs index 9a1df447e..d8921a3b9 100644 --- a/itest/rust/src/builtin_tests/geometry/quaternion_test.rs +++ b/itest/rust/src/builtin_tests/geometry/quaternion_test.rs @@ -5,8 +5,9 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -use crate::framework::itest; -use godot::builtin::Quaternion; +use crate::framework::{expect_panic, itest}; +use godot::builtin::math::assert_eq_approx; +use godot::builtin::{Quaternion, Vector3}; #[itest] fn quaternion_default() { @@ -28,4 +29,79 @@ fn quaternion_from_xyzw() { assert_eq!(quat.w, 0.8924); } +#[itest] +fn quaternion_normalization() { + expect_panic("Attempted to normalize a Quaternion with 0 length.", || { + Quaternion::new(0.0, 0.0, 0.0, 0.0).normalized(); + }); + let quat = Quaternion::default(); + assert_eq!(quat.normalized().length(), 1.0); +} + +#[itest] +fn quaternion_from_angle_axis() { + let quat = Quaternion::from_angle_axis(Vector3::new(0.0, 0.0, 1.0).normalized(), 1.0); + + // Taken from doing this in GDScript. + assert_eq!(quat.x, 0.0); + assert_eq!(quat.y, 0.0); + assert_eq_approx!(quat.z, 0.479426); + assert_eq_approx!(quat.w, 0.877583); + + expect_panic( + "Attempted to create a Quaternion from a Vector3 that was not normalized.", + || { + Quaternion::from_angle_axis(Vector3::new(0.0, 0.0, 0.0), 1.0); + }, + ); +} + +#[itest] +fn quaternion_spherical_cubic_interpolate() { + let pre_a = Quaternion::new(-1.0, -1.0, -1.0, -1.0); + let a = Quaternion::new(0.0, 0.0, 0.0, 1.0); + let b = Quaternion::new(0.0, 1.0, 0.0, 2.0); + let post_b = Quaternion::new(2.0, 2.0, 2.0, 2.0); + + let outcome = + a.spherical_cubic_interpolate(b.normalized(), pre_a.normalized(), post_b.normalized(), 0.5); + // Taken from doing this in GDScript. + let expected = Quaternion::new(-0.072151, 0.176298, -0.072151, 0.979034); + assert_eq_approx!(outcome, expected); + + expect_panic( + "Attempted spherical cubic interpolation on quaternions that are not normalized.", + || { + a.spherical_cubic_interpolate(b, pre_a, post_b, 0.5); + }, + ); +} + +#[itest] +fn quaternion_spherical_cubic_interpolate_in_time() { + let pre_a = Quaternion::new(-1.0, -1.0, -1.0, -1.0); + let a = Quaternion::new(0.0, 0.0, 0.0, 1.0); + let b = Quaternion::new(0.0, 1.0, 0.0, 2.0); + let post_b = Quaternion::new(2.0, 2.0, 2.0, 2.0); + + let outcome = a.spherical_cubic_interpolate_in_time( + b.normalized(), + pre_a.normalized(), + post_b.normalized(), + 0.5, + 0.1, + 0.1, + 0.1, + ); + // Taken from doing this in GDScript. + let expected = Quaternion::new(0.280511, 0.355936, 0.280511, 0.84613); + assert_eq_approx!(outcome, expected); + + expect_panic( + "Attempted spherical cubic interpolation in time on Quaternions that are not normalized.", + || { + a.spherical_cubic_interpolate_in_time(b, pre_a, post_b, 0.5, 0.1, 0.1, 0.1); + }, + ); +} // TODO more tests