non working drone physics. No rotation

src/main.rs

@@ -16,6 +16,11 @@ 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(50.0, 5.0, 50.0)
@@ -23,9 +28,31 @@ async fn main() {
             .build(),
         None,
     );
-    add_objects(&mut world);
+    // add_objects(&mut world);
+    //
+    renderer.update_light(&world);
 
-    let mut i = 0;
+    let drone_rb_handle = world.register_body(
+        RigidBodyBuilder::dynamic()
+            .translation(vector![0.0, 10.0, 0.0])
+            .rotation(vector![0.1, 0.0, 0.0])
+            .build(),
+    );
+    world.register_collider(
+        ColliderBuilder::cuboid(5.0, 0.5, 5.0)
+            .restitution(0.3)
+            .build(),
+        drone_rb_handle,
+        None,
+    );
+    let motor_positions = [
+        vector![5.0, 0.0, 5.0],
+        vector![-5.0, 0.0, 5.0],
+        vector![-5.0, 0.0, -5.0],
+        vector![5.0, 0.0, -5.0],
+    ];
+
+    let mut tick = 0;
     loop {
         renderer.update_camera();
         if mq::is_key_pressed(mq::KeyCode::L) {
@@ -48,15 +75,44 @@ async fn main() {
 
         // Physics Simulation
         world.step();
+
+        {
+            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 force = nalgebra::Vector3::new(0.0, drone_rb.mass() * 2.5, 0.0);
+
+                // drone_rb.reset_forces(true);
+
+                drone_rb.add_force_at_point(
+                    drone_rb.rotation().transform_vector(&force),
+                    point![pos.x, pos.y, pos.z],
+                    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());
+            }
+        }
+
         renderer.draw(&mut world);
 
-        mq::next_frame().await;
-
-        i += 1;
-        if i > 30 {
+        if tick >= 30 {
             world.clear_ofb();
             renderer.update_light(&world);
+            tick = 0;
         }
+        tick += 1;
+
+        mq::next_frame().await;
     }
 }