implement use_multithreading_depth_rendering, optimize rng

fix docs

Cargo.toml

@@ -1,6 +1,7 @@
 [package]
+exclude = ["assets/", "CONTRIBUTING.md", "CODE_OF_CONDUCT.md", "SECURITY.md"]
 name = "peng_quad"
-version = "0.5.0"
+version = "0.5.1"
 edition = "2021"
 rust-version = "1.76"
 authors = ["Yang Zhou <[email protected]>"]
@@ -27,11 +28,13 @@ codegen-units = 1 # Compile the entire crate as one unit.
 lto = "thin"      # Do a second optimization pass over the entire program, including dependencies.
 [dependencies]
 nalgebra = "0.33.0"
-rand = "0.8.5"
+rand = { version = "0.8.5", features = ["rand_chacha"] }
 rand_distr = "0.4.3"
 rerun = "0.18.2"
 thiserror = "1.0.63"
 serde = { version = "1.0.209", features = ["derive"] }
 serde_yaml = "0.9.34"
 env_logger = "0.11.5"
 log = "0.4.22"
+rayon = "1.10.0"
+rand_chacha = "0.3.1"

README.md

@@ -1,4 +1,4 @@
-# [![Peng](assets/Peng.svg)](https://github.com/makeecat/Peng)
+# [![Peng](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng.svg)](https://github.com/makeecat/Peng)
 
 [![License](https://img.shields.io/badge/license-MIT%2FApache-blue.svg)](https://github.com/makeecat/Peng#license)
 [![Crates.io](https://img.shields.io/crates/v/peng_quad.svg)](https://crates.io/crates/peng_quad)
@@ -11,7 +11,7 @@
 ## What is Peng
 
 Peng is a minimal quadrotor autonomy framework in Rust. It includes a simulator, controller, and planner, providing a basic framework for simulating quadrotor dynamics and control.
-![demo](assets/Peng_demo.gif)
+![demo](https://raw.githubusercontent.com/makeecat/Peng/main/assets/Peng_demo.gif)
 
 ## Getting Started
 

config/quad.yaml

@@ -1,5 +1,6 @@
 use_rerun: true # Enable visualization using rerun.io
-render_depth: true # Enable render depth if use_rerun is true
+render_depth: true # Enable rendering depth
+use_multithreading_depth_rendering: true # Enable multithreading for depth rendering for large resolution (above 32x24)
 
 simulation:
   control_frequency: 200 # Frequency of control loop execution (Hz)

src/config.rs

@@ -21,6 +21,8 @@ pub struct Config {
     pub use_rerun: bool,
     /// Render depth
     pub render_depth: bool,
+    /// MultiThreading depth rendering
+    pub use_multithreading_depth_rendering: bool,
 }
 
 #[derive(serde::Deserialize)]

src/lib.rs

@@ -16,8 +16,11 @@
 //! let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
 //! let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);
 //! ```
+use rand::SeedableRng;
+use rayon::prelude::*;
 pub mod config;
 use nalgebra::{Matrix3, Rotation3, SMatrix, UnitQuaternion, Vector3};
+use rand_chacha::ChaCha8Rng;
 use rand_distr::{Distribution, Normal};
 use std::f32::consts::PI;
 #[derive(thiserror::Error, Debug)]
@@ -204,6 +207,16 @@ pub struct Imu {
     pub accel_bias_std: f32,
     /// Standard deviation of gyroscope bias drift
     pub gyro_bias_std: f32,
+    /// Accelerometer noise distribution
+    accel_noise: Normal<f32>,
+    /// Gyroscope noise distribution
+    gyro_noise: Normal<f32>,
+    /// Accelerometer bias drift distribution
+    accel_bias_drift: Normal<f32>,
+    /// Gyroscope bias drift distribution
+    gyro_bias_drift: Normal<f32>,
+    /// Random number generator
+    rng: ChaCha8Rng,
 }
 /// Implements the IMU
 impl Imu {
@@ -226,15 +239,20 @@ impl Imu {
         gyro_noise_std: f32,
         accel_bias_std: f32,
         gyro_bias_std: f32,
-    ) -> Self {
-        Self {
+    ) -> Result<Self, SimulationError> {
+        Ok(Self {
             accel_bias: Vector3::zeros(),
             gyro_bias: Vector3::zeros(),
             accel_noise_std,
             gyro_noise_std,
             accel_bias_std,
             gyro_bias_std,
-        }
+            accel_noise: Normal::new(0.0, accel_noise_std)?,
+            gyro_noise: Normal::new(0.0, gyro_noise_std)?,
+            accel_bias_drift: Normal::new(0.0, accel_bias_std)?,
+            gyro_bias_drift: Normal::new(0.0, gyro_bias_std)?,
+            rng: ChaCha8Rng::from_entropy(),
+        })
     }
     /// Updates the IMU biases over time
     /// # Arguments
@@ -245,18 +263,15 @@ impl Imu {
     /// ```
     /// use peng_quad::Imu;
     ///
-    /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01);
+    /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap();
     /// imu.update(0.01).unwrap();
     /// ```
     pub fn update(&mut self, dt: f32) -> Result<(), SimulationError> {
-        let accel_drift = Normal::new(0.0, self.accel_bias_std * dt.sqrt())?;
-        let gyro_drift = Normal::new(0.0, self.gyro_bias_std * dt.sqrt())?;
-        let accel_drift_vector =
-            || Vector3::from_iterator((0..3).map(|_| accel_drift.sample(&mut rand::thread_rng())));
-        let gyro_drift_vector =
-            || Vector3::from_iterator((0..3).map(|_| gyro_drift.sample(&mut rand::thread_rng())));
-        self.accel_bias += accel_drift_vector();
-        self.gyro_bias += gyro_drift_vector();
+        let dt_sqrt = fast_sqrt(dt);
+        let accel_drift = self.accel_bias_drift.sample(&mut self.rng) * dt_sqrt;
+        let gyro_drift = self.gyro_bias_drift.sample(&mut self.rng) * dt_sqrt;
+        self.accel_bias += Vector3::from_iterator((0..3).map(|_| accel_drift));
+        self.gyro_bias += Vector3::from_iterator((0..3).map(|_| gyro_drift));
         Ok(())
     }
     /// Simulates IMU readings with added bias and noise
@@ -272,24 +287,22 @@ impl Imu {
     /// use nalgebra::Vector3;
     /// use peng_quad::Imu;
     ///
-    /// let imu = Imu::new(0.01, 0.01, 0.01, 0.01);
+    /// let mut imu = Imu::new(0.01, 0.01, 0.01, 0.01).unwrap();
     /// let true_acceleration = Vector3::new(0.0, 0.0, 9.81);
     /// let true_angular_velocity = Vector3::new(0.0, 0.0, 0.0);
     /// let (measured_acceleration, measured_ang_velocity) = imu.read(true_acceleration, true_angular_velocity).unwrap();
     /// ```
     pub fn read(
-        &self,
+        &mut self,
         true_acceleration: Vector3<f32>,
         true_angular_velocity: Vector3<f32>,
     ) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError> {
-        let accel_noise = Normal::new(0.0, self.accel_noise_std)?;
-        let gyro_noise = Normal::new(0.0, self.gyro_noise_std)?;
         let accel_noise_sample =
-            || Vector3::from_iterator((0..3).map(|_| accel_noise.sample(&mut rand::thread_rng())));
+            Vector3::from_iterator((0..3).map(|_| self.accel_noise.sample(&mut self.rng)));
         let gyro_noise_sample =
-            || Vector3::from_iterator((0..3).map(|_| gyro_noise.sample(&mut rand::thread_rng())));
-        let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample();
-        let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample();
+            Vector3::from_iterator((0..3).map(|_| self.gyro_noise.sample(&mut self.rng)));
+        let measured_acceleration = true_acceleration + self.accel_bias + accel_noise_sample;
+        let measured_ang_velocity = true_angular_velocity + self.gyro_bias + gyro_noise_sample;
         Ok((measured_acceleration, measured_ang_velocity))
     }
 }
@@ -1860,7 +1873,7 @@ impl Maze {
     /// maze.generate_obstacles(5);
     /// ```
     pub fn generate_obstacles(&mut self, num_obstacles: usize) {
-        let mut rng = rand::thread_rng();
+        let mut rng = ChaCha8Rng::from_entropy();
         self.obstacles = (0..num_obstacles)
             .map(|_| {
                 let position = Vector3::new(
@@ -1974,29 +1987,44 @@ impl Camera {
     /// let quad_orientation = UnitQuaternion::identity();
     /// let mut maze = Maze::new([-1.0, -1.0, -1.0], [1.0, 1.0, 1.0], 5, [0.1, 0.1, 0.1], [0.1, 0.5]);
     /// let mut depth_buffer = vec![0.0; 800 * 600];
-    /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer);
+    /// let use_multithreading = true;
+    /// camera.render_depth(&quad_position, &quad_orientation, &maze, &mut depth_buffer, use_multithreading);
     /// ```
     pub fn render_depth(
         &self,
         quad_position: &Vector3<f32>,
         quad_orientation: &UnitQuaternion<f32>,
         maze: &Maze,
         depth_buffer: &mut Vec<f32>,
+        use_multi_threading: bool,
     ) -> Result<(), SimulationError> {
         let (width, height) = self.resolution;
         let total_pixels = width * height;
-        depth_buffer.clear();
-        depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
         let rotation_camera_to_world = quad_orientation.to_rotation_matrix().matrix()
             * Matrix3::new(1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0);
         let rotation_world_to_camera = rotation_camera_to_world.transpose();
-        for i in 0..total_pixels {
-            depth_buffer.push(self.ray_cast(
-                quad_position,
-                &rotation_world_to_camera,
-                &(rotation_camera_to_world * self.ray_directions[i]),
-                maze,
-            )?);
+        if use_multi_threading {
+            depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
+            depth_buffer
+                .par_iter_mut()
+                .enumerate()
+                .try_for_each(|(i, depth)| {
+                    let direction = rotation_camera_to_world * self.ray_directions[i];
+                    *depth =
+                        self.ray_cast(quad_position, &rotation_world_to_camera, &direction, maze)?;
+                    Ok::<(), SimulationError>(())
+                })?;
+        } else {
+            depth_buffer.clear();
+            depth_buffer.reserve((total_pixels - depth_buffer.capacity()).max(0));
+            for i in 0..total_pixels {
+                depth_buffer.push(self.ray_cast(
+                    quad_position,
+                    &rotation_world_to_camera,
+                    &(rotation_camera_to_world * self.ray_directions[i]),
+                    maze,
+                )?);
+            }
         }
         Ok(())
     }
@@ -2058,7 +2086,8 @@ impl Camera {
             let c = oc.dot(&oc) - obstacle.radius * obstacle.radius;
             let discriminant = b * b - c;
             if discriminant >= 0.0 {
-                let t = -b - discriminant.sqrt();
+                // let t = -b - discriminant.sqrt();
+                let t = -b - fast_sqrt(discriminant);
                 if t > self.near && t < closest_hit {
                     closest_hit = t;
                 }
@@ -2411,3 +2440,15 @@ pub fn color_map_fn(gray: f32) -> (u8, u8, u8) {
         .clamp(0.0, 255.0) as u8;
     (r, g, b)
 }
+
+/// Fast square root function
+/// # Arguments
+/// * `x` - The input value
+/// # Returns
+/// * The square root of the input value
+#[inline(always)]
+fn fast_sqrt(x: f32) -> f32 {
+    let i = x.to_bits();
+    let i = 0x1fbd1df5 + (i >> 1);
+    f32::from_bits(i)
+}

src/main.rs

@@ -34,7 +34,7 @@ fn main() -> Result<(), SimulationError> {
         config.imu.gyro_noise_std,
         config.imu.accel_bias_std,
         config.imu.gyro_bias_std,
-    );
+    )?;
     let mut maze = Maze::new(
         config.maze.lower_bounds,
         config.maze.upper_bounds,
@@ -112,9 +112,7 @@ fn main() -> Result<(), SimulationError> {
             &quad.angular_velocity,
             quad.time_step,
         );
-        if i % (config.simulation.simulation_frequency / config.simulation.control_frequency)
-            == 0
-        {
+        if i % (config.simulation.simulation_frequency / config.simulation.control_frequency) == 0 {
             quad.update_dynamics_with_controls(thrust, &torque);
             previous_thrust = thrust;
             previous_torque = torque;
@@ -124,9 +122,16 @@ fn main() -> Result<(), SimulationError> {
         imu.update(quad.time_step)?;
         let (true_accel, true_gyro) = quad.read_imu()?;
         let (_measured_accel, _measured_gyro) = imu.read(true_accel, true_gyro)?;
-        if i % (config.simulation.simulation_frequency / config.simulation.log_frequency)
-            == 0
-        {
+        if i % (config.simulation.simulation_frequency / config.simulation.log_frequency) == 0 {
+            if config.render_depth {
+                camera.render_depth(
+                    &quad.position,
+                    &quad.orientation,
+                    &maze,
+                    &mut depth_buffer,
+                    config.use_multithreading_depth_rendering,
+                )?;
+            }
             if let Some(rec) = &rec {
                 rec.set_time_seconds("timestamp", time);
                 if trajectory.add_point(quad.position) {
@@ -141,12 +146,6 @@ fn main() -> Result<(), SimulationError> {
                     &_measured_gyro,
                 )?;
                 if config.render_depth {
-                    camera.render_depth(
-                        &quad.position,
-                        &quad.orientation,
-                        &maze,
-                        &mut depth_buffer,
-                    )?;
                     log_depth_image(
                         &rec,
                         &depth_buffer,