working PID controller, but just the P part

This commit is contained in:
Franchioping 2025-12-12 11:36:13 +00:00
parent b36cbe9aed
commit cba59baa33
11 changed files with 570 additions and 124 deletions

317
Cargo.lock generated
View File

@ -7,6 +7,7 @@ name = "RaylibRustPhysics"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"clearscreen", "clearscreen",
"gilrs",
"glam 0.27.0", "glam 0.27.0",
"macroquad", "macroquad",
"nalgebra", "nalgebra",
@ -66,6 +67,12 @@ version = "2.10.0"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3" checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3"
[[package]]
name = "bumpalo"
version = "3.19.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "46c5e41b57b8bba42a04676d81cb89e9ee8e859a1a66f80a5a72e1cb76b34d43"
[[package]] [[package]]
name = "bytemuck" name = "bytemuck"
version = "1.24.0" version = "1.24.0"
@ -96,7 +103,7 @@ version = "4.0.2"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "85a8ab73a1c02b0c15597b22e09c7dc36e63b2f601f9d1e83ac0c3decd38b1ae" checksum = "85a8ab73a1c02b0c15597b22e09c7dc36e63b2f601f9d1e83ac0c3decd38b1ae"
dependencies = [ dependencies = [
"nix", "nix 0.29.0",
"terminfo", "terminfo",
"thiserror", "thiserror",
"which", "which",
@ -109,6 +116,22 @@ version = "1.1.0"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b" checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b"
[[package]]
name = "core-foundation"
version = "0.10.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b2a6cd9ae233e7f62ba4e9353e81a88df7fc8a5987b8d445b4d90c879bd156f6"
dependencies = [
"core-foundation-sys",
"libc",
]
[[package]]
name = "core-foundation-sys"
version = "0.8.7"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b"
[[package]] [[package]]
name = "crc32fast" name = "crc32fast"
version = "1.5.0" version = "1.5.0"
@ -220,6 +243,40 @@ dependencies = [
"wasip2", "wasip2",
] ]
[[package]]
name = "gilrs"
version = "0.11.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "bbb2c998745a3c1ac90f64f4f7b3a54219fd3612d7705e7798212935641ed18f"
dependencies = [
"fnv",
"gilrs-core",
"log",
"uuid",
"vec_map",
]
[[package]]
name = "gilrs-core"
version = "0.6.6"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "be11a71ac3564f6965839e2ed275bf4fcf5ce16d80d396e1dfdb7b2d80bd587e"
dependencies = [
"core-foundation",
"inotify",
"io-kit-sys",
"js-sys",
"libc",
"libudev-sys",
"log",
"nix 0.30.1",
"uuid",
"vec_map",
"wasm-bindgen",
"web-sys",
"windows",
]
[[package]] [[package]]
name = "glam" name = "glam"
version = "0.14.0" version = "0.14.0"
@ -374,6 +431,46 @@ dependencies = [
"png", "png",
] ]
[[package]]
name = "inotify"
version = "0.11.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f37dccff2791ab604f9babef0ba14fbe0be30bd368dc541e2b08d07c8aa908f3"
dependencies = [
"bitflags 2.10.0",
"inotify-sys",
"libc",
]
[[package]]
name = "inotify-sys"
version = "0.1.5"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e05c02b5e89bff3b946cedeca278abc628fe811e604f027c45a8aa3cf793d0eb"
dependencies = [
"libc",
]
[[package]]
name = "io-kit-sys"
version = "0.4.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "617ee6cf8e3f66f3b4ea67a4058564628cde41901316e19f559e14c7c72c5e7b"
dependencies = [
"core-foundation-sys",
"mach2",
]
[[package]]
name = "js-sys"
version = "0.3.83"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "464a3709c7f55f1f721e5389aa6ea4e3bc6aba669353300af094b29ffbdde1d8"
dependencies = [
"once_cell",
"wasm-bindgen",
]
[[package]] [[package]]
name = "libc" name = "libc"
version = "0.2.177" version = "0.2.177"
@ -386,6 +483,16 @@ version = "0.2.15"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de" checksum = "f9fbbcab51052fe104eb5e5d351cf728d30a5be1fe14d9be8a3b097481fb97de"
[[package]]
name = "libudev-sys"
version = "0.1.4"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3c8469b4a23b962c1396b9b451dda50ef5b283e8dd309d69033475fa9b334324"
dependencies = [
"libc",
"pkg-config",
]
[[package]] [[package]]
name = "linux-raw-sys" name = "linux-raw-sys"
version = "0.11.0" version = "0.11.0"
@ -398,6 +505,15 @@ version = "0.4.28"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432" checksum = "34080505efa8e45a4b816c349525ebe327ceaa8559756f0356cba97ef3bf7432"
[[package]]
name = "mach2"
version = "0.4.3"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "d640282b302c0bb0a2a8e0233ead9035e3bed871f0b7e81fe4a1ec829765db44"
dependencies = [
"libc",
]
[[package]] [[package]]
name = "macroquad" name = "macroquad"
version = "0.4.14" version = "0.4.14"
@ -532,6 +648,18 @@ dependencies = [
"libc", "libc",
] ]
[[package]]
name = "nix"
version = "0.30.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "74523f3a35e05aba87a1d978330aef40f67b0304ac79c1c00b294c9830543db6"
dependencies = [
"bitflags 2.10.0",
"cfg-if",
"cfg_aliases",
"libc",
]
[[package]] [[package]]
name = "nom" name = "nom"
version = "7.1.3" version = "7.1.3"
@ -611,6 +739,12 @@ dependencies = [
"malloc_buf", "malloc_buf",
] ]
[[package]]
name = "once_cell"
version = "1.21.3"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d"
[[package]] [[package]]
name = "ordered-float" name = "ordered-float"
version = "5.1.0" version = "5.1.0"
@ -693,6 +827,12 @@ dependencies = [
"siphasher", "siphasher",
] ]
[[package]]
name = "pkg-config"
version = "0.3.32"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7edddbd0b52d732b21ad9a5fab5c704c14cd949e5e9a1ec5929a24fded1b904c"
[[package]] [[package]]
name = "png" name = "png"
version = "0.17.16" version = "0.17.16"
@ -876,6 +1016,12 @@ dependencies = [
"windows-sys 0.61.2", "windows-sys 0.61.2",
] ]
[[package]]
name = "rustversion"
version = "1.0.22"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b39cdef0fa800fc44525c84ccb54a029961a8215f9619753635a9c0d2538d46d"
[[package]] [[package]]
name = "safe_arch" name = "safe_arch"
version = "0.7.4" version = "0.7.4"
@ -1028,6 +1174,16 @@ version = "1.0.22"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5" checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5"
[[package]]
name = "uuid"
version = "1.19.0"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e2e054861b4bd027cd373e18e8d8d8e6548085000e41290d95ce0c373a654b4a"
dependencies = [
"js-sys",
"wasm-bindgen",
]
[[package]] [[package]]
name = "vec_map" name = "vec_map"
version = "0.8.2" version = "0.8.2"
@ -1043,6 +1199,61 @@ dependencies = [
"wit-bindgen", "wit-bindgen",
] ]
[[package]]
name = "wasm-bindgen"
version = "0.2.106"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "0d759f433fa64a2d763d1340820e46e111a7a5ab75f993d1852d70b03dbb80fd"
dependencies = [
"cfg-if",
"once_cell",
"rustversion",
"wasm-bindgen-macro",
"wasm-bindgen-shared",
]
[[package]]
name = "wasm-bindgen-macro"
version = "0.2.106"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "48cb0d2638f8baedbc542ed444afc0644a29166f1595371af4fecf8ce1e7eeb3"
dependencies = [
"quote",
"wasm-bindgen-macro-support",
]
[[package]]
name = "wasm-bindgen-macro-support"
version = "0.2.106"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cefb59d5cd5f92d9dcf80e4683949f15ca4b511f4ac0a6e14d4e1ac60c6ecd40"
dependencies = [
"bumpalo",
"proc-macro2",
"quote",
"syn",
"wasm-bindgen-shared",
]
[[package]]
name = "wasm-bindgen-shared"
version = "0.2.106"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "cbc538057e648b67f72a982e708d485b2efa771e1ac05fec311f9f63e5800db4"
dependencies = [
"unicode-ident",
]
[[package]]
name = "web-sys"
version = "0.3.83"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "9b32828d774c412041098d182a8b38b16ea816958e07cf40eec2bc080ae137ac"
dependencies = [
"js-sys",
"wasm-bindgen",
]
[[package]] [[package]]
name = "which" name = "which"
version = "8.0.0" version = "8.0.0"
@ -1086,12 +1297,107 @@ version = "0.4.0"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f"
[[package]]
name = "windows"
version = "0.62.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "527fadee13e0c05939a6a05d5bd6eec6cd2e3dbd648b9f8e447c6518133d8580"
dependencies = [
"windows-collections",
"windows-core",
"windows-future",
"windows-numerics",
]
[[package]]
name = "windows-collections"
version = "0.3.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "23b2d95af1a8a14a3c7367e1ed4fc9c20e0a26e79551b1454d72583c97cc6610"
dependencies = [
"windows-core",
]
[[package]]
name = "windows-core"
version = "0.62.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b8e83a14d34d0623b51dce9581199302a221863196a1dde71a7663a4c2be9deb"
dependencies = [
"windows-implement",
"windows-interface",
"windows-link",
"windows-result",
"windows-strings",
]
[[package]]
name = "windows-future"
version = "0.3.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e1d6f90251fe18a279739e78025bd6ddc52a7e22f921070ccdc67dde84c605cb"
dependencies = [
"windows-core",
"windows-link",
"windows-threading",
]
[[package]]
name = "windows-implement"
version = "0.60.2"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "053e2e040ab57b9dc951b72c264860db7eb3b0200ba345b4e4c3b14f67855ddf"
dependencies = [
"proc-macro2",
"quote",
"syn",
]
[[package]]
name = "windows-interface"
version = "0.59.3"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3f316c4a2570ba26bbec722032c4099d8c8bc095efccdc15688708623367e358"
dependencies = [
"proc-macro2",
"quote",
"syn",
]
[[package]] [[package]]
name = "windows-link" name = "windows-link"
version = "0.2.1" version = "0.2.1"
source = "registry+https://github.com/rust-lang/crates.io-index" source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5"
[[package]]
name = "windows-numerics"
version = "0.3.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "6e2e40844ac143cdb44aead537bbf727de9b044e107a0f1220392177d15b0f26"
dependencies = [
"windows-core",
"windows-link",
]
[[package]]
name = "windows-result"
version = "0.4.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7781fa89eaf60850ac3d2da7af8e5242a5ea78d1a11c49bf2910bb5a73853eb5"
dependencies = [
"windows-link",
]
[[package]]
name = "windows-strings"
version = "0.5.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "7837d08f69c77cf6b07689544538e017c1bfcf57e34b4c0ff58e6c2cd3b37091"
dependencies = [
"windows-link",
]
[[package]] [[package]]
name = "windows-sys" name = "windows-sys"
version = "0.59.0" version = "0.59.0"
@ -1126,6 +1432,15 @@ dependencies = [
"windows_x86_64_msvc", "windows_x86_64_msvc",
] ]
[[package]]
name = "windows-threading"
version = "0.2.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "3949bd5b99cafdf1c7ca86b43ca564028dfe27d66958f2470940f73d86d75b37"
dependencies = [
"windows-link",
]
[[package]] [[package]]
name = "windows_aarch64_gnullvm" name = "windows_aarch64_gnullvm"
version = "0.52.6" version = "0.52.6"

View File

@ -12,6 +12,8 @@ glam = "0.27"
rand = "0.9.2" rand = "0.9.2"
strum = { version = "0.27", features = ["derive"] } strum = { version = "0.27", features = ["derive"] }
clearscreen = "4.0.2" clearscreen = "4.0.2"
gilrs = "0.11.0"
[[bin]] [[bin]]
name = "tools" name = "tools"

View File

@ -13,6 +13,7 @@
mkShell rec { mkShell rec {
buildInputs = [ buildInputs = [
pkg-config pkg-config
udev
rustup rustup
alsa-lib alsa-lib

View File

@ -1,8 +1,62 @@
use macroquad::prelude::*; use macroquad::prelude::*;
use std::any::Any;
mod drone_cam;
pub use drone_cam::AttachedCamera;
use crate::engine::World;
const MOVE_SPEED: f32 = 0.1; const MOVE_SPEED: f32 = 0.1;
const LOOK_SPEED: f32 = 0.05; const LOOK_SPEED: f32 = 0.05;
pub trait CameraController {
fn update(&mut self, delta: f32, world: &World);
fn apply(&self);
fn get_position(&self) -> Vec3;
fn get_front(&self) -> Vec3;
fn as_any(&self) -> &dyn Any;
fn as_mut_any(&mut self) -> &mut dyn Any;
}
impl CameraController for FirstPersonCamera {
fn update(&mut self, delta: f32, _world: &World) {
if is_key_pressed(KeyCode::Tab) {
self.grabbed = !self.grabbed;
set_cursor_grab(self.grabbed);
show_mouse(!self.grabbed);
}
self.handle_keyboard();
self.handle_mouse(delta);
}
fn apply(&self) {
set_camera(&Camera3D {
position: self.position,
up: self.up,
target: self.position + self.front,
..Default::default()
});
}
fn get_front(&self) -> Vec3 {
return self.front;
}
fn get_position(&self) -> Vec3 {
return self.position;
}
fn as_any(&self) -> &dyn Any {
self
}
fn as_mut_any(&mut self) -> &mut dyn Any {
self
}
}
pub struct FirstPersonCamera { pub struct FirstPersonCamera {
pub position: Vec3, pub position: Vec3,
yaw: f32, yaw: f32,
@ -100,24 +154,4 @@ impl FirstPersonCamera {
self.position -= Vec3::Y * MOVE_SPEED; self.position -= Vec3::Y * MOVE_SPEED;
} }
} }
pub fn update(&mut self, delta: f32) {
if is_key_pressed(KeyCode::Tab) {
self.grabbed = !self.grabbed;
set_cursor_grab(self.grabbed);
show_mouse(!self.grabbed);
}
self.handle_keyboard();
self.handle_mouse(delta);
}
pub fn apply(&self) {
set_camera(&Camera3D {
position: self.position,
up: self.up,
target: self.position + self.front,
..Default::default()
});
}
} }

64
src/camera/drone_cam.rs Normal file
View File

@ -0,0 +1,64 @@
use macroquad::prelude as mq;
use nalgebra as na;
use rapier3d::prelude as rp;
use std::any::Any;
use crate::{camera::CameraController, engine::World};
pub struct AttachedCamera {
pub attached_handle: rp::RigidBodyHandle,
pub front_local_direction: na::Vector3<f32>,
pub offset: na::Vector3<f32>,
translation: na::Vector3<f32>,
front: na::Vector3<f32>,
up: na::Vector3<f32>,
}
impl AttachedCamera {
pub fn new(
rb_handle: rp::RigidBodyHandle,
front_direction: na::Vector3<f32>,
offset: na::Vector3<f32>,
) -> AttachedCamera {
AttachedCamera {
attached_handle: rb_handle,
front_local_direction: front_direction,
offset,
translation: Default::default(),
front: Default::default(),
up: Default::default(),
}
}
}
impl CameraController for AttachedCamera {
fn get_position(&self) -> mq::Vec3 {
self.translation.into()
}
fn get_front(&self) -> mq::Vec3 {
self.front.into()
}
fn apply(&self) {
mq::set_camera(&mq::Camera3D {
position: self.translation.into(),
up: self.up.into(),
target: (self.translation + self.front).into(),
fovy: 2.0,
..Default::default()
});
}
fn update(&mut self, _delta: f32, world: &World) {
let rb = world.bodies.get(self.attached_handle).unwrap();
self.translation = *rb.translation() + rb.rotation().transform_vector(&self.offset);
self.up = rb.rotation().transform_vector(&na::vector![0.0, 1.0, 0.0]);
self.front = rb.position().transform_vector(&self.front_local_direction);
}
fn as_any(&self) -> &dyn Any {
self
}
fn as_mut_any(&mut self) -> &mut dyn Any {
self
}
}

View File

@ -10,7 +10,7 @@ use controller::*;
const AIR_DENSITY: f32 = 1.23; const AIR_DENSITY: f32 = 1.23;
const DRAG_CONSTANT: f32 = 1.3; const DRAG_CONSTANT: f32 = 1.3;
const DRAG_MAGIC_NUM: f32 = 0.5; const DRAG_MAGIC_NUM: f32 = 0.25;
pub struct MotorCharacteristics { pub struct MotorCharacteristics {
pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4], pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
@ -74,7 +74,7 @@ impl Drone {
* A Poor Man's fluid simulation :D * A Poor Man's fluid simulation :D
*/ */
// .linear_damping(0.2) // Damps velocity slowly // .linear_damping(0.2) // Damps velocity slowly
// .angular_damping(0.2) // Damps angular velocity slowly .angular_damping(0.2) // Damps angular velocity slowly
.build(), .build(),
); );
let width = 0.40; let width = 0.40;
@ -160,8 +160,8 @@ impl Drone {
rp::vector![0.0, -torque, 0.0] rp::vector![0.0, -torque, 0.0]
}; };
drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true); drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true);
println!("Torque: \n{:?}", torque); // println!("Torque: \n{:?}", torque);
println!("Thrust: \n{:?}", thrust); // println!("Thrust: \n{:?}", thrust);
} }
} }

View File

@ -3,7 +3,7 @@ use std::any::Any;
use crate::drone::MotorCharacteristics; use crate::drone::MotorCharacteristics;
#[derive(Default)] #[derive(Default, Clone, Copy)]
pub struct JoystickInput { pub struct JoystickInput {
// Value should be between 0 and 1 // Value should be between 0 and 1
pub throttle_input: f32, pub throttle_input: f32,

View File

@ -24,6 +24,7 @@ impl PControllerState {
pub struct PController { pub struct PController {
input: JoystickInput, input: JoystickInput,
pub target_rate: f32, pub target_rate: f32,
pub multiply_mode: bool,
state: PControllerState, state: PControllerState,
} }
@ -33,6 +34,7 @@ impl Default for PController {
input: JoystickInput::default(), input: JoystickInput::default(),
target_rate: f32::consts::PI, target_rate: f32::consts::PI,
state: Default::default(), state: Default::default(),
multiply_mode: false,
} }
} }
} }
@ -47,9 +49,9 @@ impl PController {
pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> { pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
let coords = na::Vector3::new( let coords = na::Vector3::new(
self.input.pitch_input,
self.input.yaw_input,
self.input.roll_input, self.input.roll_input,
self.input.yaw_input,
self.input.pitch_input,
) * self.target_rate; ) * self.target_rate;
return coords; return coords;
} }
@ -74,35 +76,36 @@ impl DroneController for PController {
let target = self.get_desired_angular_velocity(); let target = self.get_desired_angular_velocity();
let diff = target - current; let diff = target - current;
println!("Target: {:}", target);
println!("Current: {:}", current);
println!("Diff: {:}", diff);
let throttle = self.input.throttle_input; let throttle = self.input.throttle_input;
let roll = diff.z * 0.01; let roll = diff.z * 0.01;
let pitch = diff.x * 0.01; let pitch = diff.x * 0.01;
let yaw = diff.y; let yaw = diff.y;
/* let mut motors: [f32; 4] = match self.multiply_mode {
* Motor position indices true => {
* ^ - Front let mut t = [
* | 1.0 + yaw - pitch + roll,
* | 1.0 - yaw - pitch - roll,
* 1 --- 0 1.0 + yaw + pitch - roll,
* | | 1.0 - yaw + pitch + roll,
* | | ];
* 2 --- 3 for m in t.iter_mut() {
*/ *m *= self.input.throttle_input;
let mut motors = [ }
throttle + yaw - pitch + roll, t
throttle - yaw - pitch - roll, }
throttle + yaw + pitch - roll, false => {
throttle - yaw + pitch + roll, let mut t = [
]; throttle + yaw - pitch + roll,
throttle - yaw - pitch - roll,
throttle + yaw + pitch - roll,
throttle - yaw + pitch + roll,
];
t
}
};
// motors.max
let max = motors let max = motors
.iter() .iter()
.copied() .copied()
@ -113,13 +116,10 @@ impl DroneController for PController {
*v /= max; *v /= max;
} }
} }
return motors; return motors;
} }
/*
* These should be implemented like this for all the classes that implement DroneController
* Works as an example implementation
*/
fn as_any(&self) -> &dyn Any { fn as_any(&self) -> &dyn Any {
self self
} }

View File

@ -62,7 +62,7 @@ impl World {
pub fn clear_ofb(&mut self) { pub fn clear_ofb(&mut self) {
let mut coll_to_del: Vec<ColliderHandle> = Vec::new(); let mut coll_to_del: Vec<ColliderHandle> = Vec::new();
for (handle, col) in self.colliders.iter() { for (handle, col) in self.colliders.iter() {
if (col.translation().y < -15.0) { if col.translation().y < -30.0 {
coll_to_del.push(handle); coll_to_del.push(handle);
} }
} }

View File

@ -1,4 +1,6 @@
use gilrs;
use macroquad::prelude as mq; use macroquad::prelude as mq;
use nalgebra as na;
use rapier3d::prelude::*; use rapier3d::prelude::*;
mod engine; mod engine;
@ -27,8 +29,42 @@ fn window_conf() -> mq::Conf {
#[macroquad::main(window_conf)] #[macroquad::main(window_conf)]
async fn main() { async fn main() {
/* World Setup */
let mut world = World::default(); let mut world = World::default();
let mut renderer = Renderer::new();
world.register_free_collider(
ColliderBuilder::cuboid(30.0, 1.0, 30.0)
.translation(vector![0.0, -2.0, 0.0])
.restitution(0.5)
.build(),
None,
);
let mut drone_controller = drone::pidcontroller::PController::default();
drone_controller.set_input(JoystickInput {
throttle_input: 0.75,
yaw_input: 0.4,
roll_input: 0.7,
pitch_input: 0.0,
});
let mut drone_obj = drone::Drone::new(
&mut world,
Box::new(drone_controller),
drone::MotorCharacteristics {
max_thrust: 2.6,
max_torque: 0.5,
..Default::default()
},
0.350,
);
/* Renderer Setup */
let camera = camera::AttachedCamera::new(
drone_obj.rb_handle,
vector![1.0, 0.0, 0.0],
vector![0.5, 0.0, 0.0],
);
let mut renderer = Renderer::new(Box::new(camera));
renderer.light.set_location( renderer.light.set_location(
vector![70.0, 150.0, -90.0].into(), vector![70.0, 150.0, -90.0].into(),
@ -36,44 +72,23 @@ async fn main() {
); );
renderer.update_light(&world); renderer.update_light(&world);
// Ground collider /* Command Setup */
world.register_free_collider( // let mut gilrs = gilrs::Gilrs::new().unwrap();
ColliderBuilder::cuboid(20.0, 1.0, 20.0) //
.translation(vector![0.0, -2.0, 0.0]) // // Iterate over all connected gamepads
.restitution(0.5) // for (_id, gamepad) in gilrs.gamepads() {
.build(), // println!("{} is {:?}", gamepad.name(), gamepad.power_info());
None, // }
);
let mut controller = drone::pidcontroller::PController::default(); let mut input = JoystickInput::default();
controller.set_input(JoystickInput {
throttle_input: 0.75,
yaw_input: 0.4,
roll_input: 0.7,
pitch_input: 0.0,
});
let mut drone = drone::Drone::new(
&mut world,
Box::new(controller),
drone::MotorCharacteristics {
max_thrust: 2.6,
max_torque: 0.1,
..Default::default()
},
0.350,
);
loop { loop {
renderer.update_camera(); renderer.update_camera(&world);
if mq::is_key_pressed(mq::KeyCode::L) { if mq::is_key_pressed(mq::KeyCode::L) {
renderer renderer
.light .light
.set_location(renderer.camera.position, renderer.camera.front); .set_location(renderer.camera.get_position(), renderer.camera.get_front());
renderer.update_light(&world); renderer.update_light(&world);
println!(
"Light Pos: {}, Light Front Vec{}",
renderer.light.position, renderer.light.front
);
} }
if mq::is_key_pressed(mq::KeyCode::C) { if mq::is_key_pressed(mq::KeyCode::C) {
@ -83,43 +98,59 @@ async fn main() {
renderer.apply_config(); renderer.apply_config();
} }
if mq::is_key_down(mq::KeyCode::W) {
input.throttle_input += 4.0 * mq::get_frame_time();
}
if mq::is_key_down(mq::KeyCode::S) {
input.throttle_input -= 4.0 * mq::get_frame_time();
}
if mq::is_key_down(mq::KeyCode::A) {
input.yaw_input = 1.0;
} else if mq::is_key_down(mq::KeyCode::D) {
input.yaw_input = -1.0;
} else {
input.yaw_input = 0.0;
}
if mq::is_key_down(mq::KeyCode::Up) {
input.pitch_input = -1.0;
} else if mq::is_key_down(mq::KeyCode::Down) {
input.pitch_input = 1.0;
} else {
input.pitch_input = 0.0;
}
if mq::is_key_down(mq::KeyCode::Left) {
input.roll_input = -1.0;
} else if mq::is_key_down(mq::KeyCode::Right) {
input.roll_input = 1.0;
} else {
input.roll_input = 0.0;
}
input = input.clamp();
match drone_obj
.controller
.as_mut_any()
.downcast_mut::<drone::pidcontroller::PController>()
{
Some(cont) => {
cont.set_input(input);
}
None => {}
};
// Physics // Physics
world.step(); world.step();
let _ = clearscreen::clear(); let _ = clearscreen::clear();
drone.process_tick(&mut world); drone_obj.process_tick(&mut world);
// println!(
// "Translation: {:}",
// world.bodies.get(drone.rb_handle).unwrap().translation()
// );
println!(
"Angular Velocity: {:}",
world.bodies.get(drone.rb_handle).unwrap().angvel()
);
println!(
"Rotation: {:?}",
world
.bodies
.get(drone.rb_handle)
.unwrap()
.rotation()
.euler_angles()
);
// println!(
// "Velocity: {:}",
// world.bodies.get(drone.rb_handle).unwrap().linvel()
// );
println!(
"Motor Throttles: {:?}",
drone.controller.get_motor_throttles()
);
// Rendering // Rendering
renderer.draw(&mut world); renderer.draw(&mut world);
if world.tick % (60 / 30) == 0 { if world.tick % (60 / 30) == 0 {
// world.clear_ofb();
renderer.update_light(&world); renderer.update_light(&world);
world.clear_ofb();
} }
mq::next_frame().await; mq::next_frame().await;
} }
@ -134,8 +165,8 @@ fn add_objects(world: &mut World) {
.build(), .build(),
); );
world.register_collider( world.register_collider(
ColliderBuilder::cuboid(5.0, 5.0, 5.0) ColliderBuilder::cuboid(2.0, 2.0, 2.0)
.restitution(0.3) .restitution(0.1)
.build(), .build(),
body, body,
None, None,

View File

@ -57,7 +57,7 @@ pub struct Renderer {
depth_material: mq::Material, depth_material: mq::Material,
depth_target: mq::RenderTarget, depth_target: mq::RenderTarget,
pub light: Light, pub light: Light,
pub camera: cam::FirstPersonCamera, pub camera: Box<dyn cam::CameraController>,
ui_state: ui::MaterialUi, ui_state: ui::MaterialUi,
pub enable_shadows: bool, pub enable_shadows: bool,
} }
@ -115,7 +115,7 @@ impl MaterialUni {
} }
impl Renderer { impl Renderer {
pub fn new() -> Renderer { pub fn new(camera: Box<dyn cam::CameraController>) -> Renderer {
let material = mq::load_material( let material = mq::load_material(
mq::ShaderSource::Glsl { mq::ShaderSource::Glsl {
vertex: include_str!("shaders/shader.vert"), vertex: include_str!("shaders/shader.vert"),
@ -171,7 +171,6 @@ impl Renderer {
.set_filter(mq::FilterMode::Nearest); .set_filter(mq::FilterMode::Nearest);
let light = Light::new(); let light = Light::new();
let camera = FirstPersonCamera::new(mq::vec3(-50.0, 20.0, 0.0));
let mut r = Renderer { let mut r = Renderer {
base_material: material, base_material: material,
depth_material, depth_material,
@ -190,14 +189,14 @@ impl Renderer {
self.ui_state.apply(&self.base_material); self.ui_state.apply(&self.base_material);
self.enable_shadows = self.ui_state.render_shadows_bool; self.enable_shadows = self.ui_state.render_shadows_bool;
} }
pub fn update_camera(&mut self) { pub fn update_camera(&mut self, world: &World) {
self.camera.update(mq::get_frame_time()); self.camera.update(mq::get_frame_time(), world);
} }
pub fn update_light(&mut self, world: &World) { pub fn update_light(&mut self, world: &World) {
let light_view = self.light.view_matrix; let light_view = self.light.view_matrix;
let mut min_ls = mq::vec3(f32::MAX, f32::MAX, f32::MAX); let mut min_ls = mq::vec3(f32::MAX, 0.0, f32::MAX);
let mut max_ls = mq::vec3(f32::MIN, f32::MIN, f32::MIN); let mut max_ls = mq::vec3(f32::MIN, 50.0, f32::MIN);
// 1. iterate all world objects // 1. iterate all world objects
for (handle, _) in world.colliders.iter() { for (handle, _) in world.colliders.iter() {