raidillon/game/src/systems/kinematic_character_controller.rs

use glam::{Quat, Vec3};
use hecs::Entity;
use rapier3d::prelude::{nalgebra, ColliderBuilder, QueryFilter, QueryPipeline, RigidBodyBuilder};
use rapier3d::prelude::vector;
use raidillon_core::scene::Scene;
use raidillon_engine::{EngineResources, InputState};
use raidillon_engine::system::System;
use rapier3d::control::KinematicCharacterController as RapierKinematicCharacterController;
use rapier3d::math::Isometry;
use rapier3d::na::{Isometry3, Vector3};
use winit::event::DeviceEvent::MouseMotion;
use winit::event::{ElementState, Event, MouseButton, WindowEvent};
use winit::keyboard::KeyCode;
use winit::window::CursorGrabMode;
use raidillon_core::DebugUIBuffer;
use raidillon_ecs::components::{CameraMode, CharacterBodyComponent};
use raidillon_ecs::Transform;
use raidillon_engine::systems::fps_camera::FPSDebugCameraSystem;
use raidillon_physics::Physics;
use raidillon_platform::{Camera, PlatformContext};

#[derive(Default)]
pub struct KinematicCharacterController {
    character_controller: RapierKinematicCharacterController,
    character_collider: ColliderBuilder,

    desired_movement: Vec3,
    last_position: Vector3<f32>,
    yaw:   f32,
    pitch: f32,
    speed: f32,
    sensitivity: f32,
    mouse_delta: (f64, f64),

    vertical_velocity: f32,
    gravity: f32,
    max_fall_speed: f32,
}

impl System for KinematicCharacterController {
    fn load_world(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        // create the rigid body, add it to the body set
        let p = scene.resources.get_mut::<Physics>().expect("Physics missing");
        let rb = RigidBodyBuilder::kinematic_position_based().build();
        let rb_handle = p.rigid_body_set.insert(rb);
        self.character_collider = ColliderBuilder::capsule_z(1.5, 1.0);
        p.collider_set.insert_with_parent(self.character_collider.build(), rb_handle, &mut p.rigid_body_set);
        let tr = Transform {
            translation: Vec3::new(0.0, 2.0, 3.0),
            rotation: Quat::IDENTITY,
            scale: Vec3::new(1.0, 1.0, 1.0),
        };
        self.last_position = vector![
            tr.translation.x,
            tr.translation.y,
            tr.translation.z,
        ];
        scene.world.spawn((
            tr,
            CharacterBodyComponent(rb_handle),
        ));

        self.speed = 5.0;
        self.sensitivity = 0.1;
        self.gravity = -9.81;
        self.max_fall_speed = -50.0;
        self.vertical_velocity = 0.0;
    }

    fn handle_event(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        if !self.is_camera_mode_valid(scene) {
            return
        }

        let pctx = res.get::<PlatformContext>().unwrap();
        let event2 = pctx.current_event.clone();
        match event2 {
            Event::DeviceEvent { device_id, event } => {
                match event {
                    MouseMotion { delta } => {
                        self.mouse_delta.0 += delta.0;
                        self.mouse_delta.1 += delta.1;
                    },
                    _ => {}
                }
            },
            _ => {},
        }
    }

    fn frame_update(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        if !self.is_camera_mode_valid(scene) {
            return
        }
        let (pctx, input) = res.get_many::<(PlatformContext, InputState)>().unwrap();

        self.yaw   += (self.mouse_delta.0 as f32) * self.sensitivity;
        self.pitch -= (self.mouse_delta.1 as f32) * self.sensitivity;
        self.pitch = self.pitch.clamp(-89.0, 89.0);

        let front = self.front();
        let right_vec = front.cross(Vec3::Y).normalize();

        if input.key_held(KeyCode::KeyW) {
            self.desired_movement += front * pctx.time_ctx.frame_dt * self.speed;
        }
        if input.key_held(KeyCode::KeyS) {
            self.desired_movement -= front * pctx.time_ctx.frame_dt * self.speed;
        }
        if input.key_held(KeyCode::KeyA) {
            self.desired_movement -= right_vec * pctx.time_ctx.frame_dt * self.speed;
        }
        if input.key_held(KeyCode::KeyD) {
            self.desired_movement += right_vec * pctx.time_ctx.frame_dt * self.speed;
        }

        let pos = Physics::rapier_translation_to_glam(&self.last_position);

        scene.world.query_mut::<&mut Camera>().into_iter().for_each(|(_, camera)| {
            // INTERPOLATION NEEDED.
            camera.eye = pos;
            camera.center = pos + front;
        });
        self.mouse_delta = (0.0, 0.0);
    }

    fn fixed_update(&mut self, res: &mut EngineResources, scene: &mut Scene) {
        if !self.is_camera_mode_valid(scene) {
            return
        }
        let p = scene.resources.get_mut::<Physics>().unwrap();
        let (pctx, input) = res.get_many::<(PlatformContext, InputState)>().unwrap();

        let (ch_ent, (ch_tr, ch_component)) = scene
            .world
            .query_mut::<(&mut Transform, &mut CharacterBodyComponent)>()
            .into_iter()
            .next()
            .expect("no character entity in world");

        let query_pipeline = p.broad_phase.as_query_pipeline(
            p.narrow_phase.query_dispatcher(),
            &p.rigid_body_set,
            &p.collider_set,
            QueryFilter::default().exclude_rigid_body(ch_component.0),
        );

        self.vertical_velocity = (self.vertical_velocity + self.gravity * pctx.time_ctx.fixed_dt)
            .max(self.max_fall_speed);
        let mut total_displacement = self.desired_movement;
        total_displacement.y += self.vertical_velocity * pctx.time_ctx.fixed_dt;

        let corrected_movement = self.character_controller.move_shape(
            pctx.time_ctx.fixed_dt,
            &query_pipeline,
            &*self.character_collider.shape,
            &Isometry3::from(self.last_position),
            vector![total_displacement.x, total_displacement.y, total_displacement.z],
            |_| {},
        );

        // update character rigid body with the new translation.
        if let Some(body) = p.get_rigid_body_mut(ch_component.0) {
            self.last_position = vector![
                self.last_position.x + corrected_movement.translation.x,
                self.last_position.y + corrected_movement.translation.y,
                self.last_position.z + corrected_movement.translation.z,
            ];
            body.set_next_kinematic_position(Isometry3::from(self.last_position));
            ch_tr.translation = Physics::rapier_translation_to_glam(&self.last_position);
            // reset vertical velocity if grounded
            if corrected_movement.grounded {
                self.vertical_velocity = 0.0;
            }
        }

        self.desired_movement = Vec3::ZERO;
    }
}

impl KinematicCharacterController {
    pub fn front(&self) -> Vec3 {
        let yaw_rad   = self.yaw.to_radians();
        let pitch_rad = self.pitch.to_radians();
        Vec3::new(
            yaw_rad.cos() * pitch_rad.cos(),
            pitch_rad.sin(),
            yaw_rad.sin() * pitch_rad.cos(),
        ).normalize()
    }

    fn is_camera_mode_valid(&self, scene: &mut Scene) -> bool {
        let mut q = scene.world.query::<(&Camera, &CameraMode)>();
        let (cam_ent, (cam, cam_mode)) = q
            .iter()
            .next()
            .unwrap();
        *cam_mode == CameraMode::Kinematic
    }
}