Skip to content

Commit fd68cb1

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

File tree

2 files changed

+246
-68
lines changed

2 files changed

+246
-68
lines changed

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

Lines changed: 94 additions & 66 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,90 @@ 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-
}
150-
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-
}
152+
let normalized_inputs = self.ensure_normalized(&[&to]);
153+
assert!(normalized_inputs, "Slerp requires normalized quaternions");
160154

161-
scale0 * self + scale1 * to1
155+
self.as_inner().slerp(to, weight.as_f64())
162156
}
163157

158+
/// # Panics
159+
/// If either quaternion is not normalized.
164160
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-
*/
161+
let normalized_inputs = self.ensure_normalized(&[&to]);
162+
assert!(normalized_inputs, "Slerpni requires normalized quaternions");
163+
164+
self.as_inner().slerpni(to, weight.as_f64())
165+
}
166+
167+
/// # Panics
168+
/// If any quaternions are not normalized.
169+
pub fn spherical_cubic_interpolate(
170+
self,
171+
b: Self,
172+
pre_a: Self,
173+
post_b: Self,
174+
weight: real,
175+
) -> Self {
176+
let normalized_inputs = self.ensure_normalized(&[&b, &pre_a, &post_b]);
177+
assert!(
178+
normalized_inputs,
179+
"Spherical cubic interpolation requires normalized quaternions"
180+
);
181+
182+
self.as_inner()
183+
.spherical_cubic_interpolate(b, pre_a, post_b, weight.as_f64())
184+
}
185+
186+
/// # Panics
187+
/// If any quaternions are not normalized.
188+
#[allow(clippy::too_many_arguments)]
189+
pub fn spherical_cubic_interpolate_in_time(
190+
self,
191+
b: Self,
192+
pre_a: Self,
193+
post_b: Self,
194+
weight: real,
195+
b_t: real,
196+
pre_a_t: real,
197+
post_b_t: real,
198+
) -> Self {
199+
let normalized_inputs = self.ensure_normalized(&[&b, &pre_a, &post_b]);
200+
assert!(
201+
normalized_inputs,
202+
"Spherical cubic interpolation in time requires normalized quaternions"
203+
);
204+
205+
self.as_inner().spherical_cubic_interpolate_in_time(
206+
b,
207+
pre_a,
208+
post_b,
209+
weight.as_f64(),
210+
b_t.as_f64(),
211+
pre_a_t.as_f64(),
212+
post_b_t.as_f64(),
213+
)
214+
}
192215

193216
#[doc(hidden)]
194217
pub fn as_inner(&self) -> inner::InnerQuaternion {
195218
inner::InnerQuaternion::from_outer(self)
196219
}
220+
221+
#[doc(hidden)]
222+
fn ensure_normalized(&self, quats: &[&Quaternion]) -> bool {
223+
quats.iter().all(|v| v.is_normalized()) && self.is_normalized()
224+
}
197225
}
198226

199227
impl Add for Quaternion {

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

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

0 commit comments

Comments
 (0)