72 lines
1.9 KiB
Rust
72 lines
1.9 KiB
Rust
|
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);
|
||
|
|
}
|
||
|
|
}
|