engine/engine/src/scene/content/components/free_space_control.rs

use anyhow::Result;
use context::prelude::cgmath::*;
use ecs::{ComponentDebug, EntityComponent, World};

use std::time::Duration;

pub struct FreeSpaceControlSettings {
    /// m / s^2
    pub max_forward_acceleration: f32,
    /// m / s^2
    pub max_forward_deceleration: f32,

    /// m / s^2
    /// up, down, left, right
    pub max_strafe_acceleration: f32,

    /// ° / s^2
    pub max_yaw_acceleration: Deg<f32>,
    /// ° / s^2
    pub max_pitch_acceleration: Deg<f32>,
    /// ° / s^2
    pub max_roll_acceleration: Deg<f32>,
}

impl Default for FreeSpaceControlSettings {
    fn default() -> Self {
        Self {
            max_forward_acceleration: 50.0,
            max_forward_deceleration: 10.0,

            max_strafe_acceleration: 5.0,

            max_yaw_acceleration: Deg(5.0),
            max_pitch_acceleration: Deg(5.0),
            max_roll_acceleration: Deg(5.0),
        }
    }
}

pub struct FreeSpaceControl {
    settings: FreeSpaceControlSettings,
    dead_zone: f32,

    /// (yaw, pitch, roll)
    rotation: Vector3<Deg<f32>>,
    position: Vector3<f32>,

    current_translation: Matrix4<f32>,
    current_rotation: Matrix3<f32>,
    current_transform: Matrix4<f32>,

    /// (yaw, pitch, roll)
    input_rotation: Vector3<f32>,
    /// (forward, sideward, upward)
    input_position: Vector3<f32>,

    /// (yaw, pitch, roll)
    velocity_rotation: Vector3<Deg<f32>>,
    /// (forward, sideward, upward)
    velocity_position: Vector3<f32>,

    last_update: Option<Duration>,
}

impl FreeSpaceControl {
    const EPSILON: f32 = 0.001;

    pub fn new(dead_zone: f32, settings: FreeSpaceControlSettings) -> Self {
        Self {
            settings,
            dead_zone,

            rotation: vec3(Deg(0.0), Deg(0.0), Deg(0.0)),
            position: Vector3::zero(),

            current_translation: Matrix4::one(),
            current_rotation: Matrix3::one(),
            current_transform: Matrix4::one(),

            input_rotation: Vector3::zero(),
            input_position: Vector3::zero(),

            velocity_rotation: vec3(Deg(0.0), Deg(0.0), Deg(0.0)),
            velocity_position: Vector3::zero(),

            last_update: None,
        }
    }

    pub fn replace_settings(&mut self, settings: FreeSpaceControlSettings) {
        self.settings = settings;
    }

    pub fn set_throttle(&mut self, throttle: f32) {
        debug_assert!(throttle.abs() <= (1.0 + Self::EPSILON), "value {throttle}");

        self.input_position.y = if throttle.abs() < self.dead_zone {
            0.0
        } else {
            throttle.clamp(-1.0, 1.0)
        };
    }

    pub fn set_left_right_strafe(&mut self, strafe: f32) {
        debug_assert!(strafe.abs() <= (1.0 + Self::EPSILON), "value {strafe}");

        self.input_position.x = if strafe.abs() < self.dead_zone {
            0.0
        } else {
            strafe.clamp(-1.0, 1.0)
        };
    }

    pub fn set_up_down_strafe(&mut self, strafe: f32) {
        debug_assert!(strafe.abs() <= (1.0 + Self::EPSILON), "value {strafe}");

        self.input_position.z = if strafe.abs() < self.dead_zone {
            0.0
        } else {
            strafe.clamp(-1.0, 1.0)
        };
    }

    pub fn set_yaw(&mut self, yaw: f32) {
        debug_assert!(yaw.abs() <= (1.0 + Self::EPSILON), "value {yaw}");

        self.input_rotation.x = if yaw.abs() < self.dead_zone {
            0.0
        } else {
            yaw.clamp(-1.0, 1.0)
        };
    }

    pub fn set_pitch(&mut self, pitch: f32) {
        debug_assert!(pitch.abs() <= (1.0 + Self::EPSILON), "value {pitch}");

        self.input_rotation.y = if pitch.abs() < self.dead_zone {
            0.0
        } else {
            pitch.clamp(-1.0, 1.0)
        };
    }

    pub fn set_roll(&mut self, roll: f32) {
        debug_assert!(roll.abs() <= (1.0 + Self::EPSILON), "value {roll}");

        self.input_rotation.z = if roll.abs() < self.dead_zone {
            0.0
        } else {
            roll.clamp(-1.0, 1.0)
        };
    }

    fn add_rotation(lhs: Vector3<Deg<f32>>, rhs: Vector3<Deg<f32>>) -> Vector3<Deg<f32>> {
        vec3(lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z)
    }

    fn factor(f: f32, v: Vector3<Deg<f32>>) -> Vector3<Deg<f32>> {
        vec3(Deg(f * v.x.0), Deg(f * v.y.0), Deg(f * v.z.0))
    }

    fn rotation_matrix(&self) -> Matrix3<f32> {
        Matrix3::from_angle_z(self.rotation.x)
            * Matrix3::from_angle_y(self.rotation.y)
            * Matrix3::from_angle_x(self.rotation.z)
    }

    pub fn update(&mut self, now: Duration) {
        let diff = match &mut self.last_update {
            Some(last_update) => {
                let diff = now - *last_update;
                *last_update = now;

                diff
            }
            None => {
                self.last_update = Some(now);
                return;
            }
        };

        let (rotation_changes, position_changes) = self.calculate_tick_changes(diff);

        let rotated_position_changes = self.rotation_matrix() * position_changes;
        self.velocity_rotation = Self::add_rotation(self.velocity_rotation, rotation_changes);
        self.velocity_position += rotated_position_changes;

        self.position += diff.as_secs_f32() * self.velocity_position;
        self.rotation = Self::add_rotation(
            self.rotation,
            Self::factor(diff.as_secs_f32(), self.velocity_rotation),
        );

        self.current_translation = Matrix4::from_translation(self.position);
        self.current_rotation = self.rotation_matrix();
        self.current_transform = self.current_translation * Matrix4::from(self.current_rotation);
    }

    pub fn translation(&self) -> Matrix4<f32> {
        self.current_translation
    }

    pub fn rotation(&self) -> Matrix3<f32> {
        self.current_rotation
    }

    pub fn transform(&self) -> Matrix4<f32> {
        self.current_transform
    }

    #[rustfmt::skip]
    fn calculate_tick_changes(&self, diff: Duration) -> (Vector3<Deg<f32>>, Vector3<f32>) {
        (
            vec3(
                Deg(diff.as_secs_f32() * self.input_rotation.x * self.settings.max_yaw_acceleration.0),
                Deg(diff.as_secs_f32() * self.input_rotation.y * self.settings.max_pitch_acceleration.0),
                Deg(diff.as_secs_f32() * self.input_rotation.z * self.settings.max_roll_acceleration.0),
            ),
            vec3(
                diff.as_secs_f32() * self.input_position.x * self.settings.max_strafe_acceleration,
                diff.as_secs_f32() * self.input_position.y * if self.input_position.y > 0.0 { self.settings.max_forward_acceleration } else { self.settings.max_forward_deceleration },
                diff.as_secs_f32() * self.input_position.z * self.settings.max_strafe_acceleration,
            )
        )
    }
}

impl EntityComponent for FreeSpaceControl {
    fn enable(&mut self, _world: &mut World) -> Result<()> {
        Ok(())
    }

    fn disable(&mut self, _world: &mut World) -> Result<()> {
        self.last_update = None;

        Ok(())
    }

    fn name(&self) -> &str {
        Self::debug_name()
    }
}

impl ComponentDebug for FreeSpaceControl {
    fn debug_name() -> &'static str {
        "FreeSpaceControl"
    }
}