RustPhysicsMQ/src/main.rs

use macroquad::prelude as mq;
use rapier3d::prelude::*;

mod engine;
use engine::*;

mod camera;
mod drone;
mod rendering;

mod graphics_util;

use crate::rendering::Renderer;

#[macroquad::main("3D")]
async fn main() {
    let mut world = World::default();
    let mut renderer = Renderer::new();

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

    // Physics Initialization
    world.register_free_collider(
        ColliderBuilder::cuboid(250.0, 5.0, 250.0)
            .restitution(0.5)
            .build(),
        None,
    );
    // add_objects(&mut world);
    //
    renderer.update_light(&world);

    let drone_rb_handle = world.register_body(
        RigidBodyBuilder::dynamic()
            .translation(vector![0.0, 10.0, 0.0])
            .rotation(vector![0.6, 0.0, 0.0])
            .linear_damping(0.1)
            .angular_damping(0.1)
            .build(),
    );
    world.register_collider(
        ColliderBuilder::cuboid(5.0, 0.5, 5.0)
            .restitution(0.3)
            .build(),
        drone_rb_handle,
        None,
    );
    let motor_positions = [
        point![5.0, 3.0, 5.0],
        point![-5.0, 3.0, 5.0],
        point![-5.0, 3.0, -5.0],
        point![5.0, 3.0, -5.0],
    ];

    let mut tick = 0;

    loop {
        renderer.update_camera();
        if mq::is_key_pressed(mq::KeyCode::L) {
            renderer
                .light
                .set_location(renderer.camera.position, renderer.camera.front);
            renderer.update_light(&world);
            println!(
                "Light Pos: {}, Light Front Vec{}",
                renderer.light.position, renderer.light.front
            );
        }

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

        // Physics Simulation
        {
            let drone_rb = world.bodies.get_mut(drone_rb_handle).unwrap();
            drone_rb.reset_forces(true);
            drone_rb.reset_torques(true);
            for (i, pos) in motor_positions.iter().enumerate() {
                // Thrust is applied upward at motor position
                let mut force = nalgebra::Vector3::new(0.0, drone_rb.mass() * 2.8, 0.0);

                // drone_rb.reset_forces(true);

                force = drone_rb.rotation().transform_vector(&force);
                drone_rb.add_force_at_point(force, drone_rb.position().transform_point(&pos), true);
                // drone_rb.apply_force_at_point(force, *pos, true);

                // let torque = if i % 2 == 0 {
                //     vector![0.0, 0.5, 0.0]
                // } else {
                //     vector![0.0, 0.5, 0.0]
                // };
                // drone_rb.add_torque(torque, true);
                // println!("{:}", drone_rb.translation());
            }
        }

        world.step();

        renderer.draw(&mut world);

        if tick >= 30 {
            world.clear_ofb();
            renderer.update_light(&world);
            tick = 0;
        }
        tick += 1;

        mq::next_frame().await;
    }
}

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(5.0, 5.0, 5.0)
                .restitution(0.3)
                .build(),
            body,
            None,
        );
    }
}