From fc0c894e4f3e6b94633cbf75af446446b48f3ec7 Mon Sep 17 00:00:00 2001 From: Franchioping Date: Mon, 8 Dec 2025 21:18:04 +0000 Subject: [PATCH] drone. base controller implementation --- src/camera.rs | 4 +-- src/drone.rs | 71 +++++++++++++++++++++++++++++++++++++++ src/drone/controller.rs | 41 +++++++++++++++++++++++ src/main.rs | 73 ++++++++++++++--------------------------- src/rendering.rs | 2 +- 5 files changed, 140 insertions(+), 51 deletions(-) create mode 100644 src/drone.rs create mode 100644 src/drone/controller.rs diff --git a/src/camera.rs b/src/camera.rs index a44da04..acf8ff5 100644 --- a/src/camera.rs +++ b/src/camera.rs @@ -19,8 +19,8 @@ pub struct FirstPersonCamera { impl FirstPersonCamera { pub fn new(position: Vec3) -> Self { - let yaw: f32 = 1.18; - let pitch: f32 = 0.0; + let yaw: f32 = 0.0; + let pitch: f32 = -0.25; let world_up = vec3(0.0, 1.0, 0.0); let front = vec3( diff --git a/src/drone.rs b/src/drone.rs new file mode 100644 index 0000000..c921d1a --- /dev/null +++ b/src/drone.rs @@ -0,0 +1,71 @@ +use nalgebra as na; +use rapier3d::prelude as rp; + +use crate::engine::World; + +mod controller; +use controller::*; + +pub struct MotorCharacteristics { + relative_motor_positions: [na::OPoint>; 4], + max_thrust: f32, + max_torque: f32, +} + +impl Default for MotorCharacteristics { + fn default() -> Self { + Self { + relative_motor_positions: [ + rp::point![5.0, 3.0, 5.0], + rp::point![-5.0, 3.0, 5.0], + rp::point![-5.0, 3.0, -5.0], + rp::point![5.0, 3.0, -5.0], + ], + max_thrust: 10.0, + max_torque: 1.0, + } + } +} + +struct Drone { + rb_handle: rp::RigidBodyHandle, + motor_characteristics: MotorCharacteristics, + pub controller: Box, +} + +impl Drone { + pub fn new( + world: &mut World, + controller: Box, + motor_characteristics: MotorCharacteristics, + ) -> Drone { + let drone_rb_handle = world.register_body( + rp::RigidBodyBuilder::dynamic() + .translation(rp::vector![0.0, 10.0, 0.0]) + .rotation(rp::vector![0.6, 0.0, 0.0]) + .linear_damping(0.1) + .angular_damping(0.1) + .build(), + ); + world.register_collider( + rp::ColliderBuilder::cuboid(5.0, 0.5, 5.0) + .restitution(0.3) + .build(), + drone_rb_handle, + None, + ); + + controller.set_motor_characteristics(&motor_characteristics); + + return Drone { + rb_handle: drone_rb_handle, + controller: controller, + motor_characteristics: motor_characteristics, + }; + } + pub fn update_controller(&mut self, world: &World, tick: u64) { + let rb = world.bodies.get(self.rb_handle).unwrap(); + self.controller.set_position(*rb.position()); + self.controller.set_time(tick as f32 / 60.0); + } +} diff --git a/src/drone/controller.rs b/src/drone/controller.rs new file mode 100644 index 0000000..b00993c --- /dev/null +++ b/src/drone/controller.rs @@ -0,0 +1,41 @@ +use rapier3d::prelude as rp; +use std::any::Any; + +use crate::drone::MotorCharacteristics; + +pub trait DroneController { + fn as_any(&self) -> &dyn Any; + + fn as_mut_any(&mut self) -> &mut dyn Any; + + fn set_position(&self, position: rp::Isometry); + fn set_time(&self, time: f32); + fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics); + + fn get_motor_speeds(&self) -> [f32; 4]; +} + +struct DefaultController { + motor_speed: f32, +} + +impl DefaultController { + pub fn new(motor_speed: f32) -> DefaultController { + return DefaultController { motor_speed }; + } +} + +impl DroneController for DefaultController { + fn set_position(&self, _position: rp::Isometry) {} + fn set_time(&self, _time: f32) {} + fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {} + fn get_motor_speeds(&self) -> [f32; 4] { + return [self.motor_speed; 4]; + } + fn as_any(&self) -> &dyn Any { + self + } + fn as_mut_any(&mut self) -> &mut dyn Any { + self + } +} diff --git a/src/main.rs b/src/main.rs index b1ba1a3..8c2b521 100644 --- a/src/main.rs +++ b/src/main.rs @@ -33,29 +33,7 @@ async fn main() { // 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; + let mut tick: u64 = 0; loop { renderer.update_camera(); @@ -78,38 +56,37 @@ async fn main() { } // 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()); - } - } + // { + // 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 { + if tick % 30 == 0 { world.clear_ofb(); renderer.update_light(&world); - tick = 0; } tick += 1; diff --git a/src/rendering.rs b/src/rendering.rs index 53a573b..305bb13 100644 --- a/src/rendering.rs +++ b/src/rendering.rs @@ -170,7 +170,7 @@ impl Renderer { .set_filter(mq::FilterMode::Nearest); let light = Light::new(); - let camera = FirstPersonCamera::new(mq::vec3(10.0, 50.0, 10.0)); + let camera = FirstPersonCamera::new(mq::vec3(-400.0, 100.0, 0.0)); let mut r = Renderer { base_material: material, depth_material,