diff --git a/godot-core/src/builtin/basis.rs b/godot-core/src/builtin/basis.rs index 59e8b6196..dfb9a3323 100644 --- a/godot-core/src/builtin/basis.rs +++ b/godot-core/src/builtin/basis.rs @@ -7,7 +7,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{is_equal_approx, lerp, ApproxEq, GlamConv, GlamType, CMP_EPSILON}; +use crate::builtin::math::{ApproxEq, FloatExt, GlamConv, GlamType}; use crate::builtin::real_consts::FRAC_PI_2; use crate::builtin::{real, Quaternion, RMat3, RQuat, RVec2, RVec3, Vector3}; @@ -271,9 +271,9 @@ impl Basis { } fn is_between_neg1_1(f: real) -> Ordering { - if f >= (1.0 - CMP_EPSILON) { + if f >= (1.0 - real::CMP_EPSILON) { Ordering::Greater - } else if f <= -(1.0 - CMP_EPSILON) { + } else if f <= -(1.0 - real::CMP_EPSILON) { Ordering::Less } else { Ordering::Equal @@ -364,15 +364,19 @@ impl Basis { Self::from_cols(self.rows[0], self.rows[1], self.rows[2]) } - /// Returns the orthonormalized version of the matrix (useful to call from + /// ⚠️ Returns the orthonormalized version of the matrix (useful to call from /// time to time to avoid rounding error for orthogonal matrices). This /// performs a Gram-Schmidt orthonormalization on the basis of the matrix. /// + /// # Panics + /// + /// If the determinant of the matrix is 0. + /// /// _Godot equivalent: `Basis.orthonormalized()`_ #[must_use] pub fn orthonormalized(self) -> Self { assert!( - !is_equal_approx(self.determinant(), 0.0), + !self.determinant().is_zero_approx(), "Determinant should not be zero." ); @@ -411,7 +415,7 @@ impl Basis { let mut result = Self::from_quat(from.slerp(to, weight)); for i in 0..3 { - result.rows[i] *= lerp(self.rows[i].length(), other.rows[i].length(), weight); + result.rows[i] *= self.rows[i].length().lerp(other.rows[i].length(), weight); } result diff --git a/godot-core/src/builtin/math/approx_eq.rs b/godot-core/src/builtin/math/approx_eq.rs index fefcc7bd7..e77eb2650 100644 --- a/godot-core/src/builtin/math/approx_eq.rs +++ b/godot-core/src/builtin/math/approx_eq.rs @@ -4,8 +4,6 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -use crate::builtin::real; - // TODO(bromeon): test this against Godot's own is_equal_approx() implementation for equality-comparable built-in types (excl Callable/Rid/...) /// Approximate equality-comparison of geometric types. @@ -19,12 +17,6 @@ pub trait ApproxEq: PartialEq { fn approx_eq(&self, other: &Self) -> bool; } -impl ApproxEq for real { - fn approx_eq(&self, other: &Self) -> bool { - crate::builtin::math::is_equal_approx(*self, *other) - } -} - /// Asserts that two values are approximately equal /// /// For comparison, this uses `ApproxEq::approx_eq` by default, or the provided `fn = ...` function. diff --git a/godot-core/src/builtin/math/float.rs b/godot-core/src/builtin/math/float.rs new file mode 100644 index 000000000..383cfac2a --- /dev/null +++ b/godot-core/src/builtin/math/float.rs @@ -0,0 +1,352 @@ +/* + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + */ + +use crate::builtin::{real, RealConv, Vector2}; + +use super::ApproxEq; + +mod private { + pub trait Sealed {} + + impl Sealed for f32 {} + impl Sealed for f64 {} +} + +pub trait FloatExt: private::Sealed + Copy { + const CMP_EPSILON: Self; + + /// Linearly interpolates from `self` to `to` by `weight`. + /// + /// `weight` should be in the range `0.0 ..= 1.0`, but values outside this are allowed and will perform + /// linear extrapolation. + fn lerp(self, to: Self, weight: Self) -> Self; + + /// Approximate equality of floating-point types. + /// + /// For a generalization of this, see [`ApproxEq`][crate::builtin::ApproxEq] trait. + fn is_equal_approx(self, other: Self) -> bool; + + /// Check if two angles are approximately equal, by comparing the distance + /// between the points on the unit circle with 0 using [`is_equal_approx`]. + fn is_angle_equal_approx(self, other: Self) -> bool; + + /// Check if `self` is within [`Self::CMP_EPSILON`] of `0.0`. + fn is_zero_approx(self) -> bool; + + fn fposmod(self, pmod: Self) -> Self; + + /// Returns the multiple of `step` that is closest to `self`. + fn snapped(self, step: Self) -> Self; + + /// Returns `-1.0` if `self` is negative, `1.0` if `self` is positive, `0.0` if `self` is zero. + /// + /// The return value for `NAN` is unspecified. + fn sign(self) -> Self; + + /// Returns the derivative at the given `t` on a one-dimensional Bézier curve defined by the given + /// `control_1`, `control_2`, and `end` points. + fn bezier_derivative(self, control_1: Self, control_2: Self, end: Self, t: Self) -> Self; + + /// Returns the point at the given `t` on a one-dimensional Bézier curve defined by the given + /// `control_1`, `control_2`, and `end` points. + fn bezier_interpolate(self, control_1: Self, control_2: Self, end: Self, t: Self) -> Self; + + /// Cubic interpolates between two values by the factor defined in `weight` with `pre` and `post` values. + fn cubic_interpolate(self, to: Self, pre: Self, post: Self, weight: Self) -> Self; + + /// Cubic interpolates between two values by the factor defined in `weight` with `pre` and `post` values. + /// It can perform smoother interpolation than [`cubic_interpolate`](FloatExt::cubic_interpolate) by the time values. + #[allow(clippy::too_many_arguments)] + fn cubic_interpolate_in_time( + self, + to: Self, + pre: Self, + post: Self, + weight: Self, + to_t: Self, + pre_t: Self, + post_t: Self, + ) -> Self; + + /// Linearly interpolates between two angles (in radians) by a `weight` value + /// between 0.0 and 1.0. + /// + /// Similar to [`lerp`], but interpolates correctly when the angles wrap around + /// [`TAU`]. + /// + /// The resulting angle is not normalized. + /// + /// Note: This function lerps through the shortest path between `from` and + /// `to`. However, when these two angles are approximately `PI + k * TAU` apart + /// for any integer `k`, it's not obvious which way they lerp due to + /// floating-point precision errors. For example, with single-precision floats + /// `lerp_angle(0.0, PI, weight)` lerps clockwise, while `lerp_angle(0.0, PI + 3.0 * TAU, weight)` + /// lerps counter-clockwise. + /// + /// _Godot equivalent: @GlobalScope.lerp_angle()_ + fn lerp_angle(self, to: Self, weight: Self) -> Self; +} + +macro_rules! impl_float_ext { + ($Ty:ty, $consts:path, $to_real:ident) => { + impl FloatExt for $Ty { + const CMP_EPSILON: Self = 0.00001; + + fn lerp(self, to: Self, t: Self) -> Self { + self + ((to - self) * t) + } + + fn is_equal_approx(self, other: Self) -> bool { + if self == other { + return true; + } + let mut tolerance = Self::CMP_EPSILON * self.abs(); + if tolerance < Self::CMP_EPSILON { + tolerance = Self::CMP_EPSILON; + } + (self - other).abs() < tolerance + } + + fn is_angle_equal_approx(self, other: Self) -> bool { + let (x1, y1) = self.sin_cos(); + let (x2, y2) = other.sin_cos(); + + let point_1 = Vector2::new(real::$to_real(x1), real::$to_real(y1)); + let point_2 = Vector2::new(real::$to_real(x2), real::$to_real(y2)); + + point_1.distance_to(point_2).is_zero_approx() + } + + fn is_zero_approx(self) -> bool { + self.abs() < Self::CMP_EPSILON + } + + fn fposmod(self, pmod: Self) -> Self { + let mut value = self % pmod; + if ((value < 0.0) && (pmod > 0.0)) || ((value > 0.0) && (pmod < 0.0)) { + value += pmod; + } + value + } + + fn snapped(mut self, step: Self) -> Self { + if step != 0.0 { + self = ((self / step + 0.5) * step).floor() + } + self + } + + fn sign(self) -> Self { + use std::cmp::Ordering; + + match self.partial_cmp(&0.0) { + Some(Ordering::Equal) => 0.0, + Some(Ordering::Greater) => 1.0, + Some(Ordering::Less) => -1.0, + // NAN, Godot return `1` in this case. + None => 1.0, + } + } + + fn bezier_derivative( + self, + control_1: Self, + control_2: Self, + end: Self, + t: Self, + ) -> Self { + let omt = 1.0 - t; + let omt2 = omt * omt; + let t2 = t * t; + (control_1 - self) * 3.0 * omt2 + + (control_2 - control_1) * 6.0 * omt * t + + (end - control_2) * 3.0 * t2 + } + + fn bezier_interpolate( + self, + control_1: Self, + control_2: Self, + end: Self, + t: Self, + ) -> Self { + let omt = 1.0 - t; + let omt2 = omt * omt; + let omt3 = omt2 * omt; + let t2 = t * t; + let t3 = t2 * t; + self * omt3 + control_1 * omt2 * t * 3.0 + control_2 * omt * t2 * 3.0 + end * t3 + } + + fn cubic_interpolate(self, to: Self, pre: Self, post: Self, weight: Self) -> Self { + 0.5 * ((self * 2.0) + + (-pre + to) * weight + + (2.0 * pre - 5.0 * self + 4.0 * to - post) * (weight * weight) + + (-pre + 3.0 * self - 3.0 * to + post) * (weight * weight * weight)) + } + + fn cubic_interpolate_in_time( + self, + to: Self, + pre: Self, + post: Self, + weight: Self, + to_t: Self, + pre_t: Self, + post_t: Self, + ) -> Self { + let t = Self::lerp(0.0, to_t, weight); + + let a1 = Self::lerp( + pre, + self, + if pre_t == 0.0 { + 0.0 + } else { + (t - pre_t) / -pre_t + }, + ); + + let a2 = Self::lerp(self, to, if to_t == 0.0 { 0.5 } else { t / to_t }); + + let a3 = Self::lerp( + to, + post, + if post_t - to_t == 0.0 { + 1.0 + } else { + (t - to_t) / (post_t - to_t) + }, + ); + + let b1 = Self::lerp( + a1, + a2, + if to_t - pre_t == 0.0 { + 0.0 + } else { + (t - pre_t) / (to_t - pre_t) + }, + ); + + let b2 = Self::lerp(a2, a3, if post_t == 0.0 { 1.0 } else { t / post_t }); + + Self::lerp(b1, b2, if to_t == 0.0 { 0.5 } else { t / to_t }) + } + + fn lerp_angle(self, to: Self, weight: Self) -> Self { + use $consts; + + let difference = (to - self) % consts::TAU; + let distance = (2.0 * difference) % consts::TAU - difference; + self + distance * weight + } + } + }; +} + +impl_float_ext!(f32, std::f32::consts, from_f32); +impl_float_ext!(f64, std::f64::consts, from_f64); + +impl ApproxEq for f32 { + fn approx_eq(&self, other: &Self) -> bool { + self.is_equal_approx(*other) + } +} + +impl ApproxEq for f64 { + fn approx_eq(&self, other: &Self) -> bool { + self.is_equal_approx(*other) + } +} + +#[cfg(test)] +mod test { + use crate::assert_eq_approx; + + use super::*; + + // Create functions that take references for use in `assert_eq/ne_approx`. + fn is_angle_equal_approx_f32(a: &f32, b: &f32) -> bool { + a.is_angle_equal_approx(*b) + } + + fn is_angle_equal_approx_f64(a: &f64, b: &f64) -> bool { + a.is_angle_equal_approx(*b) + } + + #[test] + fn angle_equal_approx_f32() { + use std::f32::consts::{PI, TAU}; + + assert_eq_approx!(1.0, 1.000001, fn = is_angle_equal_approx_f32); + assert_eq_approx!(0.0, TAU, fn = is_angle_equal_approx_f32); + assert_eq_approx!(PI, -PI, fn = is_angle_equal_approx_f32); + assert_eq_approx!(4.45783, -(TAU - 4.45783), fn = is_angle_equal_approx_f32); + assert_eq_approx!(31.0 * PI, -13.0 * PI, fn = is_angle_equal_approx_f32); + } + + #[test] + fn angle_equal_approx_f64() { + use std::f64::consts::{PI, TAU}; + + assert_eq_approx!(1.0, 1.000001, fn = is_angle_equal_approx_f64); + assert_eq_approx!(0.0, TAU, fn = is_angle_equal_approx_f64); + assert_eq_approx!(PI, -PI, fn = is_angle_equal_approx_f64); + assert_eq_approx!(4.45783, -(TAU - 4.45783), fn = is_angle_equal_approx_f64); + assert_eq_approx!(31.0 * PI, -13.0 * PI, fn = is_angle_equal_approx_f64); + } + + #[test] + #[should_panic(expected = "I am inside format")] + fn eq_approx_fail_with_message() { + assert_eq_approx!(1.0, 2.0, "I am inside {}", "format"); + } + + // As mentioned in the docs for `lerp_angle`, direction can be unpredictable + // when lerping towards PI radians, this also means it's different for single vs + // double precision floats. + + #[test] + fn lerp_angle_test_f32() { + use std::f32::consts::{FRAC_PI_2, PI, TAU}; + + assert_eq_approx!(f32::lerp_angle(0.0, PI, 0.5), -FRAC_PI_2, fn = is_angle_equal_approx_f32); + + assert_eq_approx!( + f32::lerp_angle(0.0, PI + 3.0 * TAU, 0.5), + FRAC_PI_2, + fn = is_angle_equal_approx_f32 + ); + + let angle = PI * 2.0 / 3.0; + assert_eq_approx!( + f32::lerp_angle(-5.0 * TAU, angle + 3.0 * TAU, 0.5), + (angle / 2.0), + fn = is_angle_equal_approx_f32 + ); + } + + #[test] + fn lerp_angle_test_f64() { + use std::f64::consts::{FRAC_PI_2, PI, TAU}; + + assert_eq_approx!(f64::lerp_angle(0.0, PI, 0.5), -FRAC_PI_2, fn = is_angle_equal_approx_f64); + + assert_eq_approx!( + f64::lerp_angle(0.0, PI + 3.0 * TAU, 0.5), + -FRAC_PI_2, + fn = is_angle_equal_approx_f64 + ); + + let angle = PI * 2.0 / 3.0; + assert_eq_approx!( + f64::lerp_angle(-5.0 * TAU, angle + 3.0 * TAU, 0.5), + (angle / 2.0), + fn = is_angle_equal_approx_f64 + ); + } +} diff --git a/godot-core/src/builtin/math/mod.rs b/godot-core/src/builtin/math/mod.rs index f13862fa9..4a9976858 100644 --- a/godot-core/src/builtin/math/mod.rs +++ b/godot-core/src/builtin/math/mod.rs @@ -4,190 +4,20 @@ * file, You can obtain one at https://mozilla.org/MPL/2.0/. */ -use crate::builtin::real_consts::TAU; -use crate::builtin::{real, Vector2}; - mod approx_eq; +mod float; mod glam_helpers; pub use crate::{assert_eq_approx, assert_ne_approx}; pub use approx_eq::ApproxEq; +pub use float::FloatExt; // Internal glam re-exports pub(crate) use glam::{IVec2, IVec3, IVec4}; pub(crate) use glam_helpers::*; -pub const CMP_EPSILON: real = 0.00001; - -pub fn lerp(a: real, b: real, t: real) -> real { - a + ((b - a) * t) -} - -/// Approximate equality of floating-point types. -/// -/// For a generalization of this, see [`ApproxEq`][crate::builtin::ApproxEq] trait. -pub fn is_equal_approx(a: real, b: real) -> bool { - if a == b { - return true; - } - let mut tolerance = CMP_EPSILON * a.abs(); - if tolerance < CMP_EPSILON { - tolerance = CMP_EPSILON; - } - (a - b).abs() < tolerance -} - -/// Check if two angles are approximately equal, by comparing the distance -/// between the points on the unit circle with 0 using [`is_equal_approx`]. -pub fn is_angle_equal_approx(a: &real, b: &real) -> bool { - let (x1, y1) = a.sin_cos(); - let (x2, y2) = b.sin_cos(); - - println!("({x1}, {y1}) ({x2}, {y2})"); - - is_equal_approx( - Vector2::distance_to(Vector2::new(x1, y1), Vector2::new(x2, y2)), - 0.0, - ) -} - -pub fn is_zero_approx(s: real) -> bool { - s.abs() < CMP_EPSILON -} - -pub fn fposmod(x: real, y: real) -> real { - let mut value = x % y; - if ((value < 0.0) && (y > 0.0)) || ((value > 0.0) && (y < 0.0)) { - value += y; - } - value += 0.0; - value -} - -pub fn snapped(mut value: real, step: real) -> real { - if step != 0.0 { - value = ((value / step + 0.5) * step).floor() - } - value -} - -pub fn sign(value: real) -> real { - if value == 0.0 { - 0.0 - } else if value < 0.0 { - -1.0 - } else { - 1.0 - } -} - -pub fn bezier_derivative( - start: real, - control_1: real, - control_2: real, - end: real, - t: real, -) -> real { - let omt = 1.0 - t; - let omt2 = omt * omt; - let t2 = t * t; - (control_1 - start) * 3.0 * omt2 - + (control_2 - control_1) * 6.0 * omt * t - + (end - control_2) * 3.0 * t2 -} - -pub fn bezier_interpolate( - start: real, - control_1: real, - control_2: real, - end: real, - t: real, -) -> real { - let omt = 1.0 - t; - let omt2 = omt * omt; - let omt3 = omt2 * omt; - let t2 = t * t; - let t3 = t2 * t; - start * omt3 + control_1 * omt2 * t * 3.0 + control_2 * omt * t2 * 3.0 + end * t3 -} - -pub fn cubic_interpolate(from: real, to: real, pre: real, post: real, weight: real) -> real { - 0.5 * ((from * 2.0) - + (-pre + to) * weight - + (2.0 * pre - 5.0 * from + 4.0 * to - post) * (weight * weight) - + (-pre + 3.0 * from - 3.0 * to + post) * (weight * weight * weight)) -} - -#[allow(clippy::too_many_arguments)] -pub fn cubic_interpolate_in_time( - from: real, - to: real, - pre: real, - post: real, - weight: real, - to_t: real, - pre_t: real, - post_t: real, -) -> real { - let t = lerp(0.0, to_t, weight); - let a1 = lerp( - pre, - from, - if pre_t == 0.0 { - 0.0 - } else { - (t - pre_t) / -pre_t - }, - ); - let a2 = lerp(from, to, if to_t == 0.0 { 0.5 } else { t / to_t }); - let a3 = lerp( - to, - post, - if post_t - to_t == 0.0 { - 1.0 - } else { - (t - to_t) / (post_t - to_t) - }, - ); - let b1 = lerp( - a1, - a2, - if to_t - pre_t == 0.0 { - 0.0 - } else { - (t - pre_t) / (to_t - pre_t) - }, - ); - let b2 = lerp(a2, a3, if post_t == 0.0 { 1.0 } else { t / post_t }); - lerp(b1, b2, if to_t == 0.0 { 0.5 } else { t / to_t }) -} - -/// Linearly interpolates between two angles (in radians) by a `weight` value -/// between 0.0 and 1.0. -/// -/// Similar to [`lerp`], but interpolates correctly when the angles wrap around -/// [`TAU`]. -/// -/// The resulting angle is not normalized. -/// -/// Note: This function lerps through the shortest path between `from` and -/// `to`. However, when these two angles are approximately `PI + k * TAU` apart -/// for any integer `k`, it's not obvious which way they lerp due to -/// floating-point precision errors. For example, with single-precision floats -/// `lerp_angle(0.0, PI, weight)` lerps clockwise, while `lerp_angle(0.0, PI + 3.0 * TAU, weight)` -/// lerps counter-clockwise. -/// -/// _Godot equivalent: @GlobalScope.lerp_angle()_ -pub fn lerp_angle(from: real, to: real, weight: real) -> real { - let difference = (to - from) % TAU; - let distance = (2.0 * difference) % TAU - difference; - from + distance * weight -} - #[cfg(test)] mod test { - use crate::builtin::real_consts::{FRAC_PI_2, PI}; - use super::*; #[test] @@ -197,48 +27,4 @@ mod test { assert_eq_approx!(1.0, 1.000001, "Message {}", "formatted"); assert_ne_approx!(1.0, 2.0, "Message {}", "formatted"); } - - #[test] - fn angle_equal_approx() { - assert_eq_approx!(1.0, 1.000001, fn = is_angle_equal_approx); - assert_eq_approx!(0.0, TAU, fn = is_angle_equal_approx); - assert_eq_approx!(PI, -PI, fn = is_angle_equal_approx); - assert_eq_approx!(4.45783, -(TAU - 4.45783), fn = is_angle_equal_approx); - assert_eq_approx!(31.0 * PI, -13.0 * PI, fn = is_angle_equal_approx); - } - - #[test] - #[should_panic(expected = "I am inside format")] - fn eq_approx_fail_with_message() { - assert_eq_approx!(1.0, 2.0, "I am inside {}", "format"); - } - - #[test] - fn lerp_angle_test() { - assert_eq_approx!(lerp_angle(0.0, PI, 0.5), -FRAC_PI_2, fn = is_angle_equal_approx); - // As mentioned in the docs for `lerp_angle`, direction can be unpredictable - // when lerping towards PI radians, this also means it's different for single vs - // double precision floats. - - // TODO: look into if it's possible to make a more robust impl. - #[cfg(not(feature = "double-precision"))] - assert_eq_approx!( - lerp_angle(0.0, PI + 3.0 * TAU, 0.5), - FRAC_PI_2, - fn = is_angle_equal_approx - ); - #[cfg(feature = "double-precision")] - assert_eq_approx!( - lerp_angle(0.0, PI + 3.0 * TAU, 0.5), - -FRAC_PI_2, - fn = is_angle_equal_approx - ); - - let angle = PI * 2.0 / 3.0; - assert_eq_approx!( - lerp_angle(-5.0 * TAU, angle + 3.0 * TAU, 0.5), - (angle / 2.0), - fn = is_angle_equal_approx - ); - } } diff --git a/godot-core/src/builtin/plane.rs b/godot-core/src/builtin/plane.rs index 1f3db54b4..bcc77ee07 100644 --- a/godot-core/src/builtin/plane.rs +++ b/godot-core/src/builtin/plane.rs @@ -7,7 +7,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{is_equal_approx, is_zero_approx, ApproxEq, CMP_EPSILON}; +use crate::builtin::math::{ApproxEq, FloatExt}; use crate::builtin::{real, Vector3}; use std::ops::Neg; @@ -134,7 +134,7 @@ impl Plane { #[doc(alias = "has_point")] pub fn contains_point(&self, point: Vector3, tolerance: Option) -> bool { let dist: real = (self.normal.dot(point) - self.d).abs(); - dist <= tolerance.unwrap_or(CMP_EPSILON) + dist <= tolerance.unwrap_or(real::CMP_EPSILON) } /// Finds the intersection point of three planes. @@ -146,7 +146,7 @@ impl Plane { let normal1 = b.normal; let normal2 = c.normal; let denom: real = normal0.cross(normal1).dot(normal2); - if is_zero_approx(denom) { + if denom.is_zero_approx() { return None; } let result = normal1.cross(normal2) * self.d @@ -163,11 +163,11 @@ impl Plane { #[inline] pub fn intersect_ray(&self, from: Vector3, dir: Vector3) -> Option { let denom: real = self.normal.dot(dir); - if is_zero_approx(denom) { + if denom.is_zero_approx() { return None; } let dist: real = (self.normal.dot(from) - self.d) / denom; - if dist > CMP_EPSILON { + if dist > real::CMP_EPSILON { return None; } Some(from - dir * dist) @@ -182,11 +182,11 @@ impl Plane { pub fn intersect_segment(&self, from: Vector3, to: Vector3) -> Option { let segment = from - to; let denom: real = self.normal.dot(segment); - if is_zero_approx(denom) { + if denom.is_zero_approx() { return None; } let dist: real = (self.normal.dot(from) - self.d) / denom; - if !(-CMP_EPSILON..=(1.0 + CMP_EPSILON)).contains(&dist) { + if !(-real::CMP_EPSILON..=(1.0 + real::CMP_EPSILON)).contains(&dist) { return None; } Some(from - segment * dist) @@ -260,9 +260,8 @@ impl ApproxEq for Plane { #[inline] fn approx_eq(&self, other: &Self) -> bool { (Vector3::approx_eq(&self.normal, &other.normal) //. - && is_equal_approx(self.d, other.d)) - || (Vector3::approx_eq(&self.normal, &(-other.normal)) - && is_equal_approx(self.d, -other.d)) + && self.d.approx_eq(&other.d)) + || (Vector3::approx_eq(&self.normal, &(-other.normal)) && self.d.approx_eq(&-other.d)) } } diff --git a/godot-core/src/builtin/projection.rs b/godot-core/src/builtin/projection.rs index 26f56f1d4..bfa251fd7 100644 --- a/godot-core/src/builtin/projection.rs +++ b/godot-core/src/builtin/projection.rs @@ -8,7 +8,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; use crate::builtin::inner::InnerProjection; -use crate::builtin::math::{is_equal_approx, ApproxEq, GlamConv, GlamType}; +use crate::builtin::math::{ApproxEq, GlamConv, GlamType}; use crate::builtin::{real, Plane, RMat4, RealConv, Transform3D, Vector2, Vector4, Vector4Axis}; use std::ops::Mul; @@ -473,10 +473,10 @@ impl ApproxEq for Projection { let v = self.cols[i]; let w = other.cols[i]; - if !is_equal_approx(v.x, w.x) - || !is_equal_approx(v.y, w.y) - || !is_equal_approx(v.z, w.z) - || !is_equal_approx(v.w, w.w) + if !v.x.approx_eq(&w.x) + || !v.y.approx_eq(&w.y) + || !v.z.approx_eq(&w.z) + || !v.w.approx_eq(&w.w) { return false; } diff --git a/godot-core/src/builtin/quaternion.rs b/godot-core/src/builtin/quaternion.rs index 890126cb2..47d67e8b9 100644 --- a/godot-core/src/builtin/quaternion.rs +++ b/godot-core/src/builtin/quaternion.rs @@ -6,7 +6,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{is_equal_approx, ApproxEq, GlamConv, GlamType, CMP_EPSILON}; +use crate::builtin::math::{ApproxEq, FloatExt, GlamConv, GlamType}; use crate::builtin::{inner, real, Basis, EulerOrder, RQuat, Vector3}; use std::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign}; @@ -55,7 +55,7 @@ impl Quaternion { let theta = v.length(); v = v.normalized(); - if theta < CMP_EPSILON || !v.is_normalized() { + if theta < real::CMP_EPSILON || !v.is_normalized() { Self::default() } else { Self::from_angle_axis(v, theta) @@ -89,7 +89,7 @@ impl Quaternion { let Self { x, y, z, w } = self; let axis = Vector3::new(x, y, z); - if self.w.abs() > 1.0 - CMP_EPSILON { + if self.w.abs() > 1.0 - real::CMP_EPSILON { axis } else { let r = 1.0 / (1.0 - w * w).sqrt(); @@ -110,7 +110,7 @@ impl Quaternion { } pub fn is_normalized(self) -> bool { - is_equal_approx(self.length_squared(), 1.0) + self.length_squared().approx_eq(&1.0) } pub fn length(self) -> real { @@ -144,7 +144,7 @@ impl Quaternion { to1 = to; } - if 1.0 - cosom > CMP_EPSILON { + if 1.0 - cosom > real::CMP_EPSILON { omega = cosom.acos(); sinom = omega.sin(); scale0 = ((1.0 - weight) * omega).sin() / sinom; @@ -265,10 +265,10 @@ impl Default for Quaternion { impl ApproxEq for Quaternion { fn approx_eq(&self, other: &Self) -> bool { - is_equal_approx(self.x, other.x) - && is_equal_approx(self.y, other.y) - && is_equal_approx(self.z, other.z) - && is_equal_approx(self.w, other.w) + self.x.approx_eq(&other.x) + && self.y.approx_eq(&other.y) + && self.z.approx_eq(&other.z) + && self.w.approx_eq(&other.w) } } diff --git a/godot-core/src/builtin/transform2d.rs b/godot-core/src/builtin/transform2d.rs index 9ba81c896..4d0e5ed69 100644 --- a/godot-core/src/builtin/transform2d.rs +++ b/godot-core/src/builtin/transform2d.rs @@ -7,7 +7,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{assert_ne_approx, lerp_angle, ApproxEq, GlamConv, GlamType}; +use crate::builtin::math::{assert_ne_approx, ApproxEq, FloatExt, GlamConv, GlamType}; use crate::builtin::real_consts::PI; use crate::builtin::{real, RAffine2, RMat2, Rect2, Vector2}; @@ -180,9 +180,9 @@ impl Transform2D { #[must_use] pub fn interpolate_with(self, other: Self, weight: real) -> Self { Self::from_angle_scale_skew_origin( - lerp_angle(self.rotation(), other.rotation(), weight), + self.rotation().lerp_angle(other.rotation(), weight), self.scale().lerp(other.scale(), weight), - lerp_angle(self.skew(), other.skew(), weight), + self.skew().lerp_angle(other.skew(), weight), self.origin.lerp(other.origin, weight), ) } diff --git a/godot-core/src/builtin/vectors/vector2.rs b/godot-core/src/builtin/vectors/vector2.rs index e54d4388b..f53d1bc27 100644 --- a/godot-core/src/builtin/vectors/vector2.rs +++ b/godot-core/src/builtin/vectors/vector2.rs @@ -8,11 +8,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; use crate::builtin::inner; -use crate::builtin::math::{ - bezier_derivative, bezier_interpolate, cubic_interpolate, cubic_interpolate_in_time, fposmod, - is_equal_approx, is_zero_approx, lerp, sign, snapped, ApproxEq, GlamConv, GlamType, - CMP_EPSILON, -}; +use crate::builtin::math::{FloatExt, GlamConv, GlamType}; use crate::builtin::vectors::Vector2Axis; use crate::builtin::{real, RAffine2, RVec2, Vector2i}; @@ -105,24 +101,6 @@ impl Vector2 { self.x / self.y } - pub fn lerp(self, other: Self, weight: real) -> Self { - Self::new(lerp(self.x, other.x, weight), lerp(self.y, other.y, weight)) - } - - pub fn bezier_derivative(self, control_1: Self, control_2: Self, end: Self, t: real) -> Self { - let x = bezier_derivative(self.x, control_1.x, control_2.x, end.x, t); - let y = bezier_derivative(self.y, control_1.y, control_2.y, end.y, t); - - Self::new(x, y) - } - - pub fn bezier_interpolate(self, control_1: Self, control_2: Self, end: Self, t: real) -> Self { - let x = bezier_interpolate(self.x, control_1.x, control_2.x, end.x, t); - let y = bezier_interpolate(self.y, control_1.y, control_2.y, end.y, t); - - Self::new(x, y) - } - pub fn bounce(self, normal: Self) -> Self { -self.reflect(normal) } @@ -139,34 +117,6 @@ impl Vector2 { self.to_glam().perp_dot(with.to_glam()) } - pub fn cubic_interpolate(self, b: Self, pre_a: Self, post_b: Self, weight: real) -> Self { - let x = cubic_interpolate(self.x, b.x, pre_a.x, post_b.x, weight); - let y = cubic_interpolate(self.y, b.y, pre_a.y, post_b.y, weight); - - Self::new(x, y) - } - - #[allow(clippy::too_many_arguments)] - pub fn 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 x = cubic_interpolate_in_time( - self.x, b.x, pre_a.x, post_b.x, weight, b_t, pre_a_t, post_b_t, - ); - let y = cubic_interpolate_in_time( - self.y, b.y, pre_a.y, post_b.y, weight, b_t, pre_a_t, post_b_t, - ); - - Self::new(x, y) - } - pub fn direction_to(self, to: Self) -> Self { (to - self).normalized() } @@ -199,10 +149,6 @@ impl Vector2 { self.to_glam().is_normalized() } - pub fn is_zero_approx(self) -> bool { - is_zero_approx(self.x) && is_zero_approx(self.y) - } - pub fn length_squared(self) -> real { self.to_glam().length_squared() } @@ -230,7 +176,7 @@ impl Vector2 { pub fn move_toward(self, to: Self, delta: real) -> Self { let vd = to - self; let len = vd.length(); - if len <= delta || len < CMP_EPSILON { + if len <= delta || len < real::CMP_EPSILON { to } else { self + vd / len * delta @@ -241,14 +187,6 @@ impl Vector2 { Self::new(self.y, -self.x) } - pub fn posmod(self, pmod: real) -> Self { - Self::new(fposmod(self.x, pmod), fposmod(self.y, pmod)) - } - - pub fn posmodv(self, modv: Self) -> Self { - Self::new(fposmod(self.x, modv.x), fposmod(self.y, modv.y)) - } - pub fn project(self, b: Self) -> Self { Self::from_glam(self.to_glam().project_onto(b.to_glam())) } @@ -261,10 +199,6 @@ impl Vector2 { Self::from_glam(self.to_glam().round()) } - pub fn sign(self) -> Self { - Self::new(sign(self.x), sign(self.y)) - } - // TODO compare with gdnative implementation: // https://github.com/godot-rust/gdnative/blob/master/gdnative-core/src/core_types/vector3.rs#L335-L343 pub fn slerp(self, to: Self, weight: real) -> Self { @@ -274,7 +208,7 @@ impl Vector2 { return self.lerp(to, weight); } let start_length = start_length_sq.sqrt(); - let result_length = lerp(start_length, end_length_sq.sqrt(), weight); + let result_length = real::lerp(start_length, end_length_sq.sqrt(), weight); let angle = self.angle_to(to); self.rotated(angle * weight) * (result_length / start_length) } @@ -283,10 +217,6 @@ impl Vector2 { self - normal * self.dot(normal) } - pub fn snapped(self, step: Self) -> Self { - Self::new(snapped(self.x, step.x), snapped(self.y, step.y)) - } - /// Returns the result of rotating this vector by `angle` (in radians). pub fn rotated(self, angle: real) -> Self { Self::from_glam(RAffine2::from_angle(angle).transform_vector2(self.to_glam())) @@ -310,7 +240,8 @@ impl fmt::Display for Vector2 { } impl_common_vector_fns!(Vector2, real); -impl_float_vector_fns!(Vector2, real); +impl_float_vector_glam_fns!(Vector2, real); +impl_float_vector_component_fns!(Vector2, real, (x, y)); impl_vector_operators!(Vector2, real, (x, y)); impl_from_tuple_for_vector2x!(Vector2, real); @@ -320,13 +251,6 @@ unsafe impl GodotFfi for Vector2 { ffi_methods! { type sys::GDExtensionTypePtr = *mut Self; .. } } -impl ApproxEq for Vector2 { - #[inline] - fn approx_eq(&self, other: &Self) -> bool { - is_equal_approx(self.x, other.x) && is_equal_approx(self.y, other.y) - } -} - impl GlamConv for Vector2 { type Glam = RVec2; } diff --git a/godot-core/src/builtin/vectors/vector3.rs b/godot-core/src/builtin/vectors/vector3.rs index 2b7debfa4..14c5b5707 100644 --- a/godot-core/src/builtin/vectors/vector3.rs +++ b/godot-core/src/builtin/vectors/vector3.rs @@ -7,11 +7,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{ - bezier_derivative, bezier_interpolate, cubic_interpolate, cubic_interpolate_in_time, fposmod, - is_equal_approx, is_zero_approx, lerp, sign, snapped, ApproxEq, GlamConv, GlamType, - CMP_EPSILON, -}; +use crate::builtin::math::{FloatExt, GlamConv, GlamType}; use crate::builtin::vectors::Vector3Axis; use crate::builtin::{real, Basis, RVec3, Vector3i}; @@ -99,22 +95,6 @@ impl Vector3 { self.to_glam().angle_between(to.to_glam()) } - pub fn bezier_derivative(self, control_1: Self, control_2: Self, end: Self, t: real) -> Self { - let x = bezier_derivative(self.x, control_1.x, control_2.x, end.x, t); - let y = bezier_derivative(self.y, control_1.y, control_2.y, end.y, t); - let z = bezier_derivative(self.z, control_1.z, control_2.z, end.z, t); - - Self::new(x, y, z) - } - - pub fn bezier_interpolate(self, control_1: Self, control_2: Self, end: Self, t: real) -> Self { - let x = bezier_interpolate(self.x, control_1.x, control_2.x, end.x, t); - let y = bezier_interpolate(self.y, control_1.y, control_2.y, end.y, t); - let z = bezier_interpolate(self.z, control_1.z, control_2.z, end.z, t); - - Self::new(x, y, z) - } - pub fn bounce(self, normal: Self) -> Self { -self.reflect(normal) } @@ -131,38 +111,6 @@ impl Vector3 { Self::from_glam(self.to_glam().cross(with.to_glam())) } - pub fn cubic_interpolate(self, b: Self, pre_a: Self, post_b: Self, weight: real) -> Self { - let x = cubic_interpolate(self.x, b.x, pre_a.x, post_b.x, weight); - let y = cubic_interpolate(self.y, b.y, pre_a.y, post_b.y, weight); - let z = cubic_interpolate(self.z, b.z, pre_a.z, post_b.z, weight); - - Self::new(x, y, z) - } - - #[allow(clippy::too_many_arguments)] - pub fn 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 x = cubic_interpolate_in_time( - self.x, b.x, pre_a.x, post_b.x, weight, b_t, pre_a_t, post_b_t, - ); - let y = cubic_interpolate_in_time( - self.y, b.y, pre_a.y, post_b.y, weight, b_t, pre_a_t, post_b_t, - ); - let z = cubic_interpolate_in_time( - self.z, b.z, pre_a.z, post_b.z, weight, b_t, pre_a_t, post_b_t, - ); - - Self::new(x, y, z) - } - pub fn direction_to(self, to: Self) -> Self { (to - self).normalized() } @@ -195,18 +143,10 @@ impl Vector3 { self.to_glam().is_normalized() } - pub fn is_zero_approx(self) -> bool { - is_zero_approx(self.x) && is_zero_approx(self.y) && is_zero_approx(self.z) - } - pub fn length_squared(self) -> real { self.to_glam().length_squared() } - pub fn lerp(self, to: Self, weight: real) -> Self { - Self::from_glam(self.to_glam().lerp(to.to_glam(), weight)) - } - pub fn limit_length(self, length: Option) -> Self { Self::from_glam(self.to_glam().clamp_length_max(length.unwrap_or(1.0))) } @@ -242,29 +182,13 @@ impl Vector3 { pub fn move_toward(self, to: Self, delta: real) -> Self { let vd = to - self; let len = vd.length(); - if len <= delta || len < CMP_EPSILON { + if len <= delta || len < real::CMP_EPSILON { to } else { self + vd / len * delta } } - pub fn posmod(self, pmod: real) -> Self { - Self::new( - fposmod(self.x, pmod), - fposmod(self.y, pmod), - fposmod(self.z, pmod), - ) - } - - pub fn posmodv(self, modv: Self) -> Self { - Self::new( - fposmod(self.x, modv.x), - fposmod(self.y, modv.y), - fposmod(self.z, modv.z), - ) - } - pub fn project(self, b: Self) -> Self { Self::from_glam(self.to_glam().project_onto(b.to_glam())) } @@ -277,10 +201,6 @@ impl Vector3 { Self::from_glam(self.to_glam().round()) } - pub fn sign(self) -> Self { - Self::new(sign(self.x), sign(self.y), sign(self.z)) - } - pub fn signed_angle_to(self, to: Self, axis: Self) -> real { let cross_to = self.cross(to); let unsigned_angle = self.dot(to).atan2(cross_to.length()); @@ -320,7 +240,7 @@ impl Vector3 { let unit_axis = axis.normalized(); let start_length = start_length_sq.sqrt(); - let result_length = lerp(start_length, end_length_sq.sqrt(), weight); + let result_length = start_length.lerp(end_length_sq.sqrt(), weight); let angle = self.angle_to(to); self.rotated(unit_axis, angle * weight) * (result_length / start_length) } @@ -329,14 +249,6 @@ impl Vector3 { self - normal * self.dot(normal) } - pub fn snapped(self, step: Self) -> Self { - Self::new( - snapped(self.x, step.x), - snapped(self.y, step.y), - snapped(self.z, step.z), - ) - } - /// Returns this vector rotated around `axis` by `angle` radians. `axis` must be normalized. /// /// # Panics @@ -359,7 +271,8 @@ impl fmt::Display for Vector3 { } impl_common_vector_fns!(Vector3, real); -impl_float_vector_fns!(Vector3, real); +impl_float_vector_glam_fns!(Vector3, real); +impl_float_vector_component_fns!(Vector3, real, (x, y, z)); impl_vector_operators!(Vector3, real, (x, y, z)); impl_from_tuple_for_vector3x!(Vector3, real); @@ -369,15 +282,6 @@ unsafe impl GodotFfi for Vector3 { ffi_methods! { type sys::GDExtensionTypePtr = *mut Self; .. } } -impl ApproxEq for Vector3 { - #[inline] - fn approx_eq(&self, other: &Self) -> bool { - is_equal_approx(self.x, other.x) - && is_equal_approx(self.y, other.y) - && is_equal_approx(self.z, other.z) - } -} - impl GlamType for RVec3 { type Mapped = Vector3; diff --git a/godot-core/src/builtin/vectors/vector4.rs b/godot-core/src/builtin/vectors/vector4.rs index c75f8e93b..6a81fa7a6 100644 --- a/godot-core/src/builtin/vectors/vector4.rs +++ b/godot-core/src/builtin/vectors/vector4.rs @@ -7,7 +7,7 @@ use godot_ffi as sys; use sys::{ffi_methods, GodotFfi}; -use crate::builtin::math::{is_equal_approx, ApproxEq, GlamConv, GlamType}; +use crate::builtin::math::{FloatExt, GlamConv, GlamType}; use crate::builtin::{real, RVec4, Vector4i}; use std::fmt; @@ -40,7 +40,8 @@ pub struct Vector4 { impl_vector_operators!(Vector4, real, (x, y, z, w)); impl_common_vector_fns!(Vector4, real); -impl_float_vector_fns!(Vector4, real); +impl_float_vector_glam_fns!(Vector4, real); +impl_float_vector_component_fns!(Vector4, real, (x, y, z, w)); impl_from_tuple_for_vector4x!(Vector4, real); impl Vector4 { @@ -101,16 +102,6 @@ unsafe impl GodotFfi for Vector4 { ffi_methods! { type sys::GDExtensionTypePtr = *mut Self; .. } } -impl ApproxEq for Vector4 { - #[inline] - fn approx_eq(&self, other: &Self) -> bool { - is_equal_approx(self.x, other.x) - && is_equal_approx(self.y, other.y) - && is_equal_approx(self.z, other.z) - && is_equal_approx(self.w, other.w) - } -} - impl GlamType for RVec4 { type Mapped = Vector4; diff --git a/godot-core/src/builtin/vectors/vector_macros.rs b/godot-core/src/builtin/vectors/vector_macros.rs index 2801ca996..92cc84d38 100644 --- a/godot-core/src/builtin/vectors/vector_macros.rs +++ b/godot-core/src/builtin/vectors/vector_macros.rs @@ -256,7 +256,7 @@ macro_rules! impl_common_vector_fns { /// Implements common constants and methods for floating-point type vectors. Works for any vector /// type that has `to_glam` and `from_glam` functions. -macro_rules! impl_float_vector_fns { +macro_rules! impl_float_vector_glam_fns { ( // Name of the vector type. $Vector:ty, @@ -282,6 +282,107 @@ macro_rules! impl_float_vector_fns { }; } +/// Implements common constants and methods for floating-point type vectors based on their components. +macro_rules! impl_float_vector_component_fns { + ( + // Name of the vector type. + $Vector:ty, + // Type of target component, for example `real`. + $Scalar:ty, + // Names of the components, with parentheses, for example `(x, y)`. + ($($comp:ident),*) + ) => { + impl $Vector { + pub fn lerp(self, other: Self, weight: $Scalar) -> Self { + Self::new( + $( + self.$comp.lerp(other.$comp, weight) + ),* + ) + } + + pub fn bezier_derivative(self, control_1: Self, control_2: Self, end: Self, t: $Scalar) -> Self { + $( + let $comp = self.$comp.bezier_derivative(control_1.$comp, control_2.$comp, end.$comp, t); + )* + + Self::new($($comp),*) + } + + pub fn bezier_interpolate(self, control_1: Self, control_2: Self, end: Self, t: $Scalar) -> Self { + $( + let $comp = self.$comp.bezier_interpolate(control_1.$comp, control_2.$comp, end.$comp, t); + )* + + Self::new($($comp),*) + } + + pub fn cubic_interpolate(self, b: Self, pre_a: Self, post_b: Self, weight: $Scalar) -> Self { + $( + let $comp = self.$comp.cubic_interpolate(b.$comp, pre_a.$comp, post_b.$comp, weight); + )* + + Self::new($($comp),*) + } + + #[allow(clippy::too_many_arguments)] + pub fn cubic_interpolate_in_time( + self, + b: Self, + pre_a: Self, + post_b: Self, + weight: $Scalar, + b_t: $Scalar, + pre_a_t: $Scalar, + post_b_t: $Scalar, + ) -> Self { + $( + let $comp = self.$comp.cubic_interpolate_in_time( + b.$comp, pre_a.$comp, post_b.$comp, weight, b_t, pre_a_t, post_b_t, + ); + )* + + Self::new($($comp),*) + } + + pub fn is_zero_approx(self) -> bool { + $(self.$comp.is_zero_approx())&&* + } + + pub fn posmod(self, pmod: $Scalar) -> Self { + Self::new( + $( self.$comp.fposmod(pmod) ),* + ) + } + + pub fn posmodv(self, modv: Self) -> Self { + Self::new( + $( self.$comp.fposmod(modv.$comp) ),* + ) + } + + pub fn sign(self) -> Self { + Self::new( + $( self.$comp.sign() ),* + ) + } + + pub fn snapped(self, step: Self) -> Self { + Self::new( + $( self.$comp.snapped(step.$comp) ),* + ) + } + } + + impl $crate::builtin::math::ApproxEq for $Vector { + #[inline] + fn approx_eq(&self, other: &Self) -> bool { + $( self.$comp.is_equal_approx(other.$comp) )&&* + } + } + }; +} + macro_rules! impl_from_tuple_for_vector2x { ( $Vector:ty, diff --git a/godot/src/lib.rs b/godot/src/lib.rs index 415a6edb5..aed084435 100644 --- a/godot/src/lib.rs +++ b/godot/src/lib.rs @@ -167,6 +167,7 @@ pub mod prelude { pub use super::bind::property::{Export, Property, TypeStringHint}; pub use super::bind::{godot_api, FromVariant, GodotClass, ToVariant}; + pub use super::builtin::math::FloatExt as _; pub use super::builtin::*; pub use super::builtin::{array, dict, varray}; // Re-export macros. pub use super::engine::{