From 77c55be90637806cd5faa873c816c3f910962bfa Mon Sep 17 00:00:00 2001 From: Tim Date: Sat, 21 Dec 2024 23:33:43 +0000 Subject: [PATCH] Add `Vec3::slerp` (#583) --- benches/vec3.rs | 10 +++++++ benches/vec3a.rs | 10 +++++++ codegen/templates/vec.rs.tera | 51 ++++++++++++++++++++++++++++++++++- src/f32/coresimd/vec3a.rs | 45 ++++++++++++++++++++++++++++++- src/f32/neon/vec3a.rs | 45 ++++++++++++++++++++++++++++++- src/f32/scalar/vec3a.rs | 45 ++++++++++++++++++++++++++++++- src/f32/sse2/vec3a.rs | 45 ++++++++++++++++++++++++++++++- src/f32/vec2.rs | 2 +- src/f32/vec3.rs | 45 ++++++++++++++++++++++++++++++- src/f32/wasm32/vec3a.rs | 45 ++++++++++++++++++++++++++++++- src/f64/dvec2.rs | 2 +- src/f64/dvec3.rs | 45 ++++++++++++++++++++++++++++++- tests/vec3.rs | 44 ++++++++++++++++++++++++++++++ 13 files changed, 424 insertions(+), 10 deletions(-) diff --git a/benches/vec3.rs b/benches/vec3.rs index 58c013a8..122ec9e7 100644 --- a/benches/vec3.rs +++ b/benches/vec3.rs @@ -184,6 +184,15 @@ bench_select!( from => random_vec3 ); +bench_trinop!( + vec3_slerp, + "vec3 slerp", + op => slerp, + from1 => random_vec3, + from2 => random_vec3, + from3 => random_f32 +); + criterion_group!( benches, vec3_angle_between, @@ -203,6 +212,7 @@ criterion_group!( vec3_to_array_into, vec3_to_rgb, vec3_to_tuple_into, + vec3_slerp, ); criterion_main!(benches); diff --git a/benches/vec3a.rs b/benches/vec3a.rs index 6e034534..857e0956 100644 --- a/benches/vec3a.rs +++ b/benches/vec3a.rs @@ -154,6 +154,15 @@ bench_select!( from => random_vec3a ); +bench_trinop!( + vec3a_slerp, + "vec3a slerp", + op => slerp, + from1 => random_vec3a, + from2 => random_vec3a, + from3 => random_f32 +); + criterion_group!( benches, vec3a_normalize_bench, @@ -171,6 +180,7 @@ criterion_group!( vec3a_to_rgb, vec3a_to_tuple_into, vec3a_to_vec3, + vec3a_slerp, ); criterion_main!(benches); diff --git a/codegen/templates/vec.rs.tera b/codegen/templates/vec.rs.tera index 468cf321..7eb16d70 100644 --- a/codegen/templates/vec.rs.tera +++ b/codegen/templates/vec.rs.tera @@ -34,12 +34,14 @@ {% set vec3_t = "Vec3" %} {% set vec3a_t = "Vec3A" %} {% set vec4_t = "Vec4" %} + {% set quat_t = "Quat" %} {% elif scalar_t == "f64" %} {% set self_t = "DVec" ~ dim %} {% set vec2_t = "DVec2" %} {% set vec3_t = "DVec3" %} {% set vec4_t = "DVec4" %} {% set from_types = ["Vec" ~ dim, "IVec" ~ dim, "UVec" ~ dim] %} + {% set quat_t = "DQuat" %} {% endif %} {% elif scalar_t == "i8" %} {% set is_signed = true %} @@ -199,6 +201,10 @@ {% endif %} {% if is_float %} {{ scalar_t }}::math, + {% if dim == 3 %} + FloatExt, + {{ quat_t }}, + {% endif %} {% endif %} {% if from_types %} {% for ty in from_types %} @@ -2163,6 +2169,49 @@ impl {{ self_t }} { Self::new(b, sign + self.y * self.y * a, -self.y), ) } + + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: {{ scalar_t }}) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize(){% if is_align %}.into(){% endif %}; + let rotation = {{ quat_t }}::from_axis_angle(axis, core::{{ scalar_t }}::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } {% endif %} {% endif %} @@ -2205,7 +2254,7 @@ impl {{ self_t }} { /// Rotates towards `rhs` up to `max_angle` (in radians). /// /// When `max_angle` is `0.0`, the result will be equal to `self`. When `max_angle` is equal to - /// `self.angle_between(rhs)`, the result will be equal to `rhs`. If `max_angle` is negative, + /// `self.angle_between(rhs)`, the result will be parallel to `rhs`. If `max_angle` is negative, /// rotates towards the exact opposite of `rhs`. Will not go past the target. #[inline] #[must_use] diff --git a/src/f32/coresimd/vec3a.rs b/src/f32/coresimd/vec3a.rs index d0be62dc..2912a20b 100644 --- a/src/f32/coresimd/vec3a.rs +++ b/src/f32/coresimd/vec3a.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{coresimd::*, f32::math, BVec3, BVec3A, Vec2, Vec3, Vec4}; +use crate::{coresimd::*, f32::math, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec3, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -939,6 +939,49 @@ impl Vec3A { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize().into(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f32/neon/vec3a.rs b/src/f32/neon/vec3a.rs index 244aaf2d..930336a8 100644 --- a/src/f32/neon/vec3a.rs +++ b/src/f32/neon/vec3a.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f32::math, neon::*, BVec3, BVec3A, Vec2, Vec3, Vec4}; +use crate::{f32::math, neon::*, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec3, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -983,6 +983,49 @@ impl Vec3A { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize().into(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f32/scalar/vec3a.rs b/src/f32/scalar/vec3a.rs index 503568ab..91333f1e 100644 --- a/src/f32/scalar/vec3a.rs +++ b/src/f32/scalar/vec3a.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f32::math, BVec3, BVec3A, Vec2, Vec3, Vec4}; +use crate::{f32::math, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec3, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -986,6 +986,49 @@ impl Vec3A { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize().into(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f32/sse2/vec3a.rs b/src/f32/sse2/vec3a.rs index d345490f..f1247b55 100644 --- a/src/f32/sse2/vec3a.rs +++ b/src/f32/sse2/vec3a.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f32::math, sse2::*, BVec3, BVec3A, Vec2, Vec3, Vec4}; +use crate::{f32::math, sse2::*, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec3, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -991,6 +991,49 @@ impl Vec3A { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize().into(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f32/vec2.rs b/src/f32/vec2.rs index 79bac00b..e1b2e600 100644 --- a/src/f32/vec2.rs +++ b/src/f32/vec2.rs @@ -921,7 +921,7 @@ impl Vec2 { /// Rotates towards `rhs` up to `max_angle` (in radians). /// /// When `max_angle` is `0.0`, the result will be equal to `self`. When `max_angle` is equal to - /// `self.angle_between(rhs)`, the result will be equal to `rhs`. If `max_angle` is negative, + /// `self.angle_between(rhs)`, the result will be parallel to `rhs`. If `max_angle` is negative, /// rotates towards the exact opposite of `rhs`. Will not go past the target. #[inline] #[must_use] diff --git a/src/f32/vec3.rs b/src/f32/vec3.rs index b16a291a..c0494ddb 100644 --- a/src/f32/vec3.rs +++ b/src/f32/vec3.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f32::math, BVec3, BVec3A, Vec2, Vec4}; +use crate::{f32::math, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -976,6 +976,49 @@ impl Vec3 { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f32/wasm32/vec3a.rs b/src/f32/wasm32/vec3a.rs index ea7dee21..e9185414 100644 --- a/src/f32/wasm32/vec3a.rs +++ b/src/f32/wasm32/vec3a.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f32::math, wasm32::*, BVec3, BVec3A, Vec2, Vec3, Vec4}; +use crate::{f32::math, wasm32::*, BVec3, BVec3A, FloatExt, Quat, Vec2, Vec3, Vec4}; use core::fmt; use core::iter::{Product, Sum}; @@ -954,6 +954,49 @@ impl Vec3A { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f32) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize().into(); + let rotation = Quat::from_axis_angle(axis, core::f32::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f64`. #[inline] #[must_use] diff --git a/src/f64/dvec2.rs b/src/f64/dvec2.rs index c1edd9c6..a5d7de7c 100644 --- a/src/f64/dvec2.rs +++ b/src/f64/dvec2.rs @@ -921,7 +921,7 @@ impl DVec2 { /// Rotates towards `rhs` up to `max_angle` (in radians). /// /// When `max_angle` is `0.0`, the result will be equal to `self`. When `max_angle` is equal to - /// `self.angle_between(rhs)`, the result will be equal to `rhs`. If `max_angle` is negative, + /// `self.angle_between(rhs)`, the result will be parallel to `rhs`. If `max_angle` is negative, /// rotates towards the exact opposite of `rhs`. Will not go past the target. #[inline] #[must_use] diff --git a/src/f64/dvec3.rs b/src/f64/dvec3.rs index 249bc1dd..a13680c6 100644 --- a/src/f64/dvec3.rs +++ b/src/f64/dvec3.rs @@ -1,6 +1,6 @@ // Generated from vec.rs.tera template. Edit the template, not the generated file. -use crate::{f64::math, BVec3, BVec3A, DVec2, DVec4, IVec3, UVec3, Vec3}; +use crate::{f64::math, BVec3, BVec3A, DQuat, DVec2, DVec4, FloatExt, IVec3, UVec3, Vec3}; use core::fmt; use core::iter::{Product, Sum}; @@ -976,6 +976,49 @@ impl DVec3 { ) } + /// Performs a spherical linear interpolation between `self` and `rhs` based on the value `s`. + /// + /// When `s` is `0.0`, the result will be equal to `self`. When `s` is `1.0`, the result + /// will be equal to `rhs`. When `s` is outside of range `[0, 1]`, the result is linearly + /// extrapolated. + #[inline] + #[must_use] + pub fn slerp(self, rhs: Self, s: f64) -> Self { + let self_length = self.length(); + let rhs_length = rhs.length(); + // Cosine of the angle between the vectors [-1, 1], or NaN if either vector has a zero length + let dot = self.dot(rhs) / (self_length * rhs_length); + // If dot is close to 1 or -1, or is NaN the calculations for t1 and t2 break down + if math::abs(dot) < 1.0 - 3e-7 { + // Angle between the vectors [0, +π] + let theta = math::acos_approx(dot); + // Sine of the angle between vectors [0, 1] + let sin_theta = math::sin(theta); + let t1 = math::sin(theta * (1. - s)); + let t2 = math::sin(theta * s); + + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + // Scale the vectors to the target length and interpolate them + return (self * (result_length / self_length) * t1 + + rhs * (result_length / rhs_length) * t2) + * sin_theta.recip(); + } + if dot < 0.0 { + // Vectors are almost parallel in opposing directions + + // Create a rotation from self to rhs along some axis + let axis = self.any_orthogonal_vector().normalize(); + let rotation = DQuat::from_axis_angle(axis, core::f64::consts::PI * s); + // Interpolate vector lengths + let result_length = self_length.lerp(rhs_length, s); + rotation * self * (result_length / self_length) + } else { + // Vectors are almost parallel in the same direction, or dot was NaN + self.lerp(rhs, s) + } + } + /// Casts all elements of `self` to `f32`. #[inline] #[must_use] diff --git a/tests/vec3.rs b/tests/vec3.rs index 4f4ad052..1e49af5b 100644 --- a/tests/vec3.rs +++ b/tests/vec3.rs @@ -1101,6 +1101,50 @@ macro_rules! impl_vec3_float_tests { assert_approx_eq!(v1, v0.lerp(v1, 1.0)); }); + glam_test!(test_slerp, { + let v0 = $vec3::new(1.0, 2.0, 3.0); + let v1 = $vec3::new(4.0, 5.0, 6.0); + assert_approx_eq!(v0.slerp(v1, 0.0), v0); + assert_approx_eq!(v0.slerp(v1, 1.0), v1); + assert_approx_eq!( + v0.slerp(v1, 0.5).length(), + (v0.length() + v1.length()) / 2., + 5e-7 + ); + assert_approx_eq!( + v0.angle_between(v0.slerp(v1, 0.5)) * 2.0, + v0.angle_between(v1) + ); + assert_approx_eq!( + $vec3::new(5.0, 0.0, 0.0).slerp($vec3::new(0.0, 5.0, 0.0), 0.5), + $vec3::new(5.0, 5.0, 0.0) * std::$t::consts::FRAC_1_SQRT_2 + ); + }); + glam_test!(test_slerp_extrapolate, { + let v0 = $vec3::Y; + let v1 = $vec3::X; + // Normalized + assert_approx_eq!(v0.slerp(v1, -1.0), $vec3::NEG_X, 2e-7); + assert_approx_eq!(v0.slerp(v1, 2.0), $vec3::NEG_Y, 2e-7); + // Scaled + assert_approx_eq!(v0.slerp(v1 * 1.5, -1.0), $vec3::NEG_X * 0.5); + assert_approx_eq!(v0.slerp(v1 * 1.5, 2.0), $vec3::NEG_Y * 2.0, 4e-7); + }); + glam_test!(test_slerp_parallel, { + // Same direction + assert_approx_eq!($vec3::ONE.slerp($vec3::splat(2.), 0.5), $vec3::splat(1.5)); + assert_approx_eq!($vec3::splat(2.).slerp($vec3::ONE, 0.5), $vec3::splat(1.5)); + // Opposite direction + assert_approx_eq!($vec3::Y.slerp($vec3::NEG_Y, 0.5), $vec3::X); + assert_approx_eq!(($vec3::Y * 1.5).slerp($vec3::NEG_Y * 0.5, 0.5), $vec3::X); + assert_approx_eq!(($vec3::Y * 0.5).slerp($vec3::NEG_Y * 1.5, 0.5), $vec3::X); + }); + glam_test!(test_slerp_zero_length, { + assert_approx_eq!($vec3::ZERO.slerp($vec3::ZERO, 0.5), $vec3::ZERO); + assert_approx_eq!($vec3::ZERO.slerp($vec3::ONE, 0.5), $vec3::splat(0.5)); + assert_approx_eq!($vec3::ONE.slerp($vec3::ZERO, 0.5), $vec3::splat(0.5)); + }); + glam_test!(test_move_towards, { let v0 = $vec3::new(-1.0, -1.0, -1.0); let v1 = $vec3::new(1.0, 1.0, 1.0);