character.rs

use rapier3d::{
    control::{CharacterLength, KinematicCharacterController, PidController},
    prelude::*,
};
use rapier_testbed3d::{
    ui::egui::{Align2, ComboBox, Slider, Ui, Window},
    KeyCode, PhysicsState, TestbedGraphics,
};

#[derive(PartialEq, Eq, Clone, Copy, Debug)]
pub enum CharacterControlMode {
    Kinematic,
    Pid,
}

pub fn update_character(
    graphics: &mut TestbedGraphics,
    physics: &mut PhysicsState,
    control_mode: &mut CharacterControlMode,
    controller: &mut KinematicCharacterController,
    pid: &mut PidController,
    character_handle: RigidBodyHandle,
) {
    let prev_control_mode = *control_mode;
    character_control_ui(graphics, controller, pid, control_mode);

    if *control_mode != prev_control_mode {
        match control_mode {
            CharacterControlMode::Kinematic => physics.bodies[character_handle]
                .set_body_type(RigidBodyType::KinematicPositionBased, false),
            CharacterControlMode::Pid => {
                physics.bodies[character_handle].set_body_type(RigidBodyType::Dynamic, true)
            }
        }
    }

    match *control_mode {
        CharacterControlMode::Kinematic => {
            update_kinematic_controller(graphics, physics, character_handle, controller)
        }
        CharacterControlMode::Pid => {
            update_pid_controller(graphics, physics, character_handle, pid)
        }
    }
}

fn character_movement_from_inputs(
    gfx: &TestbedGraphics,
    mut speed: Real,
    artificial_gravity: bool,
) -> Vector<Real> {
    let mut desired_movement = Vector::zeros();

    let rot = gfx.camera_rotation();
    let mut rot_x = rot * Vector::x();
    let mut rot_z = rot * Vector::z();
    rot_x.y = 0.0;
    rot_z.y = 0.0;

    for key in gfx.keys().get_pressed() {
        match *key {
            KeyCode::ArrowRight => {
                desired_movement += rot_x;
            }
            KeyCode::ArrowLeft => {
                desired_movement -= rot_x;
            }
            KeyCode::ArrowUp => {
                desired_movement -= rot_z;
            }
            KeyCode::ArrowDown => {
                desired_movement += rot_z;
            }
            KeyCode::Space => {
                desired_movement += Vector::y() * 2.0;
            }
            KeyCode::ControlRight => {
                desired_movement -= Vector::y();
            }
            KeyCode::ShiftLeft => {
                speed /= 10.0;
            }
            _ => {}
        }
    }

    desired_movement *= speed;

    if artificial_gravity {
        desired_movement -= Vector::y() * speed;
    }

    desired_movement
}

fn update_pid_controller(
    gfx: &mut TestbedGraphics,
    phx: &mut PhysicsState,
    character_handle: RigidBodyHandle,
    pid: &mut PidController,
) {
    let desired_movement = character_movement_from_inputs(gfx, 0.1, false);
    let character_body = &mut phx.bodies[character_handle];

    // Adjust the controlled axis depending on the keys pressed by the user.
    // - If the user is jumping, enable control over Y.
    // - If the user isn’t pressing any key, disable all linear controls to let
    //   gravity/collision do their thing freely.
    let mut axes = AxesMask::ANG_X | AxesMask::ANG_Y | AxesMask::ANG_Z;

    if desired_movement.norm() != 0.0 {
        axes |= if desired_movement.y == 0.0 {
            AxesMask::LIN_X | AxesMask::LIN_Z
        } else {
            AxesMask::LIN_X | AxesMask::LIN_Z | AxesMask::LIN_Y
        }
    };

    pid.set_axes(axes);

    let corrective_vel = pid.rigid_body_correction(
        phx.integration_parameters.dt,
        character_body,
        (character_body.translation() + desired_movement).into(),
        RigidBodyVelocity::zero(),
    );
    let new_vel = *character_body.vels() + corrective_vel;

    character_body.set_vels(new_vel, true);
}

fn update_kinematic_controller(
    gfx: &mut TestbedGraphics,
    phx: &mut PhysicsState,
    character_handle: RigidBodyHandle,
    controller: &KinematicCharacterController,
) {
    let speed = 0.1;
    let desired_movement = character_movement_from_inputs(gfx, speed, true);

    let character_body = &phx.bodies[character_handle];
    let character_collider = &phx.colliders[character_body.colliders()[0]];
    let character_mass = character_body.mass();

    let mut collisions = vec![];
    let mvt = controller.move_shape(
        phx.integration_parameters.dt,
        &phx.bodies,
        &phx.colliders,
        &phx.query_pipeline,
        character_collider.shape(),
        character_collider.position(),
        desired_movement.cast::<Real>(),
        QueryFilter::new().exclude_rigid_body(character_handle),
        |c| collisions.push(c),
    );

    if mvt.grounded {
        gfx.set_body_color(character_handle, [0.1, 0.8, 0.1]);
    } else {
        gfx.set_body_color(character_handle, [0.8, 0.1, 0.1]);
    }

    controller.solve_character_collision_impulses(
        phx.integration_parameters.dt,
        &mut phx.bodies,
        &phx.colliders,
        &phx.query_pipeline,
        character_collider.shape(),
        character_mass,
        &*collisions,
        QueryFilter::new().exclude_rigid_body(character_handle),
    );

    let character_body = &mut phx.bodies[character_handle];
    let pose = character_body.position();
    character_body.set_next_kinematic_translation(pose.translation.vector + mvt.translation);
}

fn character_control_ui(
    gfx: &mut TestbedGraphics,
    character_controller: &mut KinematicCharacterController,
    pid_controller: &mut PidController,
    control_mode: &mut CharacterControlMode,
) {
    Window::new("Character Control")
        .anchor(Align2::RIGHT_TOP, [-15.0, 15.0])
        .show(gfx.ui_context_mut().ctx_mut(), |ui| {
            ComboBox::from_label("control mode")
                .selected_text(format!("{:?}", *control_mode))
                .show_ui(ui, |ui| {
                    ui.selectable_value(control_mode, CharacterControlMode::Kinematic, "Kinematic");
                    ui.selectable_value(control_mode, CharacterControlMode::Pid, "Pid");
                });

            match control_mode {
                CharacterControlMode::Kinematic => {
                    kinematic_control_ui(ui, character_controller);
                }
                CharacterControlMode::Pid => {
                    pid_control_ui(ui, pid_controller);
                }
            }
        });
}

fn pid_control_ui(ui: &mut Ui, pid_controller: &mut PidController) {
    let mut lin_kp = pid_controller.pd.lin_kp.x;
    let mut lin_ki = pid_controller.lin_ki.x;
    let mut lin_kd = pid_controller.pd.lin_kd.x;
    let mut ang_kp = pid_controller.pd.ang_kp.x;
    let mut ang_ki = pid_controller.ang_ki.x;
    let mut ang_kd = pid_controller.pd.ang_kd.x;

    ui.add(Slider::new(&mut lin_kp, 0.0..=100.0).text("linear Kp"));
    ui.add(Slider::new(&mut lin_ki, 0.0..=10.0).text("linear Ki"));
    ui.add(Slider::new(&mut lin_kd, 0.0..=1.0).text("linear Kd"));
    ui.add(Slider::new(&mut ang_kp, 0.0..=100.0).text("angular Kp"));
    ui.add(Slider::new(&mut ang_ki, 0.0..=10.0).text("angular Ki"));
    ui.add(Slider::new(&mut ang_kd, 0.0..=1.0).text("angular Kd"));

    pid_controller.pd.lin_kp.fill(lin_kp);
    pid_controller.lin_ki.fill(lin_ki);
    pid_controller.pd.lin_kd.fill(lin_kd);
    pid_controller.pd.ang_kp.fill(ang_kp);
    pid_controller.ang_ki.fill(ang_ki);
    pid_controller.pd.ang_kd.fill(ang_kd);
}

fn kinematic_control_ui(ui: &mut Ui, character_controller: &mut KinematicCharacterController) {
    ui.checkbox(&mut character_controller.slide, "slide")
        .on_hover_text("Should the character try to slide against the floor if it hits it?");
    #[allow(clippy::useless_conversion)]
    {
        ui.add(Slider::new(&mut character_controller.max_slope_climb_angle, 0.0..=std::f32::consts::TAU.into()).text("max_slope_climb_angle"))
            .on_hover_text("The maximum angle (radians) between the floor’s normal and the `up` vector that the character is able to climb.");
        ui.add(Slider::new(&mut character_controller.min_slope_slide_angle, 0.0..=std::f32::consts::FRAC_PI_2.into()).text("min_slope_slide_angle"))
            .on_hover_text("The minimum angle (radians) between the floor’s normal and the `up` vector before the character starts to slide down automatically.");
    }
    let mut is_snapped = character_controller.snap_to_ground.is_some();
    if ui.checkbox(&mut is_snapped, "snap_to_ground").changed() {
        match is_snapped {
            true => {
                character_controller.snap_to_ground = Some(CharacterLength::Relative(0.1));
            }
            false => {
                character_controller.snap_to_ground = None;
            }
        }
    }
    if let Some(snapped) = &mut character_controller.snap_to_ground {
        match snapped {
            CharacterLength::Relative(val) => {
                ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Relative Character Length"));
            }
            CharacterLength::Absolute(val) => {
                ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Absolute Character Length"));
            }
        }
    }
}