Stable interpolation and smooth following

Cargo.toml

@@ -3035,6 +3035,17 @@ description = "Demonstrates how to sample random points from mathematical primit
 category = "Math"
 wasm = true
 
+[[example]]
+name = "smooth_follow"
+path = "examples/math/smooth_follow.rs"
+doc-scrape-examples = true
+
+[package.metadata.example.smooth_follow]
+name = "Smooth Follow"
+description = "Demonstrates how to make an entity smoothly follow another using interpolation"
+category = "Math"
+wasm = true
+
 # Gizmos
 [[example]]
 name = "2d_gizmos"

crates/bevy_math/src/common_traits.rs

@@ -1,4 +1,4 @@
-use glam::{Vec2, Vec3, Vec3A, Vec4};
+use crate::{Dir2, Dir3, Dir3A, Quat, Rot2, Vec2, Vec3, Vec3A, Vec4};
 use std::fmt::Debug;
 use std::ops::{Add, Div, Mul, Neg, Sub};
 
@@ -161,3 +161,142 @@ impl NormedVectorSpace for f32 {
         self * self
     }
 }
+
+/// A type with a natural interpolation that provides strong subdivision guarantees.
+///
+/// Although the only required method is `interpolate_stable`, many things are expected of it:
+///
+/// 1. The notion of interpolation should follow naturally from the semantics of the type, so
+///    that inferring the interpolation mode from the type alone is sensible.
+///
+/// 2. The interpolation recovers something equivalent to the starting value at `t = 0.0`
+///    and likewise with the ending value at `t = 1.0`.
+///
+/// 3. Importantly, the interpolation must be *subdivision-stable*: for any interpolation curve
+///    between two (unnamed) values and any parameter-value pairs `(t0, p)` and `(t1, q)`, the
+///    interpolation curve between `p` and `q` must be the *linear* reparametrization of the original
+///    interpolation curve restricted to the interval `[t0, t1]`.
+///
+/// The last of these conditions is very strong and indicates something like constant speed. It
+/// is called "subdivision stability" because it guarantees that breaking up the interpolation
+/// into segments and joining them back together has no effect.
+///
+/// Here is a diagram depicting it:
+/// ```text
+/// top curve = u.interpolate_stable(v, t)
+///
+///              t0 => p   t1 => q    
+///   |-------------|---------|-------------|
+/// 0 => u         /           \          1 => v
+///              /               \
+///            /                   \
+///          /        linear         \
+///        /     reparametrization     \
+///      /   t = t0 * (1 - s) + t1 * s   \
+///    /                                   \
+///   |-------------------------------------|
+/// 0 => p                                1 => q
+///
+/// bottom curve = p.interpolate_stable(q, s)
+/// ```
+///
+/// Note that some common forms of interpolation do not satisfy this criterion. For example,
+/// [`Quat::lerp`] and [`Rot2::nlerp`] are not subdivision-stable.
+///
+/// Furthermore, this is not to be used as a general trait for abstract interpolation.
+/// Consumers rely on the strong guarantees in order for behavior based on this trait to be
+/// well-behaved.
+///
+/// [`Quat::lerp`]: crate::Quat::lerp
+/// [`Rot2::nlerp`]: crate::Rot2::nlerp
+pub trait StableInterpolate: Clone {
+    /// Interpolate between this value and the `other` given value using the parameter `t`.
+    /// Note that the parameter `t` is not necessarily clamped to lie between `0` and `1`.
+    /// When `t = 0.0`, `self` is recovered, while `other` is recovered at `t = 1.0`,
+    /// with intermediate values lying between the two.
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self;
+
+    /// A version of [`interpolate_stable`] that assigns the result to `self` for convenience.
+    ///
+    /// [`interpolate_stable`]: StableInterpolate::interpolate_stable
+    fn interpolate_stable_assign(&mut self, other: &Self, t: f32) {
+        *self = self.interpolate_stable(other, t);
+    }
+
+    /// Smoothly nudge this value towards the `target` at a given decay rate. The `decay_rate`
+    /// parameter controls how fast the distance between `self` and `target` decays relative to
+    /// the units of `delta`; the intended usage is for `decay_rate` to generally remain fixed,
+    /// while `delta` is something like `delta_time` from an updating system. This produces a
+    /// smooth following of the target that is independent of framerate.
+    ///
+    /// More specifically, when this is called repeatedly, the result is that the distance between
+    /// `self` and a fixed `target` attenuates exponentially, with the rate of this exponential
+    /// decay given by `decay_rate`.
+    ///
+    /// For example, at `decay_rate = 0.0`, this has no effect.
+    /// At `decay_rate = f32::INFINITY`, `self` immediately snaps to `target`.
+    /// In general, higher rates mean that `self` moves more quickly towards `target`.
+    ///
+    /// # Example
+    /// ```
+    /// # use bevy_math::{Vec3, StableInterpolate};
+    /// # let delta_time: f32 = 1.0 / 60.0;
+    /// let mut object_position: Vec3 = Vec3::ZERO;
+    /// let target_position: Vec3 = Vec3::new(2.0, 3.0, 5.0);
+    /// // Decay rate of ln(10) => after 1 second, remaining distance is 1/10th
+    /// let decay_rate = f32::ln(10.0);
+    /// // Calling this repeatedly will move `object_position` towards `target_position`:
+    /// object_position.smooth_nudge(&target_position, decay_rate, delta_time);
+    /// ```
+    fn smooth_nudge(&mut self, target: &Self, decay_rate: f32, delta: f32) {
+        self.interpolate_stable_assign(target, 1.0 - f32::exp(-decay_rate * delta));
+    }
+}
+
+// Conservatively, we presently only apply this for normed vector spaces, where the notion
+// of being constant-speed is literally true. The technical axioms are satisfied for any
+// VectorSpace type, but the "natural from the semantics" part is less clear in general.
+impl<V> StableInterpolate for V
+where
+    V: NormedVectorSpace,
+{
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.lerp(*other, t)
+    }
+}
+
+impl StableInterpolate for Rot2 {
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.slerp(*other, t)
+    }
+}
+
+impl StableInterpolate for Quat {
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.slerp(*other, t)
+    }
+}
+
+impl StableInterpolate for Dir2 {
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.slerp(*other, t)
+    }
+}
+
+impl StableInterpolate for Dir3 {
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.slerp(*other, t)
+    }
+}
+
+impl StableInterpolate for Dir3A {
+    #[inline]
+    fn interpolate_stable(&self, other: &Self, t: f32) -> Self {
+        self.slerp(*other, t)
+    }
+}

crates/bevy_math/src/lib.rs

@@ -53,8 +53,8 @@ pub mod prelude {
         direction::{Dir2, Dir3, Dir3A},
         primitives::*,
         BVec2, BVec3, BVec4, EulerRot, FloatExt, IRect, IVec2, IVec3, IVec4, Mat2, Mat3, Mat4,
-        Quat, Ray2d, Ray3d, Rect, Rot2, URect, UVec2, UVec3, UVec4, Vec2, Vec2Swizzles, Vec3,
-        Vec3Swizzles, Vec4, Vec4Swizzles,
+        Quat, Ray2d, Ray3d, Rect, Rot2, StableInterpolate, URect, UVec2, UVec3, UVec4, Vec2,
+        Vec2Swizzles, Vec3, Vec3Swizzles, Vec4, Vec4Swizzles,
     };
 }
 

examples/README.md

@@ -325,6 +325,7 @@ Example | Description
 [Random Sampling](../examples/math/random_sampling.rs) | Demonstrates how to sample random points from mathematical primitives
 [Rendering Primitives](../examples/math/render_primitives.rs) | Shows off rendering for all math primitives as both Meshes and Gizmos
 [Sampling Primitives](../examples/math/sampling_primitives.rs) | Demonstrates all the primitives which can be sampled.
+[Smooth Follow](../examples/math/smooth_follow.rs) | Demonstrates how to make an entity smoothly follow another using interpolation
 
 ## Reflection
 

examples/math/smooth_follow.rs

@@ -0,0 +1,145 @@
+//! This example demonstrates how to use interpolation to make one entity smoothly follow another.
+
+use bevy::math::{prelude::*, vec3, NormedVectorSpace};
+use bevy::prelude::*;
+use rand::SeedableRng;
+use rand_chacha::ChaCha8Rng;
+use std::cmp::min_by;
+
+fn main() {
+    App::new()
+        .add_plugins(DefaultPlugins)
+        .add_systems(Startup, setup)
+        .add_systems(Update, (move_target, move_follower).chain())
+        .run();
+}
+
+// The sphere that the following sphere targets at all times:
+#[derive(Component)]
+struct TargetSphere;
+
+// The speed of the target sphere moving to its next location:
+#[derive(Resource)]
+struct TargetSphereSpeed(f32);
+
+// The position that the target sphere always moves linearly toward:
+#[derive(Resource)]
+struct TargetPosition(Vec3);
+
+// The decay rate used by the smooth following:
+#[derive(Resource)]
+struct DecayRate(f32);
+
+// The sphere that follows the target sphere by moving towards it with nudging:
+#[derive(Component)]
+struct FollowingSphere;
+
+/// The source of randomness used by this example.
+#[derive(Resource)]
+struct RandomSource(ChaCha8Rng);
+
+fn setup(
+    mut commands: Commands,
+    mut meshes: ResMut<Assets<Mesh>>,
+    mut materials: ResMut<Assets<StandardMaterial>>,
+) {
+    // A plane:
+    commands.spawn(PbrBundle {
+        mesh: meshes.add(Plane3d::default().mesh().size(12.0, 12.0)),
+        material: materials.add(Color::srgb(0.3, 0.15, 0.3)),
+        transform: Transform::from_xyz(0.0, -2.5, 0.0),
+        ..default()
+    });
+
+    // The target sphere:
+    commands.spawn((
+        PbrBundle {
+            mesh: meshes.add(Sphere::new(0.3)),
+            material: materials.add(Color::srgb(0.3, 0.15, 0.9)),
+            ..default()
+        },
+        TargetSphere,
+    ));
+
+    // The sphere that follows it:
+    commands.spawn((
+        PbrBundle {
+            mesh: meshes.add(Sphere::new(0.3)),
+            material: materials.add(Color::srgb(0.9, 0.3, 0.3)),
+            transform: Transform::from_translation(vec3(0.0, -2.0, 0.0)),
+            ..default()
+        },
+        FollowingSphere,
+    ));
+
+    // A light:
+    commands.spawn(PointLightBundle {
+        point_light: PointLight {
+            intensity: 15_000_000.0,
+            shadows_enabled: true,
+            ..default()
+        },
+        transform: Transform::from_xyz(4.0, 8.0, 4.0),
+        ..default()
+    });
+
+    // A camera:
+    commands.spawn(Camera3dBundle {
+        transform: Transform::from_xyz(-2.0, 3.0, 5.0).looking_at(Vec3::ZERO, Vec3::Y),
+        ..default()
+    });
+
+    // Set starting values for resources used by the systems:
+    commands.insert_resource(TargetSphereSpeed(5.0));
+    commands.insert_resource(DecayRate(2.0));
+    commands.insert_resource(TargetPosition(Vec3::ZERO));
+    commands.insert_resource(RandomSource(ChaCha8Rng::seed_from_u64(68941654987813521)));
+}
+
+fn move_target(
+    mut target: Query<&mut Transform, With<TargetSphere>>,
+    target_speed: Res<TargetSphereSpeed>,
+    mut target_pos: ResMut<TargetPosition>,
+    time: Res<Time>,
+    mut rng: ResMut<RandomSource>,
+) {
+    let mut target = target.single_mut();
+
+    match Dir3::new(target_pos.0 - target.translation) {
+        // The target and the present position of the target sphere are far enough to have a well-
+        // defined direction between them, so let's move closer:
+        Ok(dir) => {
+            let delta_time = time.delta_seconds();
+            let abs_delta = (target_pos.0 - target.translation).norm();
+
+            // Avoid overshooting in case of high values of `delta_time``:
+            let magnitude = min_by(abs_delta, delta_time * target_speed.0, |f0, f1| {
+                f0.partial_cmp(f1).unwrap()
+            });
+            target.translation += dir * magnitude;
+        }
+
+        // The two are really close, so let's generate a new target position:
+        Err(_) => {
+            let legal_region = Cuboid::from_size(Vec3::splat(4.0));
+            *target_pos = TargetPosition(legal_region.sample_interior(&mut rng.0));
+        }
+    }
+}
+
+fn move_follower(
+    mut following: Query<&mut Transform, With<FollowingSphere>>,
+    target: Query<&Transform, (With<TargetSphere>, Without<FollowingSphere>)>,
+    decay_rate: Res<DecayRate>,
+    time: Res<Time>,
+) {
+    let target = target.single();
+    let mut following = following.single_mut();
+    let decay_rate = decay_rate.0;
+    let delta_time = time.delta_seconds();
+
+    // Calling `smooth_nudge` is what moves the following sphere smoothly toward the target.
+    following
+        .translation
+        .smooth_nudge(&target.translation, decay_rate, delta_time);
+}