bevy-ichun 0.3.0

//! # Core Kinematic Character Controller
//!
//! This module implements the core functionality of the kinematic character controller.
//! It handles physics interactions, collision response, gravity application,
//! and other fundamental KCC behaviors.
//!
//! A kinematic character controller provides precise control over character movement
//! while still respecting physics constraints like collisions and gravity.
//! Unlike dynamic rigid bodies, kinematic controllers are not directly affected
//! by forces, but instead have their positions explicitly set each frame.
//!
//! ## Components
//!
//! * [`Kcc`]: The main component that defines KCC behavior and parameters
//! * [`KccVelocity`]: Stores and manages the character's current velocity
//!
//! ## Systems
//!
//! * `update_grounded_sys`: Updates the grounded state of the KCC
//! * `apply_gravity_sys`: Applies gravity to the KCC when not grounded
//! * `handle_moving_platforms_sys`: Handles interaction with moving platforms and affects external_force field
//! * `kinematic_controller_collisions_sys`: Handles collision response
//! * `apply_velocity_with_damping_sys`: Applies damping to horizontal kcc velocity and external_force when not grounded and adds them up
use avian3d::{
    math::{Vector, *},
    prelude::*,
};
use bevy::prelude::*;

use crate::system_sets::IchunSystemSet;

/// Amount of frames where no external forces are added from platforms after jumping
const DISABLE_PLATFORM_SYNC_FRAMES: u8 = 4;

pub struct IchunKccPlugin;

impl Plugin for IchunKccPlugin {
    fn build(&self, app: &mut App) {
        app.add_systems(
            Update,
            (
                update_grounded_sys,
                apply_gravity_sys,
                handle_moving_platforms_sys,
                count_jump_frames_sys,
            )
                .chain()
                .in_set(IchunSystemSet::KccPhysicsSet),
        )
        .add_systems(
            Update,
            apply_velocity_with_damping_sys.in_set(IchunSystemSet::VelocityModifiersSet),
        )
        .add_systems(
            // Run collision handling after collision detection.
            //
            // NOTE: The collision implementation here is very basic and a bit buggy.
            //       A collide-and-slide algorithm would likely work better.
            PostProcessCollisions,
            kinematic_controller_collisions_sys,
        );
    }
}

/// A marker component indicating that an entity is using a character controller.
///
/// This component marks an entity as a kinematic character controller and stores
/// parameters that control its behavior such as gravity, maximum fall speed,
/// slope handling, and damping factors.
///
/// The KCC has the following features:
/// - Physics-based collision handling
/// - Ground detection with slope support
/// - Gravity application with maximum fall speed
/// - Support for movement on moving platforms
/// - Configurable damping for smooth movement deceleration
///
/// # Required Components
///
/// This component requires the following components to be present on the entity:
/// - `RigidBody`: Must be set to `RigidBody::Kinematic`
/// - `Collider`: Typically a capsule shape for character controllers
/// - `ShapeCaster`: Used for ground detection
/// - `KccVelocity`: Stores the character's velocity
///
/// # Example
///
/// ```rust
/// use bevy::prelude::*;
/// use avian3d::prelude::*;
/// use bevy_ichun::kcc::{Kcc, KccVelocity};
///
/// fn spawn_character(mut commands: Commands) {
///     commands.spawn((
///         RigidBody::Kinematic,
///         Collider::capsule(0.4, 1.0),
///         ShapeCaster::new(
///             Collider::capsule(0.4, 1.0),
///             Vector::ZERO,
///             Quaternion::default(),
///             Dir3::NEG_Y
///         ).with_max_distance(0.2),
///         Kcc::default(),
///     ));
/// }
/// ```
#[derive(Component)]
#[require(
RigidBody(|| RigidBody::Kinematic),
Collider(|| Collider::capsule(0.4, 1.0)),
ShapeCaster(|| ShapeCaster::new(
    {
        let mut collider = Collider::capsule(0.4, 1.0);
        collider.set_scale(Vector::ONE * 0.99, 10);
        collider
    },
    Vector::ZERO,
    Quaternion::default(),
    Dir3::NEG_Y).with_max_distance(0.2)
), KccVelocity)]
pub struct Kcc {
    /// The gravitational acceleration used for a character controller.
    pub controller_gravity: Vector,
    /// The maximum fall speed. If it's reached no more gravity is added.
    /// External forces will still be applied.
    pub max_fall_speed: Scalar,
    /// The maximum angle a slope can have for a character controller
    /// to be able to climb and jump. If the slope is steeper than this angle,
    /// the character will slide down.
    pub max_slope_angle: Option<Scalar>,
    /// True if the KCC touches the ground
    pub is_grounded: bool,
    /// The damping factor used for slowing down movement.
    /// A value between 0 and 1, where lower values cause faster deceleration.
    /// The default value of 0.92 provides smooth deceleration.
    pub movement_damping_factor: Scalar,
    /// The velocity which is not affected by the movement dampening
    /// (moving platforms)
    pub external_force: Vector,
    /// The damping factor applied to external forces when no new external force is applied.
    /// A value between 0 and 1, where lower values cause faster deceleration.
    pub external_force_damping_factor: Scalar,
    /// The maximum number of frames before the `JumpedRecently` component is removed from the KCC.
    /// This should not be lower than `DISABLE_PLATFORM_SYNC_FRAMES` or any other value
    /// that needs the frame counter on it.
    pub max_jumped_frames: u8,
}

impl Default for Kcc {
    fn default() -> Self {
        Self {
            controller_gravity: Vector::NEG_Y * 9.81 * 2.0,
            max_fall_speed: 30.0,
            max_slope_angle: Some((30.0 as Scalar).to_radians()),
            is_grounded: false,
            movement_damping_factor: 0.025,
            external_force: Vector::ZERO,
            external_force_damping_factor: 0.9,
            max_jumped_frames: 4,
        }
    }
}

/// Component which holds the kcc velocity
///
/// This component stores the character's current velocity vector.
/// Movement damping is applied to this velocity in the horizontal plane
/// to create smooth deceleration.
///
/// This is separate from Avian3D's `LinearVelocity` component, as it allows
/// the KCC systems to manipulate the velocity before it's applied to the
/// physics body.
///
/// # Example
///
/// ```rust
/// use bevy_ichun::kcc::KccVelocity;
/// use avian3d::math::Vector;
///
/// // Create a velocity pointing along the positive X axis
/// let velocity = KccVelocity(Vector::new(5.0, 0.0, 0.0));
/// ```
#[derive(Clone, Copy, Component, Debug, Deref, DerefMut, PartialEq)]
pub struct KccVelocity(pub Vector);

impl Default for KccVelocity {
    fn default() -> Self {
        Self(Vector::ZERO)
    }
}

/// A frame counter which is added as soon as the KCC jumped
/// If you don't use the `movement` feature you should add it on your own
/// If the is available on the KCC the counter will be increased each frame until `kcc.max_jumped_frames` is reached
/// When  the `max_jumped_frames` has reached the frame counter will be removed from the KCC
/// This component is necessary to "unstick" the KCC (prevents adding new down force) from moving platform when they move downwards
#[derive(Component, Clone, Copy)]
pub struct JumpedRecently {
    /// The frames since Kcc has jumped
    frame_counter: u8,
}

impl Default for JumpedRecently {
    fn default() -> Self {
        Self { frame_counter: 0 }
    }
}

impl JumpedRecently {
    pub fn get_frame_counter(self) -> u8 {
        self.frame_counter
    }
}

/// Updates the [`Grounded`] status for character controllers.
fn update_grounded_sys(mut query: Query<(&ShapeHits, &Rotation, &mut Kcc)>) {
    for (hits, rotation, mut kcc) in &mut query {
        // The character is grounded if the shape caster has a hit with a normal
        // that isn't too steep.
        let is_grounded = hits.iter().any(|hit| {
            if let Some(angle) = kcc.max_slope_angle {
                (rotation * -hit.normal2).angle_between(Vector::Y).abs() <= angle
            } else {
                true
            }
        });

        if is_grounded {
            kcc.is_grounded = true;
        } else {
            kcc.is_grounded = false;
        }
    }
}

/// Applies [`ControllerGravity`] to character controllers.
fn apply_gravity_sys(time: Res<Time>, mut controllers: Query<(&Kcc, &mut KccVelocity)>) {
    // Precision is adjusted so that the example works with
    // both the `f32` and `f64` features. Otherwise you don't need this.
    let delta_time = time.delta_secs_f64().adjust_precision();

    for (kcc, mut kcc_vel) in &mut controllers {
        // Only add external forces (elevator) to gravity when not grounded and the external force is higher than the gravity
        // This is necessary to make the KCC follow platforms downwards and not "jumping" on it
        if kcc.is_grounded {
            continue;
        }

        // Don't apply extra gravity when KCC already at max fall speed
        if kcc_vel.0.dot(kcc.controller_gravity.normalize_or_zero()) < kcc.max_fall_speed {
            let gravity = kcc.controller_gravity * delta_time;
            kcc_vel.0 += gravity;
        }
    }
}

/// Applies external forces to KCC when standing on a moving platform
fn handle_moving_platforms_sys(
    time: Res<Time>,
    mut kcc_qry: Query<(
        Entity,
        &ShapeHits,
        &Rotation,
        &mut Kcc,
        &mut KccVelocity,
        Has<JumpedRecently>,
    )>,
    jumped_recently_qry: Query<(Entity, &JumpedRecently)>,
    hit_qry: Query<(Entity, &LinearVelocity), Without<Kcc>>,
) {
    let delta_time = time.delta_secs_f64().adjust_precision();

    for (entity, hits, rotation, mut kcc, mut kcc_vel, has_jumped_recently) in kcc_qry.iter_mut() {
        // If the KCC just jumped we ignore the platform movement for some frames
        // This "unsticks" the KCC from the platform
        if has_jumped_recently {
            if let Ok((_, jumped_recently)) = jumped_recently_qry.get(entity) {
                if jumped_recently.get_frame_counter() <= DISABLE_PLATFORM_SYNC_FRAMES {
                    continue;
                }
            }
        }

        // When the KCC is not grounded anymore the velocity should reduce (damping) over time
        if !kcc.is_grounded {
            continue;
        }

        let mut platform_velocity = Vec3::ZERO;

        // Get all hits with the ground and calculate the velocity
        for hit in hits.iter() {
            // check if the hit data is the ground
            let grounded_hit = match kcc.max_slope_angle {
                Some(angle) => {
                    if (rotation * -hit.normal2).angle_between(Vector::Y).abs() > angle {
                        continue;
                    }
                    hit
                }
                None => hit,
            };

            // get the velocity of the ground
            let Some((_, ground_velocity)) = hit_qry
                .iter()
                .find(|ground| ground.0 == grounded_hit.entity)
            else {
                continue;
            };

            platform_velocity += ground_velocity.0;
        }

        if platform_velocity != kcc.external_force {
            kcc.external_force = platform_velocity;
        }

        // applies extra gravity to make the KCC stick to the ground
        if kcc.is_grounded {
            if kcc.external_force.y < 0.0 {
                kcc_vel.0.y = -kcc.max_fall_speed * delta_time;
            }
        }
    }
}

/// Increment frame counter after jumping
fn count_jump_frames_sys(
    mut cmd: Commands,
    mut kcc_qry: Query<(Entity, &Kcc, &mut JumpedRecently)>,
) {
    for (entity, kcc, mut jumped_recently) in &mut kcc_qry {
        if jumped_recently.get_frame_counter() < kcc.max_jumped_frames {
            jumped_recently.frame_counter += 1;
        } else {
            cmd.entity(entity).remove::<JumpedRecently>();
        }
    }
}

/// Kinematic bodies do not get pushed by collisions by default,
/// so it needs to be done manually.
///
/// This system handles collision response for kinematic character controllers
/// by pushing them along their contact normals by the current penetration depth,
/// and applying velocity corrections in order to snap to slopes, slide along walls,
/// and predict collisions using speculative contacts.
#[allow(clippy::type_complexity)]
fn kinematic_controller_collisions_sys(
    collisions: Res<Collisions>,
    bodies_qry: Query<&RigidBody>,
    collider_parents_qry: Query<&ColliderParent, Without<Sensor>>,
    mut character_controllers_qry: Query<
        (&mut Position, &Rotation, &mut KccVelocity, &Kcc),
        With<RigidBody>,
    >,
    time: Res<Time>,
) {
    // Iterate through collisions and move the kinematic body to resolve penetration
    for contacts in collisions.iter() {
        // Get the rigid body entities of the colliders (colliders could be children)
        let Ok([collider_parent1, collider_parent2]) =
            collider_parents_qry.get_many([contacts.entity1, contacts.entity2])
        else {
            continue;
        };

        // Get the body of the character controller and whether it is the first
        // or second entity in the collision.
        let is_first: bool;

        let character_rb: RigidBody;
        let is_other_dynamic: bool;

        let (mut position, rotation, mut kcc_vel, kcc) = if let Ok(character) =
            character_controllers_qry.get_mut(collider_parent1.get())
        {
            is_first = true;
            character_rb = *bodies_qry.get(collider_parent1.get()).unwrap();
            is_other_dynamic = bodies_qry
                .get(collider_parent2.get())
                .is_ok_and(|rb| rb.is_dynamic());
            character
        } else if let Ok(character) = character_controllers_qry.get_mut(collider_parent2.get()) {
            is_first = false;
            character_rb = *bodies_qry.get(collider_parent2.get()).unwrap();
            is_other_dynamic = bodies_qry
                .get(collider_parent1.get())
                .is_ok_and(|rb| rb.is_dynamic());
            character
        } else {
            continue;
        };

        // This system only handles collision response for kinematic character controllers.
        if !character_rb.is_kinematic() {
            continue;
        }

        // Iterate through contact manifolds and their contacts.
        // Each contact in a single manifold shares the same contact normal.
        for manifold in contacts.manifolds.iter() {
            let normal = if is_first {
                -manifold.global_normal1(rotation)
            } else {
                -manifold.global_normal2(rotation)
            };

            let mut deepest_penetration: Scalar = Scalar::MIN;

            // get deepest penetration
            for contact in manifold.contacts.iter() {
                deepest_penetration = deepest_penetration.max(contact.penetration);
            }

            if deepest_penetration > 0.0 {
                // Move the position out of collision
                position.0 += normal * deepest_penetration;
            }

            // For now, this system only handles velocity corrections for collisions against static geometry.
            if is_other_dynamic {
                continue;
            }

            // Determine if the slope is climbable or if it's too steep to walk on.
            let slope_angle = normal.angle_between(Vector::Y);
            let climbable = kcc
                .max_slope_angle
                .is_some_and(|angle| slope_angle.abs() <= angle);

            let total_velocity = kcc_vel.0 + kcc.external_force;

            if deepest_penetration > 0.0 {
                // If the slope is climbable, snap the velocity so that the character
                // up and down the surface smoothly.
                if climbable {
                    // Points in the normal's direction in the XZ plane.
                    let normal_direction_xz =
                        normal.reject_from_normalized(Vector::Y).normalize_or_zero();

                    // The movement speed along the direction above.
                    let kcc_vel_xz = total_velocity.dot(normal_direction_xz);

                    // Snap the Y speed based on the speed at which the character is moving
                    // up or down the slope, and how steep the slope is.
                    //
                    // A 2D visualization of the slope, the contact normal, and the velocity components:
                    //
                    //             ╱
                    //     normal ╱
                    // *         ╱
                    // │   *    ╱   velocity_x
                    // │       * - - - - - -
                    // │           *       | velocity_y
                    // │               *   |
                    // *───────────────────*

                    let max_y_speed = -kcc_vel_xz * slope_angle.tan();
                    kcc_vel.y = kcc_vel.y.max(max_y_speed);
                } else {
                    // The character is intersecting an unclimbable object, like a wall.
                    // We want the character to slide along the surface, similarly to
                    // a collide-and-slide algorithm.

                    // Don't apply an impulse if the character is moving away from the surface.
                    if total_velocity.dot(normal) > 0.0 {
                        continue;
                    }

                    // Slide along the surface, rejecting the velocity along the contact normal.
                    let impulse = total_velocity.reject_from_normalized(normal);
                    kcc_vel.0 = impulse;
                }
            } else {
                // The character is not yet intersecting the other object,
                // but the narrow phase detected a speculative collision.
                //
                // We need to push back the part of the velocity
                // that would cause penetration within the next frame.

                let normal_speed = total_velocity.dot(normal);

                // Don't apply an impulse if the character is moving away from the surface.
                if normal_speed > 0.0 {
                    continue;
                }

                // Compute the impulse to apply.
                let impulse_magnitude =
                    normal_speed - (deepest_penetration / time.delta_secs_f64().adjust_precision());
                let mut impulse = impulse_magnitude * normal;

                // Apply the impulse differently depending on the slope angle.
                if climbable {
                    // Avoid sliding down slopes.
                    kcc_vel.y -= impulse.y.min(0.0);
                } else {
                    // Avoid climbing up walls.
                    impulse.y = impulse.y.max(0.0);
                    kcc_vel.0 -= impulse;
                }
            }
        }
    }
}

/// Slows down kinematic velocity in the XZ plane.
/// Slows down external forces when no contact is present anymore
/// Adds up external force and kinematic velocity and applies it to the linear velocity
fn apply_velocity_with_damping_sys(
    time: Res<Time>,
    mut controllers_qry: Query<(&mut Kcc, &mut KccVelocity, &mut LinearVelocity)>,
) {
    let delta_time = time.delta_secs_f64().adjust_precision();

    for (mut kcc, mut kcc_vel, mut linear_velocity) in &mut controllers_qry {
        // We could use `LinearDamping`, but we don't want to dampen movement along the Y axis
        kcc_vel.x *= f32::powf(kcc.movement_damping_factor, delta_time);
        kcc_vel.z *= f32::powf(kcc.movement_damping_factor, delta_time);

        if !kcc.is_grounded {
            let damping = kcc.external_force_damping_factor;
            kcc.external_force *= f32::powf(damping, delta_time);
        }

        linear_velocity.0 = kcc_vel.0 + kcc.external_force;
    }
}