Skip to content

Commit 4c2d189

Browse files
0awful0awful
committed
function-completeness
1 parent 52628a0 commit 4c2d189

File tree

2 files changed

+266
-67
lines changed

2 files changed

+266
-67
lines changed

‎godot-core/src/builtin/quaternion.rs

Lines changed: 118 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ use godot_ffi as sys;
99
use sys::{ffi_methods, GodotFfi};
1010

1111
use crate::builtin::math::{ApproxEq, FloatExt, GlamConv, GlamType};
12-
use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, Vector3};
12+
use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, RealConv, Vector3};
1313

1414
use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
1515

@@ -30,22 +30,30 @@ impl Quaternion {
3030
Self { x, y, z, w }
3131
}
3232

33-
pub fn from_angle_axis(axis: Vector3, angle: real) -> Self {
33+
/// Creates a quaternion from a Vector3 and an angle.
34+
///
35+
/// # Panics
36+
/// If the vector3 is not normalized.
37+
pub fn from_axis_angle(axis: Vector3, angle: real) -> Self {
38+
assert!(
39+
axis.is_normalized(),
40+
"Quaternion axis {axis:?} is not normalized."
41+
);
3442
let d = axis.length();
35-
if d == 0.0 {
36-
Self::new(0.0, 0.0, 0.0, 0.0)
37-
} else {
38-
let sin_angle = (angle * 0.5).sin();
39-
let cos_angle = (angle * 0.5).cos();
40-
let s = sin_angle / d;
41-
let x = axis.x * s;
42-
let y = axis.y * s;
43-
let z = axis.z * s;
44-
let w = cos_angle;
45-
Self::new(x, y, z, w)
46-
}
43+
let sin_angle = (angle * 0.5).sin();
44+
let cos_angle = (angle * 0.5).cos();
45+
let s = sin_angle / d;
46+
let x = axis.x * s;
47+
let y = axis.y * s;
48+
let z = axis.z * s;
49+
let w = cos_angle;
50+
Self::new(x, y, z, w)
4751
}
4852

53+
// TODO: Constructors.
54+
// pub fn from_vector_vector(arc_to: Vector3, arc_from: Vector3) -> Self {}
55+
// pub fn from_basis(basis: Basis) -> Self {}
56+
4957
pub fn angle_to(self, to: Self) -> real {
5058
self.glam2(&to, RQuat::angle_between)
5159
}
@@ -62,7 +70,7 @@ impl Quaternion {
6270
if theta < real::CMP_EPSILON || !v.is_normalized() {
6371
Self::default()
6472
} else {
65-
Self::from_angle_axis(v, theta)
73+
Self::from_axis_angle(v, theta)
6674
}
6775
}
6876

@@ -130,70 +138,115 @@ impl Quaternion {
130138
Quaternion::new(v.x, v.y, v.z, 0.0)
131139
}
132140

141+
/// # Panics
142+
/// If the quaternion has length of 0.
133143
pub fn normalized(self) -> Self {
134-
self / self.length()
144+
let length = self.length();
145+
assert!(!length.approx_eq(&0.0), "Quaternion has length 0");
146+
self / length
135147
}
136148

149+
/// # Panics
150+
/// If either quaternion is not normalized.
137151
pub fn slerp(self, to: Self, weight: real) -> Self {
138-
let mut cosom = self.dot(to);
139-
let to1: Self;
140-
let omega: real;
141-
let sinom: real;
142-
let scale0: real;
143-
let scale1: real;
144-
if cosom < 0.0 {
145-
cosom = -cosom;
146-
to1 = -to;
147-
} else {
148-
to1 = to;
149-
}
152+
let interpolated = self.as_inner().slerp(to, weight.as_f64());
150153

151-
if 1.0 - cosom > real::CMP_EPSILON {
152-
omega = cosom.acos();
153-
sinom = omega.sin();
154-
scale0 = ((1.0 - weight) * omega).sin() / sinom;
155-
scale1 = (weight * omega).sin() / sinom;
156-
} else {
157-
scale0 = 1.0 - weight;
158-
scale1 = weight;
159-
}
154+
// Godot returns default if you give it quaternions that are not normalized. This means we can check for default
155+
// Then check if we should panic.
156+
let normalized_inputs = !interpolated.is_default()
157+
|| interpolated.is_default() && self.ensure_normalized(&[&to]);
158+
assert!(normalized_inputs, "Slerp requires normalized quaternions");
160159

161-
scale0 * self + scale1 * to1
160+
interpolated
162161
}
163162

163+
/// # Panics
164+
/// If either quaternion is not normalized.
164165
pub fn slerpni(self, to: Self, weight: real) -> Self {
165-
let dot = self.dot(to);
166-
if dot.abs() > 0.9999 {
167-
return self;
168-
}
169-
let theta = dot.acos();
170-
let sin_t = 1.0 / theta.sin();
171-
let new_factor = (weight * theta).sin() * sin_t;
172-
let inv_factor = ((1.0 - weight) * theta).sin() * sin_t;
173-
174-
inv_factor * self + new_factor * to
175-
}
176-
177-
// pub fn spherical_cubic_interpolate(self, b: Self, pre_a: Self, post_b: Self, weight: real) -> Self {}
178-
// TODO: Implement godot's function in Rust
179-
/*
180-
pub fn spherical_cubic_interpolate_in_time(
181-
self,
182-
b: Self,
183-
pre_a: Self,
184-
post_b: Self,
185-
weight: real,
186-
b_t: real,
187-
pre_a_t: real,
188-
post_b_t: real,
189-
) -> Self {
190-
}
191-
*/
166+
let interpolated = self.as_inner().slerpni(to, weight.as_f64());
167+
168+
// Godot returns default if you give it quaternions that are not normalized. This means we can check for default
169+
// Then check if we should panic.
170+
let normalized_inputs = !interpolated.is_default()
171+
|| interpolated.is_default() && self.ensure_normalized(&[&to]);
172+
assert!(normalized_inputs, "Slerpni requires normalized quaternions");
173+
174+
interpolated
175+
}
176+
177+
/// # Panics
178+
/// If any quaternions are not normalized.
179+
pub fn spherical_cubic_interpolate(
180+
self,
181+
b: Self,
182+
pre_a: Self,
183+
post_b: Self,
184+
weight: real,
185+
) -> Self {
186+
let interpolated =
187+
self.as_inner()
188+
.spherical_cubic_interpolate(b, pre_a, post_b, weight.as_f64());
189+
190+
// Godot returns default if you give it quaternions that are not normalized. This means we can check for default
191+
// Then check if we should panic.
192+
let normalized_inputs = !interpolated.is_default()
193+
|| interpolated.is_default() && self.ensure_normalized(&[&b, &pre_a, &post_b]);
194+
assert!(
195+
normalized_inputs,
196+
"Spherical cubic interpolation requires normalized quaternions"
197+
);
198+
199+
interpolated
200+
}
201+
202+
/// # Panics
203+
/// If any quaternions are not normalized.
204+
#[allow(clippy::too_many_arguments)]
205+
pub fn spherical_cubic_interpolate_in_time(
206+
self,
207+
b: Self,
208+
pre_a: Self,
209+
post_b: Self,
210+
weight: real,
211+
b_t: real,
212+
pre_a_t: real,
213+
post_b_t: real,
214+
) -> Self {
215+
let interpolated = self.as_inner().spherical_cubic_interpolate_in_time(
216+
b,
217+
pre_a,
218+
post_b,
219+
weight.as_f64(),
220+
b_t.as_f64(),
221+
pre_a_t.as_f64(),
222+
post_b_t.as_f64(),
223+
);
224+
225+
// Godot returns default if you give it quaternions that are not normalized. This means we can check for default
226+
// Then check if we should panic.
227+
let normalized_inputs = !interpolated.is_default()
228+
|| interpolated.is_default() && self.ensure_normalized(&[&b, &pre_a, &post_b]);
229+
assert!(
230+
normalized_inputs,
231+
"Spherical cubic interpolation in time requires normalized quaternions"
232+
);
233+
interpolated
234+
}
192235

193236
#[doc(hidden)]
194237
pub fn as_inner(&self) -> inner::InnerQuaternion {
195238
inner::InnerQuaternion::from_outer(self)
196239
}
240+
241+
#[doc(hidden)]
242+
pub fn is_default(&self) -> bool {
243+
*self == Self::default()
244+
}
245+
246+
#[doc(hidden)]
247+
pub fn ensure_normalized(&self, quats: &[&Quaternion]) -> bool {
248+
quats.iter().all(|v| v.is_normalized()) && self.is_normalized()
249+
}
197250
}
198251

199252
impl Add for Quaternion {

‎itest/rust/src/builtin_tests/geometry/quaternion_test.rs

Lines changed: 148 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,9 @@
55
* file, You can obtain one at https://mozilla.org/MPL/2.0/.
66
*/
77

8-
use crate::framework::itest;
9-
use godot::builtin::Quaternion;
8+
use crate::framework::{expect_panic, itest};
9+
use godot::builtin::math::assert_eq_approx;
10+
use godot::builtin::{Quaternion, Vector3};
1011

1112
#[itest]
1213
fn quaternion_default() {
@@ -28,4 +29,149 @@ fn quaternion_from_xyzw() {
2829
assert_eq!(quat.w, 0.8924);
2930
}
3031

32+
#[itest]
33+
fn quaternion_from_axis_angle() {
34+
// 1. Should generate quaternion from axis angle.
35+
let quat = Quaternion::from_axis_angle(Vector3::new(0.0, 0.0, 1.0).normalized(), 1.0);
36+
37+
// Taken from doing this in GDScript.
38+
assert_eq!(quat.x, 0.0);
39+
assert_eq!(quat.y, 0.0);
40+
assert_eq_approx!(quat.z, 0.479426);
41+
assert_eq_approx!(quat.w, 0.877583);
42+
43+
// 2. Should panic if axis is not normalized.
44+
expect_panic("Quaternion axis {axis:?} is not normalized.", || {
45+
Quaternion::from_axis_angle(Vector3::new(0.0, 0.0, 0.0), 1.0);
46+
});
47+
}
48+
49+
#[itest]
50+
fn quaternion_normalization() {
51+
// 1. Should panic on quaternions with length 0.
52+
expect_panic("Quaternion has length 0", || {
53+
Quaternion::new(0.0, 0.0, 0.0, 0.0).normalized();
54+
});
55+
56+
// 2. Should not panic on any other length.
57+
let quat = Quaternion::default().normalized();
58+
assert_eq!(quat.length(), 1.0);
59+
assert!(quat.is_normalized());
60+
}
61+
62+
#[itest]
63+
fn quaternion_slerp() {
64+
let a = Quaternion::new(-1.0, -1.0, -1.0, 10.0);
65+
let b = Quaternion::new(3.0, 3.0, 3.0, 5.0);
66+
67+
// 1. Should perform interpolation.
68+
let outcome = a.normalized().slerp(b.normalized(), 1.0);
69+
let expected = Quaternion::new(0.41602516, 0.41602516, 0.41602516, 0.69337523);
70+
assert_eq_approx!(outcome, expected);
71+
72+
// 2. Should panic on quaternions that are not normalized.
73+
expect_panic("Slerp requires normalized quaternions", || {
74+
a.slerp(b, 1.9);
75+
});
76+
77+
// 3. Should not panic on default values.
78+
let outcome = Quaternion::default().slerp(Quaternion::default(), 1.0);
79+
assert_eq!(outcome, Quaternion::default());
80+
}
81+
82+
#[itest]
83+
fn quaternion_slerpni() {
84+
let a = Quaternion::new(-1.0, -1.0, -1.0, 10.0);
85+
let b = Quaternion::new(3.0, 3.0, 3.0, 6.0);
86+
87+
// 1. Should perform interpolation.
88+
let outcome = a.normalized().slerpni(b.normalized(), 1.0);
89+
let expected = Quaternion::new(0.37796447, 0.37796447, 0.37796447, 0.75592893);
90+
assert_eq_approx!(outcome, expected);
91+
92+
// 2. Should panic on quaternions that are not normalized.
93+
expect_panic("Slerpni requires normalized quaternions", || {
94+
a.slerpni(b, 1.9);
95+
});
96+
97+
// 3. Should not panic on default values.
98+
let outcome = Quaternion::default().slerpni(Quaternion::default(), 1.0);
99+
assert_eq!(outcome, Quaternion::default());
100+
}
101+
102+
#[itest]
103+
fn quaternion_spherical_cubic_interpolate() {
104+
let pre_a = Quaternion::new(-1.0, -1.0, -1.0, -1.0);
105+
let a = Quaternion::new(0.0, 0.0, 0.0, 1.0);
106+
let b = Quaternion::new(0.0, 1.0, 0.0, 2.0);
107+
let post_b = Quaternion::new(2.0, 2.0, 2.0, 2.0);
108+
109+
// 1. Should perform interpolation.
110+
let outcome =
111+
a.spherical_cubic_interpolate(b.normalized(), pre_a.normalized(), post_b.normalized(), 0.5);
112+
113+
// Taken from doing this in GDScript.
114+
let expected = Quaternion::new(-0.072151, 0.176298, -0.072151, 0.979034);
115+
assert_eq_approx!(outcome, expected);
116+
117+
// 2. Should panic on quaternions that are not normalized.
118+
expect_panic(
119+
"Spherical cubic interpolation requires normalized quaternions",
120+
|| {
121+
a.spherical_cubic_interpolate(b, pre_a, post_b, 0.5);
122+
},
123+
);
124+
125+
// 3. Should not panic on default returns when inputs are normalized.
126+
let outcome = Quaternion::default().spherical_cubic_interpolate(
127+
Quaternion::default(),
128+
Quaternion::default(),
129+
Quaternion::default(),
130+
1.0,
131+
);
132+
assert_eq!(outcome, Quaternion::default());
133+
}
134+
135+
#[itest]
136+
fn quaternion_spherical_cubic_interpolate_in_time() {
137+
let pre_a = Quaternion::new(-1.0, -1.0, -1.0, -1.0);
138+
let a = Quaternion::new(0.0, 0.0, 0.0, 1.0);
139+
let b = Quaternion::new(0.0, 1.0, 0.0, 2.0);
140+
let post_b = Quaternion::new(2.0, 2.0, 2.0, 2.0);
141+
142+
// 1. Should perform interpolation.
143+
let outcome = a.spherical_cubic_interpolate_in_time(
144+
b.normalized(),
145+
pre_a.normalized(),
146+
post_b.normalized(),
147+
0.5,
148+
0.1,
149+
0.1,
150+
0.1,
151+
);
152+
153+
// Taken from doing this in GDScript.
154+
let expected = Quaternion::new(0.280511, 0.355936, 0.280511, 0.84613);
155+
assert_eq_approx!(outcome, expected);
156+
157+
// 2. Should panic on quaternions that are not normalized.
158+
expect_panic(
159+
"Spherical cubic interpolation in time requires normalized quaternions",
160+
|| {
161+
a.spherical_cubic_interpolate_in_time(b, pre_a, post_b, 0.5, 0.1, 0.1, 0.1);
162+
},
163+
);
164+
165+
// 3. Should not panic on default returns when inputs are normalized.
166+
let outcome = Quaternion::default().spherical_cubic_interpolate_in_time(
167+
Quaternion::default(),
168+
Quaternion::default(),
169+
Quaternion::default(),
170+
1.0,
171+
1.0,
172+
1.0,
173+
1.0,
174+
);
175+
assert_eq!(outcome, Quaternion::default())
176+
}
31177
// TODO more tests

0 commit comments

Comments
 (0)