RustPhysicsMQ/src/main.rs

use gilrs;
use macroquad::prelude as mq;
use nalgebra as na;
use rapier3d::prelude::*;

mod engine;
use engine::*;

mod camera;
mod drone;
mod rendering;

mod graphics_util;

use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};

fn window_conf() -> mq::Conf {
    mq::Conf {
        window_title: "RustDroneSim".to_owned(),
        window_resizable: true,
        // fullscreen: true,
        platform: mq::miniquad::conf::Platform {
            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
            ..Default::default()
        },
        ..Default::default()
    }
}

#[macroquad::main(window_conf)]
async fn main() {
    /* World Setup */
    let mut world = World::default();

    world.register_free_collider(
        ColliderBuilder::cuboid(30.0, 1.0, 30.0)
            .translation(vector![0.0, -2.0, 0.0])
            .restitution(0.5)
            .build(),
        None,
    );

    let mut drone_controller = drone::pidcontroller::PIDController::default();
    drone_controller.set_input(JoystickInput {
        throttle_input: 0.75,
        yaw_input: 0.4,
        roll_input: 0.7,
        pitch_input: 0.0,
    });
    let mut drone_obj = drone::Drone::new(
        &mut world,
        Box::new(drone_controller),
        drone::MotorCharacteristics {
            max_thrust: 2.6,
            max_torque: 0.5,
            ..Default::default()
        },
        0.350,
    );

    /* Renderer Setup */
    let camera = camera::AttachedCamera::new(
        drone_obj.rb_handle,
        vector![1.0, 0.0, 0.0],
        vector![0.5, 0.0, 0.0],
    );
    let mut renderer = Renderer::new(Box::new(camera));

    renderer.light.set_location(
        vector![70.0, 150.0, -90.0].into(),
        vector![-0.4, -0.7, 0.6].into(),
    );
    renderer.update_light(&world);

    /* Command Setup */
    // let mut gilrs = gilrs::Gilrs::new().unwrap();
    //
    // // Iterate over all connected gamepads
    // for (_id, gamepad) in gilrs.gamepads() {
    //     println!("{} is {:?}", gamepad.name(), gamepad.power_info());
    // }

    let mut input = JoystickInput::default();

    loop {
        renderer.update_camera(&world);
        if mq::is_key_pressed(mq::KeyCode::L) {
            renderer
                .light
                .set_location(renderer.camera.get_position(), renderer.camera.get_front());
            renderer.update_light(&world);
        }

        if mq::is_key_pressed(mq::KeyCode::Q) {
            break;
        }
        if mq::is_key_pressed(mq::KeyCode::C) {
            add_objects(&mut world);
        }
        if mq::is_key_pressed(mq::KeyCode::M) {
            renderer.apply_config();
        }

        if mq::is_key_down(mq::KeyCode::W) {
            input.throttle_input += 4.0 * mq::get_frame_time();
        }
        if mq::is_key_down(mq::KeyCode::S) {
            input.throttle_input -= 4.0 * mq::get_frame_time();
        }

        if mq::is_key_down(mq::KeyCode::A) {
            input.yaw_input = 1.0;
        } else if mq::is_key_down(mq::KeyCode::D) {
            input.yaw_input = -1.0;
        } else {
            input.yaw_input = 0.0;
        }

        if mq::is_key_down(mq::KeyCode::Up) {
            input.pitch_input = -1.0;
        } else if mq::is_key_down(mq::KeyCode::Down) {
            input.pitch_input = 1.0;
        } else {
            input.pitch_input = 0.0;
        }

        if mq::is_key_down(mq::KeyCode::Left) {
            input.roll_input = -1.0;
        } else if mq::is_key_down(mq::KeyCode::Right) {
            input.roll_input = 1.0;
        } else {
            input.roll_input = 0.0;
        }
        input = input.clamp();
        match drone_obj
            .controller
            .as_mut_any()
            .downcast_mut::<drone::pidcontroller::PIDController>()
        {
            Some(cont) => {
                cont.set_input(input);
            }
            None => {}
        };

        // Physics
        world.step();
        let _ = clearscreen::clear();
        drone_obj.process_tick(&mut world);

        // Rendering
        renderer.draw(&mut world);

        if world.tick % (20) == 0 {
            // renderer.update_light(&world);
            // world.clear_ofb();
            mq::next_frame().await;
        }
    }

    match drone_obj
        .controller
        .as_mut_any()
        .downcast_mut::<drone::pidcontroller::PIDController>()
    {
        Some(cont) => {
            let _ = cont.save_logs_to_csv("./pid_logs.csv");
        }
        None => {}
    };
}

fn add_objects(world: &mut World) {
    for i in 0..1 {
        let body = world.register_body(
            RigidBodyBuilder::dynamic()
                .translation(vector![0.0, 50.0 + i as f32, 0.0])
                .rotation(vector![std::f32::consts::PI / 4.2, i as f32, i as f32])
                .build(),
        );
        world.register_collider(
            ColliderBuilder::cuboid(2.0, 2.0, 2.0)
                .restitution(0.1)
                .build(),
            body,
            None,
        );
    }
}