diff --git a/crates/bevy_math/src/cubic_splines.rs b/crates/bevy_math/src/cubic_splines.rs index 47682619040d0..0bd50202d5c4e 100644 --- a/crates/bevy_math/src/cubic_splines.rs +++ b/crates/bevy_math/src/cubic_splines.rs @@ -3,7 +3,7 @@ use std::{ fmt::Debug, iter::Sum, - ops::{Add, Mul, Sub}, + ops::{Add, Mul, Sub, Div}, }; use bevy_utils::{thiserror, thiserror::Error}; @@ -13,6 +13,7 @@ use glam::Vec2; /// interpolation. pub trait Point: Mul + + Div + Add + Sub + Add @@ -27,6 +28,7 @@ pub trait Point: impl Point for T where T: Mul + + Div + Add + Sub + Add @@ -364,6 +366,7 @@ pub enum CubicNurbsError { /// ``` pub struct CubicNurbs { control_points: Vec

, + weights: Vec, knot_vector: Vec, } impl CubicNurbs

{ @@ -427,11 +430,12 @@ impl CubicNurbs

{ control_points .iter_mut() - .zip(weights) - .for_each(|(p, w)| *p = *p * w); + .zip(weights.iter()) + .for_each(|(p, w)| *p = *p * *w); Ok(Self { control_points, + weights, knot_vector, }) } @@ -507,29 +511,6 @@ impl CubicNurbs

{ weights.into_iter().map(|w| w * mul).collect() } } -impl CubicGenerator

for CubicNurbs

{ - #[inline] - fn to_curve(&self) -> CubicCurve

{ - let segments = self - .control_points - .windows(4) - .zip(self.knot_vector.windows(8)) - .map(|(points, knot_vector_segment)| { - let knot_vector_segment = knot_vector_segment - .try_into() - .expect("Knot vector windows are of length 8"); - let matrix = Self::generate_matrix(knot_vector_segment); - CubicSegment::coefficients( - points - .try_into() - .expect("Points vector windows are of length 4"), - matrix, - ) - }) - .collect(); - CubicCurve { segments } - } -} /// A spline interpolated linearly between the nearest 2 points. pub struct LinearSpline { @@ -580,20 +561,23 @@ impl CubicSegment

{ #[inline] pub fn position(&self, t: f32) -> P { let [a, b, c, d] = self.coeff; - a + b * t + c * t.powi(2) + d * t.powi(3) + // Evaluate `a + bt + ct^2 + dt^3` + a + (b + (c + d * t) * t) * t } /// Instantaneous velocity of a point at parametric value `t`. #[inline] pub fn velocity(&self, t: f32) -> P { let [_, b, c, d] = self.coeff; - b + c * 2.0 * t + d * 3.0 * t.powi(2) + // Evaluate the derivative, which is `b + 2ct + 3dt^2` + b + (c * 2.0 + d * 3.0 * t) * t } /// Instantaneous acceleration of a point at parametric value `t`. #[inline] pub fn acceleration(&self, t: f32) -> P { let [_, _, c, d] = self.coeff; + // Evaluate the second derivative, which is `2c + 6dt` c * 2.0 + d * 6.0 * t } @@ -846,11 +830,287 @@ impl IntoIterator for CubicCurve

{ } } +impl RationalGenerator

for CubicNurbs

{ + #[inline] + fn to_curve(&self) -> RationalCurve

{ + let segments = self + .control_points.windows(4) + .zip(self.weights.windows(4)) + .zip(self.knot_vector.windows(8)) + .map(|((points, weights), knot_vector_segment)| { + let knot_vector_segment = knot_vector_segment + .try_into() + .expect("Knot vector windows are of length 8"); + let matrix = Self::generate_matrix(knot_vector_segment); + RationalSegment::coefficients( + points + .try_into() + .expect("Points vector windows are of length 4"), + weights + .try_into() + .expect("Weights vector windows are of length 4"), + matrix, + ) + }) + .collect(); + RationalCurve { segments } + } +} + +/// Implement this on cubic splines that can generate a curve from their spline parameters. +pub trait RationalGenerator { + /// Build a [`RationalCurve`] by computing the interpolation coefficients for each curve segment. + fn to_curve(&self) -> RationalCurve

; +} + +/// A segment of a rational cubic curve, used to hold precomputed coefficients for fast interpolation. +/// +/// Segments can be chained together to form a longer compound curve. +#[derive(Clone, Debug, Default, PartialEq)] +pub struct RationalSegment { + coeff: [P; 4], + weight_coeff: [f32; 4], +} + +impl RationalSegment

{ + /// Instantaneous position of a point at parametric value `t`. + #[inline] + pub fn position(&self, t: f32) -> P { + let [a, b, c, d] = self.coeff; + let [x, y, z, w] = self.weight_coeff; + // Compute a cubic polynomial for the control points + let numerator = a + (b + (c + d * t) * t) * t; + // Compute a cubic polynomial for the weights + let denominator = x + (y + (z + w * t) * t) * t; + numerator / denominator + } + + /// Instantaneous velocity of a point at parametric value `t`. + #[inline] + pub fn velocity(&self, t: f32) -> P { + let [a, b, c, d] = self.coeff; + let [x, y, z, w] = self.weight_coeff; + // Compute a cubic polynomial for the control points + let numerator = a + (b + (c + d * t) * t) * t; + // Compute a cubic polynomial for the weights + let denominator = x + (y + (z + w * t) * t) * t; + + // Compute the derivative of the control point polynomial + let numerator_derivative = b + (c * 2.0 + d * 3.0 * t) * t; + // Compute the derivative of the weight polynomial + let denominator_derivative = y + (z * 2.0 + w * 3.0 * t) * t; + + // Velocity is the first derivative (wrt to the parameter `t`) + // Position = N/D therefore + // Velocity = (N/D)' = N'/D - N * D'/D^2 = (N' * D - N * D')/D^2 + numerator_derivative / denominator - numerator * (denominator_derivative / denominator.powi(2)) + } + + /// Instantaneous acceleration of a point at parametric value `t`. + #[inline] + pub fn acceleration(&self, t: f32) -> P { + let [a, b, c, d] = self.coeff; + let [x, y, z, w] = self.weight_coeff; + // Compute a cubic polynomial for the control points + let numerator = a + (b + (c + d * t) * t) * t; + // Compute a cubic polynomial for the weights + let denominator = x + (y + (z + w * t) * t) * t; + + // Compute the derivative of the control point polynomial + let numerator_derivative = b + (c * 2.0 + d * 3.0 * t) * t; + // Compute the derivative of the weight polynomial + let denominator_derivative = y + (z * 2.0 + w * 3.0 * t) * t; + + // Compute the second derivative of the control point polynomial + let numerator_second_derivative = c * 2.0 + d * 6.0 * t; + // Compute the second derivative of the weight polynomial + let denominator_second_derivative = z * 2.0 + w * 6.0 * t; + + // Velocity is the first derivative (wrt to the parameter `t`) + // Position = N/D therefore + // Velocity = (N/D)' = N'/D - N * D'/D^2 = (N' * D - N * D')/D^2 + // Acceleration = (N/D)'' = ((N' * D - N * D')/D^2)' = N''/D + N' * (-2D'/D^2) + N * (-D''/D^2 + 2D'^2/D^3) + numerator_second_derivative/denominator + + numerator_derivative * (-2.0 * denominator_derivative/denominator.powi(2)) + + numerator * (-denominator_second_derivative/denominator.powi(2) + 2.0 * denominator_derivative.powi(2)/denominator.powi(3)) + } + + /// Calculate polynomial coefficients for the cubic polynomials using a characteristic matrix. + #[inline] + fn coefficients(control_points: [P; 4], weights: [f32; 4], char_matrix: [[f32; 4]; 4]) -> Self { + let [c0, c1, c2, c3] = char_matrix; + let p = control_points; + let w = weights; + // These are the control point polynomial coefficients, computed by multiplying the characteristic + // matrix by the point matrix. + let coeff = [ + p[0] * c0[0] + p[1] * c0[1] + p[2] * c0[2] + p[3] * c0[3], + p[0] * c1[0] + p[1] * c1[1] + p[2] * c1[2] + p[3] * c1[3], + p[0] * c2[0] + p[1] * c2[1] + p[2] * c2[2] + p[3] * c2[3], + p[0] * c3[0] + p[1] * c3[1] + p[2] * c3[2] + p[3] * c3[3], + ]; + // These are the weight polynomial coefficients, computed by multiplying the characteristic + // matrix by the weight matrix. + let weight_coeff = [ + w[0] * c0[0] + w[1] * c0[1] + w[2] * c0[2] + w[3] * c0[3], + w[0] * c1[0] + w[1] * c1[1] + w[2] * c1[2] + w[3] * c1[3], + w[0] * c2[0] + w[1] * c2[1] + w[2] * c2[2] + w[3] * c2[3], + w[0] * c3[0] + w[1] * c3[1] + w[2] * c3[2] + w[3] * c3[3], + ]; + Self { coeff, weight_coeff } + } +} + +/// A collection of [`RationalSegment`]s chained into a curve. +/// +/// Use any struct that implements the [`RationalGenerator`] trait to create a new curve, such as +/// [`CubicNURBS`]. +#[derive(Clone, Debug, PartialEq)] +pub struct RationalCurve { + segments: Vec>, +} + +impl RationalCurve

{ + /// Compute the position of a point on the curve at the parametric value `t`. + /// + /// Note that `t` varies from `0..=(n_points - 3)`. + #[inline] + pub fn position(&self, t: f32) -> P { + let (segment, t) = self.segment(t); + segment.position(t) + } + + /// Compute the first derivative with respect to t at `t`. This is the instantaneous velocity of + /// a point on the curve at `t`. + /// + /// Note that `t` varies from `0..=(n_points - 3)`. + #[inline] + pub fn velocity(&self, t: f32) -> P { + let (segment, t) = self.segment(t); + segment.velocity(t) + } + + /// Compute the second derivative with respect to t at `t`. This is the instantaneous + /// acceleration of a point on the curve at `t`. + /// + /// Note that `t` varies from `0..=(n_points - 3)`. + #[inline] + pub fn acceleration(&self, t: f32) -> P { + let (segment, t) = self.segment(t); + segment.acceleration(t) + } + + /// A flexible iterator used to sample curves with arbitrary functions. + /// + /// This splits the curve into `subdivisions` of evenly spaced `t` values across the + /// length of the curve from start (t = 0) to end (t = n), where `n = self.segment_count()`, + /// returning an iterator evaluating the curve with the supplied `sample_function` at each `t`. + /// + /// For `subdivisions = 2`, this will split the curve into two lines, or three points, and + /// return an iterator with 3 items, the three points, one at the start, middle, and end. + #[inline] + pub fn iter_samples<'a, 'b: 'a>( + &'b self, + subdivisions: usize, + mut sample_function: impl FnMut(&Self, f32) -> P + 'a, + ) -> impl Iterator + 'a { + self.iter_uniformly(subdivisions) + .map(move |t| sample_function(self, t)) + } + + /// An iterator that returns values of `t` uniformly spaced over `0..=subdivisions`. + #[inline] + fn iter_uniformly(&self, subdivisions: usize) -> impl Iterator { + let segments = self.segments.len() as f32; + let step = segments / subdivisions as f32; + (0..=subdivisions).map(move |i| i as f32 * step) + } + + /// The list of segments contained in this `RationalCurve`. + /// + /// This spline's global `t` value is equal to how many segments it has. + /// + /// All method accepting `t` on `RationalCurve` depends on the global `t`. + /// When sampling over the entire curve, you should either use one of the + /// `iter_*` methods or account for the segment count using `curve.segments().len()`. + #[inline] + pub fn segments(&self) -> &[RationalSegment

] { + &self.segments + } + + /// Iterate over the curve split into `subdivisions`, sampling the position at each step. + pub fn iter_positions(&self, subdivisions: usize) -> impl Iterator + '_ { + self.iter_samples(subdivisions, Self::position) + } + + /// Iterate over the curve split into `subdivisions`, sampling the velocity at each step. + pub fn iter_velocities(&self, subdivisions: usize) -> impl Iterator + '_ { + self.iter_samples(subdivisions, Self::velocity) + } + + /// Iterate over the curve split into `subdivisions`, sampling the acceleration at each step. + pub fn iter_accelerations(&self, subdivisions: usize) -> impl Iterator + '_ { + self.iter_samples(subdivisions, Self::acceleration) + } + + #[inline] + /// Adds a segment to the curve + pub fn push_segment(&mut self, segment: RationalSegment

) { + self.segments.push(segment); + } + + /// Returns the [`RationalSegment`] and local `t` value given a spline's global `t` value. + #[inline] + fn segment(&self, t: f32) -> (&RationalSegment

, f32) { + if self.segments.len() == 1 { + (&self.segments[0], t) + } else { + let i = (t.floor() as usize).clamp(0, self.segments.len() - 1); + (&self.segments[i], t - i as f32) + } + } +} + +impl Extend> for RationalCurve

{ + fn extend>>(&mut self, iter: T) { + self.segments.extend(iter); + } +} + +impl IntoIterator for RationalCurve

{ + type IntoIter = > as IntoIterator>::IntoIter; + + type Item = RationalSegment

; + + fn into_iter(self) -> Self::IntoIter { + self.segments.into_iter() + } +} + +impl From> for RationalSegment

{ + fn from(value: CubicSegment

) -> Self { + Self { + coeff: value.coeff, + weight_coeff: [1.0, 0.0, 0.0, 0.0], + } + } +} + +impl From> for RationalCurve

{ + fn from(value: CubicCurve

) -> Self { + Self { + segments: value.segments.into_iter().map(Into::into).collect() + } + } +} + #[cfg(test)] mod tests { use glam::{vec2, Vec2}; - use crate::cubic_splines::{CubicBezier, CubicGenerator, CubicSegment}; + use crate::cubic_splines::{CubicBezier, CubicBSpline, CubicGenerator, CubicSegment}; + + use super::RationalCurve; /// How close two floats can be and still be considered equal const FLOAT_EQ: f32 = 1e-5; @@ -913,4 +1173,42 @@ mod tests { assert!(bezier.ease(0.5) < -0.5); assert_eq!(bezier.ease(1.0), 1.0); } + + #[test] + fn cubic_to_rational() { + const EPSILON: f32 = 0.00001; + + let points = [ + vec2(0.0, 0.0), + vec2(1.0, 1.0), + vec2(1.0, 1.0), + vec2(2.0, -1.0), + vec2(3.0, 1.0), + vec2(0.0, 0.0), + ]; + + let b_spline = CubicBSpline::new(points).to_curve(); + let rational_b_spline = RationalCurve::from(b_spline.clone()); + + /// Tests if two vectors of points are approximately the same + fn compare_vectors(cubic_curve: Vec, rational_curve: Vec, name: &str) { + assert_eq!(cubic_curve.len(), rational_curve.len(), "{name} vector lengths mismatch"); + for (a,b) in cubic_curve.iter().zip(rational_curve.iter()) { + assert!(a.distance(*b) < EPSILON, "Mismatch between {name} values CubicCurve: {} Converted RationalCurve: {}", a, b); + } + } + + // Both curves should yield the same values + let cubic_positions: Vec<_> = b_spline.iter_positions(10).collect(); + let rational_positions: Vec<_> = rational_b_spline.iter_positions(10).collect(); + compare_vectors(cubic_positions, rational_positions, "position"); + + let cubic_velocities: Vec<_> = b_spline.iter_velocities(10).collect(); + let rational_velocities: Vec<_> = rational_b_spline.iter_velocities(10).collect(); + compare_vectors(cubic_velocities, rational_velocities, "velocity"); + + let cubic_accelerations: Vec<_> = b_spline.iter_accelerations(10).collect(); + let rational_accelerations: Vec<_> = rational_b_spline.iter_accelerations(10).collect(); + compare_vectors(cubic_accelerations, rational_accelerations, "acceleration"); + } } diff --git a/crates/bevy_math/src/lib.rs b/crates/bevy_math/src/lib.rs index faae8f720c957..85c64dc307245 100644 --- a/crates/bevy_math/src/lib.rs +++ b/crates/bevy_math/src/lib.rs @@ -25,7 +25,7 @@ pub mod prelude { pub use crate::{ cubic_splines::{ CubicBSpline, CubicBezier, CubicCardinalSpline, CubicGenerator, CubicHermite, - CubicNurbs, CubicNurbsError, CubicSegment, + CubicNurbs, CubicNurbsError, CubicCurve, CubicSegment, RationalGenerator, RationalCurve, RationalSegment }, primitives::*, BVec2, BVec3, BVec4, EulerRot, FloatExt, IRect, IVec2, IVec3, IVec4, Mat2, Mat3, Mat4,