Cansat
/
RustPhysicsMQ
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

drone. base controller implementation

This commit is contained in:
Franchioping 2025-12-08 21:18:04 +00:00
parent 91598ea20e
commit fc0c894e4f
5 changed files with 140 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/camera.rs
View File

@ -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(

71
src/drone.rs Normal file
View File

@ -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<f32, na::Const<3>>; 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<dyn DroneController>,
}
impl Drone {
pub fn new(
world: &mut World,
controller: Box<dyn DroneController>,
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);
}
}

41
src/drone/controller.rs Normal file
View File

@ -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<f32>);
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<f32>) {}
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
}
}

73
src/main.rs
View File

@ -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;

2
src/rendering.rs
View File

@ -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,