drone. base controller implementation

src/camera.rs

@@ -19,8 +19,8 @@ pub struct FirstPersonCamera {
 
 impl FirstPersonCamera {
     pub fn new(position: Vec3) -> Self {
-        let yaw: f32 = 1.18;
+        let yaw: f32 = 0.0;
-        let pitch: f32 = 0.0;
+        let pitch: f32 = -0.25;
         let world_up = vec3(0.0, 1.0, 0.0);
 
         let front = vec3(

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<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);
+    }
+}

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<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
+    }
+}

src/main.rs

@@ -33,29 +33,7 @@ async fn main() {
     //
     renderer.update_light(&world);
 
-    let drone_rb_handle = world.register_body(
+    let mut tick: u64 = 0;
-        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();
@@ -78,38 +56,37 @@ async fn main() {
         }
 
         // Physics Simulation
-        {
+        // {
-            let drone_rb = world.bodies.get_mut(drone_rb_handle).unwrap();
+        //     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);
-
+        //     drone_rb.reset_torques(true);
-                force = drone_rb.rotation().transform_vector(&force);
+        //     for (i, pos) in motor_positions.iter().enumerate() {
-                drone_rb.add_force_at_point(force, drone_rb.position().transform_point(&pos), true);
+        //         // Thrust is applied upward at motor position
-                // drone_rb.apply_force_at_point(force, *pos, true);
+        //         let mut force = nalgebra::Vector3::new(0.0, drone_rb.mass() * 2.8, 0.0);
-
+        //
-                // let torque = if i % 2 == 0 {
+        //         // drone_rb.reset_forces(true);
-                //     vector![0.0, 0.5, 0.0]
+        //
-                // } else {
+        //         force = drone_rb.rotation().transform_vector(&force);
-                //     vector![0.0, 0.5, 0.0]
+        //         drone_rb.add_force_at_point(force, drone_rb.position().transform_point(&pos), true);
-                // };
+        //         // drone_rb.apply_force_at_point(force, *pos, true);
-                // drone_rb.add_torque(torque, true);
+        //
-                // println!("{:}", drone_rb.translation());
+        //         // 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;
 

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,