lightyear_avian2d 0.29.0

//! Implement lightyear traits for some common bevy types
use avian2d::math::{AdjustPrecision, AsF32, Quaternion, Scalar};
use avian2d::prelude::*;
use bevy_math::Vec3;
use bevy_transform::components::Transform;
use bevy_transform_interpolation::hermite::{hermite_quat, hermite_vec3};
use lightyear_interpolation::prelude::InterpolationSampleContext;
use tracing::trace;

#[cfg(feature = "deterministic")]
use core::hash::Hasher;

pub mod position {
    use super::*;

    pub fn lerp(start: &Position, other: &Position, t: f32) -> Position {
        let u = Scalar::from(t);
        let res = Position::new(start.0 * (1.0 - u) + other.0 * u);
        trace!(
            "position lerp: start: {:?} end: {:?} t: {} res: {:?}",
            start, other, t, res
        );
        res
    }

    #[cfg(feature = "deterministic")]
    pub fn hash(pos: &Position, hasher: &mut seahash::SeaHasher) {
        hasher.write_u32(pos.x.to_bits());
        hasher.write_u32(pos.y.to_bits());
    }
}

pub mod rotation {
    use super::*;

    pub fn lerp(start: &Rotation, other: &Rotation, t: f32) -> Rotation {
        let u = Scalar::from(t);
        let shortest_angle =
            ((((other.as_degrees() - start.as_degrees()) % 360.0) + 540.0) % 360.0) - 180.0;
        let res = Rotation::degrees(start.as_degrees() + shortest_angle * u);
        // // as_radians() returns a value between -Pi and Pi
        // // add Pi to get positive values, for interpolation
        // let res = Rotation::from_radians(
        //     (start.as_radians() + std::f32::consts::PI) * (1.0 - t)
        //         + (other.as_radians() + std::f32::consts::PI) * t,
        // );
        trace!(
            "rotation lerp: start: {:?} end: {:?} t: {} res: {:?}",
            start.as_degrees(),
            other.as_degrees(),
            t,
            res.as_degrees()
        );
        res
    }

    #[cfg(feature = "deterministic")]
    pub fn hash(rot: &Rotation, hasher: &mut seahash::SeaHasher) {
        hasher.write_u32(rot.cos.to_bits());
        hasher.write_u32(rot.sin.to_bits());
    }
}

pub mod linear_velocity {
    use super::*;

    pub fn lerp(start: &LinearVelocity, other: &LinearVelocity, t: f32) -> LinearVelocity {
        let u = Scalar::from(t);
        let res = LinearVelocity(start.0 * (1.0 - u) + other.0 * u);
        trace!(
            "linear velocity lerp: start: {:?} end: {:?} t: {} res: {:?}",
            start, other, t, res
        );
        res
    }
}

pub mod angular_velocity {
    use super::*;

    pub fn lerp(start: &AngularVelocity, other: &AngularVelocity, t: f32) -> AngularVelocity {
        let u = Scalar::from(t);
        let res = AngularVelocity(start.0 * (1.0 - u) + other.0 * u);
        trace!(
            "angular velocity lerp: start: {:?} end: {:?} t: {} res: {:?}",
            start, other, t, res
        );
        res
    }
}

pub mod position_rotation {
    use super::*;

    /// Interpolates `(Position, Rotation, LinearVelocity, AngularVelocity)`
    /// using Hermite interpolation for position and rotation.
    ///
    /// This is intended for [`FrameInterpolate`](lightyear_core::prelude::FrameInterpolate)
    /// and prediction correction in
    /// [`AvianReplicationMode::Position`](crate::plugin::AvianReplicationMode::Position).
    /// Avian velocities are expressed per second, so
    /// [`InterpolationSampleContext::sample_delta_secs`] scales the endpoint
    /// tangents to the bracketing sample interval. If that interval is
    /// unavailable, position falls back to linear interpolation and rotation
    /// falls back to shortest-path interpolation.
    ///
    /// [`LightyearAvianPlugin`](crate::plugin::LightyearAvianPlugin)
    /// automatically registers this as a full bundle rule in
    /// [`AvianReplicationMode::Position`](crate::plugin::AvianReplicationMode::Position).
    pub fn hermite(
        start: (Position, Rotation, LinearVelocity, AngularVelocity),
        end: (Position, Rotation, LinearVelocity, AngularVelocity),
        ctx: InterpolationSampleContext,
    ) -> (Position, Rotation, LinearVelocity, AngularVelocity) {
        let (position, rotation) = if let Some(sample_delta_secs) = ctx.sample_delta_secs {
            let position = hermite_vec3(
                start.0.0.f32().extend(0.0),
                end.0.0.f32().extend(0.0),
                start.2.0.f32().extend(0.0) * sample_delta_secs,
                end.2.0.f32().extend(0.0) * sample_delta_secs,
                ctx.t,
            );
            let rotation = hermite_quat(
                Quaternion::from(start.1).f32(),
                Quaternion::from(end.1).f32(),
                Vec3::Z * scalar_to_f32(start.3.0) * sample_delta_secs,
                Vec3::Z * scalar_to_f32(end.3.0) * sample_delta_secs,
                ctx.t,
                true,
            );
            (
                Position(position.truncate().adjust_precision()),
                Rotation::from(rotation),
            )
        } else {
            (
                super::position::lerp(&start.0, &end.0, ctx.t),
                super::rotation::lerp(&start.1, &end.1, ctx.t),
            )
        };

        (
            position,
            rotation,
            super::linear_velocity::lerp(&start.2, &end.2, ctx.t),
            super::angular_velocity::lerp(&start.3, &end.3, ctx.t),
        )
    }

    fn scalar_to_f32(value: Scalar) -> f32 {
        #[allow(clippy::unnecessary_cast)]
        {
            value as f32
        }
    }
}

pub mod transform {
    use super::*;

    /// Interpolates `(Transform, AngularVelocity, LinearVelocity)` using
    /// Hermite interpolation for translation and rotation.
    ///
    /// Avian velocities are expressed per second, so this function uses
    /// [`InterpolationSampleContext::sample_delta_secs`] to scale the endpoint
    /// velocities to the bracketing sample interval. If the sample interval is
    /// unavailable, it falls back to linear translation/scale interpolation and
    /// spherical rotation interpolation.
    ///
    /// Bundle interpolation requires all three component histories to have the
    /// same bracketing ticks. The returned velocity components are linearly
    /// interpolated over the same fraction.
    ///
    /// Register it as a higher-priority bundle rule when Transform-mode
    /// smoothing should use velocity-aware Hermite interpolation:
    ///
    /// ```rust,ignore
    /// app.interpolate_bundle_with_priority::<(Transform, AngularVelocity, LinearVelocity)>(
    ///     100,
    ///     InterpolationFns::interpolate_with_context(
    ///         lightyear_avian2d::types::transform::hermite,
    ///     ),
    /// );
    /// ```
    pub fn hermite(
        start: (Transform, AngularVelocity, LinearVelocity),
        end: (Transform, AngularVelocity, LinearVelocity),
        ctx: InterpolationSampleContext,
    ) -> (Transform, AngularVelocity, LinearVelocity) {
        let transform = if let Some(sample_delta_secs) = ctx.sample_delta_secs {
            let start_linear_velocity = start.2.0.f32().extend(0.0) * sample_delta_secs;
            let end_linear_velocity = end.2.0.f32().extend(0.0) * sample_delta_secs;
            let start_angular_velocity = Vec3::Z * scalar_to_f32(start.1.0) * sample_delta_secs;
            let end_angular_velocity = Vec3::Z * scalar_to_f32(end.1.0) * sample_delta_secs;
            Transform {
                translation: hermite_vec3(
                    start.0.translation,
                    end.0.translation,
                    start_linear_velocity,
                    end_linear_velocity,
                    ctx.t,
                ),
                rotation: hermite_quat(
                    start.0.rotation,
                    end.0.rotation,
                    start_angular_velocity,
                    end_angular_velocity,
                    ctx.t,
                    true,
                ),
                scale: start.0.scale.lerp(end.0.scale, ctx.t),
            }
        } else {
            linear(start.0, end.0, ctx.t)
        };

        (
            transform,
            super::angular_velocity::lerp(&start.1, &end.1, ctx.t),
            super::linear_velocity::lerp(&start.2, &end.2, ctx.t),
        )
    }

    fn linear(start: Transform, end: Transform, t: f32) -> Transform {
        Transform {
            translation: start.translation.lerp(end.translation, t),
            rotation: start.rotation.slerp(end.rotation, t),
            scale: start.scale.lerp(end.scale, t),
        }
    }

    fn scalar_to_f32(value: Scalar) -> f32 {
        #[allow(clippy::unnecessary_cast)]
        {
            value as f32
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use avian2d::math::Vector;
    use bevy_math::Quat;

    fn state(
        transform: Transform,
        angular_velocity: f32,
        linear_velocity: (f32, f32),
    ) -> (Transform, AngularVelocity, LinearVelocity) {
        (
            transform,
            AngularVelocity(Scalar::from(angular_velocity)),
            LinearVelocity(Vector::new(
                Scalar::from(linear_velocity.0),
                Scalar::from(linear_velocity.1),
            )),
        )
    }

    fn position_rotation_state(
        position: (f32, f32),
        rotation: f32,
        linear_velocity: (f32, f32),
        angular_velocity: f32,
    ) -> (Position, Rotation, LinearVelocity, AngularVelocity) {
        (
            Position(Vector::new(
                Scalar::from(position.0),
                Scalar::from(position.1),
            )),
            Rotation::radians(Scalar::from(rotation)),
            LinearVelocity(Vector::new(
                Scalar::from(linear_velocity.0),
                Scalar::from(linear_velocity.1),
            )),
            AngularVelocity(Scalar::from(angular_velocity)),
        )
    }

    fn assert_vec3_close(actual: Vec3, expected: Vec3) {
        assert!(
            actual.distance(expected) <= 1e-5,
            "expected {expected:?}, got {actual:?}"
        );
    }

    fn assert_quat_close(actual: Quat, expected: Quat) {
        assert!(
            actual.dot(expected).abs() >= 1.0 - 1e-5,
            "expected {expected:?}, got {actual:?}"
        );
    }

    #[test]
    fn hermite_preserves_endpoints() {
        let start = state(
            Transform::from_xyz(1.0, 2.0, 0.0)
                .with_rotation(Quat::from_rotation_z(0.25))
                .with_scale(Vec3::splat(2.0)),
            3.0,
            (4.0, 5.0),
        );
        let end = state(
            Transform::from_xyz(6.0, 7.0, 0.0)
                .with_rotation(Quat::from_rotation_z(1.25))
                .with_scale(Vec3::splat(4.0)),
            -2.0,
            (8.0, 9.0),
        );

        let at_start =
            transform::hermite(start, end, InterpolationSampleContext::new(0.0, Some(0.5)));
        let at_end =
            transform::hermite(start, end, InterpolationSampleContext::new(1.0, Some(0.5)));

        assert_vec3_close(at_start.0.translation, start.0.translation);
        assert_quat_close(at_start.0.rotation, start.0.rotation);
        assert_vec3_close(at_start.0.scale, start.0.scale);
        assert_vec3_close(at_end.0.translation, end.0.translation);
        assert_quat_close(at_end.0.rotation, end.0.rotation);
        assert_vec3_close(at_end.0.scale, end.0.scale);
    }

    #[test]
    fn hermite_scales_velocity_by_sample_interval() {
        let start = state(Transform::default(), 0.0, (4.0, 0.0));
        let end = state(Transform::default(), 0.0, (0.0, 0.0));

        let one_second =
            transform::hermite(start, end, InterpolationSampleContext::new(0.5, Some(1.0)));
        let two_seconds =
            transform::hermite(start, end, InterpolationSampleContext::new(0.5, Some(2.0)));

        assert_vec3_close(one_second.0.translation, Vec3::X * 0.5);
        assert_vec3_close(two_seconds.0.translation, Vec3::X);
    }

    #[test]
    fn hermite_unwraps_full_revolution() {
        let start = state(Transform::default(), core::f32::consts::TAU, (0.0, 0.0));
        let end = start;
        let midpoint =
            transform::hermite(start, end, InterpolationSampleContext::new(0.5, Some(1.0)));

        assert_vec3_close(midpoint.0.rotation * Vec3::X, -Vec3::X);
    }

    #[test]
    fn hermite_without_sample_interval_falls_back_to_linear() {
        let start = state(Transform::default(), 0.0, (100.0, 0.0));
        let end_transform = Transform::from_xyz(8.0, 4.0, 0.0)
            .with_rotation(Quat::from_rotation_z(core::f32::consts::FRAC_PI_2))
            .with_scale(Vec3::splat(3.0));
        let end = state(end_transform, 0.0, (0.0, 0.0));
        let result = transform::hermite(start, end, InterpolationSampleContext::from_t(0.25));

        assert_vec3_close(
            result.0.translation,
            start.0.translation.lerp(end.0.translation, 0.25),
        );
        assert_quat_close(
            result.0.rotation,
            start.0.rotation.slerp(end.0.rotation, 0.25),
        );
        assert_vec3_close(result.0.scale, start.0.scale.lerp(end.0.scale, 0.25));
    }

    #[test]
    fn position_rotation_hermite_uses_velocity_tangents() {
        let start = position_rotation_state((0.0, 0.0), 0.0, (4.0, 0.0), core::f32::consts::TAU);
        let end = position_rotation_state((0.0, 0.0), 0.0, (0.0, 0.0), core::f32::consts::TAU);

        let midpoint =
            position_rotation::hermite(start, end, InterpolationSampleContext::new(0.5, Some(1.0)));

        assert_vec3_close(midpoint.0.0.f32().extend(0.0), Vec3::X * 0.5);
        assert_vec3_close(Quaternion::from(midpoint.1).f32() * Vec3::X, -Vec3::X);
        assert_vec3_close(midpoint.2.0.f32().extend(0.0), Vec3::X * 2.0);
        #[allow(clippy::unnecessary_cast)]
        let angular_velocity = midpoint.3.0 as f32;
        assert!((angular_velocity - core::f32::consts::TAU).abs() <= 1e-5);
    }

    #[test]
    fn position_rotation_hermite_without_interval_falls_back_to_lerp() {
        let start = position_rotation_state((0.0, 0.0), 0.0, (100.0, 0.0), 10.0);
        let end =
            position_rotation_state((8.0, 4.0), core::f32::consts::FRAC_PI_2, (0.0, 0.0), 0.0);

        let result =
            position_rotation::hermite(start, end, InterpolationSampleContext::from_t(0.25));

        assert_vec3_close(result.0.0.f32().extend(0.0), Vec3::new(2.0, 1.0, 0.0));
        assert_vec3_close(
            Quaternion::from(result.1).f32() * Vec3::X,
            Quat::from_rotation_z(core::f32::consts::FRAC_PI_8) * Vec3::X,
        );
    }
}