diff --git a/Cargo.toml b/Cargo.toml index 5a260c9..6f2b4ba 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,6 +14,7 @@ rust-version = "1.62.1" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dev-dependencies] +criterion = "0.3" rand = "0.8.5" [dependencies] @@ -30,3 +31,7 @@ panic = "abort" [package.metadata.docs.rs] all-features = true + +[[bench]] +name = "bench_dubins" +harness = false diff --git a/benches/bench_dubins.rs b/benches/bench_dubins.rs new file mode 100644 index 0000000..c91f9fd --- /dev/null +++ b/benches/bench_dubins.rs @@ -0,0 +1,53 @@ +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use dubins_paths::{DubinsPath, PathType, PosRot}; + +const TURN_RADIUS: f32 = 1. / 0.00076; + +fn setup_benchmark() -> (PosRot, PosRot) { + let q0: PosRot = [2000., 2000., 0.].into(); + let q1: PosRot = [0., 0., std::f32::consts::PI].into(); + (q0, q1) +} + +fn bench_shortest_path_type(c: &mut Criterion, name: &str, path_types: &[PathType]) { + let (q0, q1) = setup_benchmark(); + + c.bench_function(name, |b| { + b.iter(|| { + DubinsPath::shortest_in(black_box(q0), black_box(q1), black_box(TURN_RADIUS), black_box(path_types)).unwrap() + }) + }); +} + +fn bench_shortest_csc_path(c: &mut Criterion) { + bench_shortest_path_type(c, "shortest_csc_path", &PathType::CSC); +} + +fn bench_shortest_ccc_path(c: &mut Criterion) { + bench_shortest_path_type(c, "shortest_ccc_path", &PathType::CCC); +} + +fn bench_shortest_path(c: &mut Criterion) { + let (q0, q1) = setup_benchmark(); + + c.bench_function("shortest_path", |b| { + b.iter(|| DubinsPath::shortest_from(black_box(q0), black_box(q1), black_box(TURN_RADIUS)).unwrap()) + }); +} + +fn bench_many_sample(c: &mut Criterion) { + const STEP_DISTANCE: f32 = 10.; + let (q0, q1) = setup_benchmark(); + let path = DubinsPath::shortest_from(q0, q1, TURN_RADIUS).unwrap(); + + c.bench_function("many_sample", |b| b.iter(|| path.sample_many(black_box(STEP_DISTANCE)))); +} + +criterion_group!( + benches, + bench_shortest_csc_path, + bench_shortest_ccc_path, + bench_shortest_path, + bench_many_sample +); +criterion_main!(benches); diff --git a/src/lib.rs b/src/lib.rs index deaf384..2d03e10 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -146,7 +146,6 @@ impl PathType { /// Convert the path type an array of it's [`SegmentType`]s #[must_use] - #[inline] pub const fn to_segment_types(&self) -> [SegmentType; 3] { match self { Self::LSL => SegmentType::LSL, @@ -186,28 +185,24 @@ pub struct PosRot([f32; 3]); impl PosRot { /// Create a new `PosRot` from a position and rotation #[must_use] - #[inline] pub const fn from_f32(x: f32, y: f32, rot: f32) -> Self { Self([x, y, rot]) } /// Get the x position #[must_use] - #[inline] pub const fn x(&self) -> f32 { self.0[0] } /// Get the y position #[must_use] - #[inline] pub const fn y(&self) -> f32 { self.0[1] } /// Get the rotation #[must_use] - #[inline] pub const fn rot(&self) -> f32 { self.0[2] } @@ -222,42 +217,36 @@ pub struct PosRot(Vec2, f32); impl PosRot { /// Create a new `PosRot` from a `Vec2` and rotation #[must_use] - #[inline] pub const fn new(pos: Vec2, rot: f32) -> Self { Self(pos, rot) } /// Create a new `PosRot` from a position and rotation #[must_use] - #[inline] pub const fn from_f32(x: f32, y: f32, rot: f32) -> Self { Self(Vec2::new(x, y), rot) } /// Get the position #[must_use] - #[inline] pub const fn pos(&self) -> Vec2 { self.0 } /// Get the x position #[must_use] - #[inline] pub const fn x(&self) -> f32 { self.0.x } /// Get the y position #[must_use] - #[inline] pub const fn y(&self) -> f32 { self.0.y } /// Get the rotation #[must_use] - #[inline] pub const fn rot(&self) -> f32 { self.1 } @@ -265,7 +254,6 @@ impl PosRot { impl PosRot { #[must_use] - #[inline] const fn from_rot(rot: Self) -> Self { Self::from_f32(0., 0., rot.rot()) } @@ -273,14 +261,12 @@ impl PosRot { impl Add for PosRot { type Output = Self; - #[inline] fn add(self, rhs: Self) -> Self { Self::from_f32(self.x() + rhs.x(), self.y() + rhs.y(), self.rot() + rhs.rot()) } } impl From<[f32; 3]> for PosRot { - #[inline] fn from(posrot: [f32; 3]) -> Self { Self::from_f32(posrot[0], posrot[1], posrot[2]) } @@ -469,7 +455,6 @@ impl Intermediate { /// /// assert!(word.is_ok()); /// ``` - #[inline] pub fn word(&self, path_type: PathType) -> Result { match path_type { PathType::LSL => self.lsl(), @@ -488,7 +473,6 @@ impl Intermediate { /// /// * `theta`: The value to be modded #[must_use] -#[inline] pub fn mod2pi(theta: f32) -> f32 { theta.rem_euclid(2. * PI) } @@ -548,7 +532,6 @@ impl DubinsPath { } /// Scale the target configuration, translate back to the original starting point - #[inline] fn offset(&self, q: PosRot) -> PosRot { PosRot::from_f32( q.x() * self.rho + self.qi.x(), @@ -705,7 +688,6 @@ impl DubinsPath { /// /// assert!(shortest_path_possible.is_ok()); /// ``` - #[inline] pub fn shortest_from(q0: PosRot, q1: PosRot, rho: f32) -> Result { Self::shortest_in(q0, q1, rho, &PathType::ALL) } @@ -742,7 +724,6 @@ impl DubinsPath { /// /// assert!(path.is_ok()); /// ``` - #[inline] pub fn new(q0: PosRot, q1: PosRot, rho: f32, path_type: PathType) -> Result { Ok(Self { qi: q0, @@ -765,7 +746,6 @@ impl DubinsPath { /// let total_path_length = shortest_path_possible.length(); /// ``` #[must_use] - #[inline] pub fn length(&self) -> f32 { (self.param[0] + self.param[1] + self.param[2]) * self.rho } @@ -789,7 +769,6 @@ impl DubinsPath { /// let total_segment_length: f32 = shortest_path_possible.segment_length(1); /// ``` #[must_use] - #[inline] pub fn segment_length(&self, i: usize) -> f32 { self.param[i] * self.rho } @@ -813,7 +792,6 @@ impl DubinsPath { /// /// let samples: Vec = shortest_path_possible.sample_many(step_distance); /// ``` - #[inline] #[must_use] pub fn sample_many(&self, step_distance: f32) -> Vec { self.sample_many_range(step_distance, 0_f32..self.length()) @@ -882,7 +860,6 @@ impl DubinsPath { /// let endpoint: PosRot = shortest_path_possible.endpoint(); /// ``` #[must_use] - #[inline] pub fn endpoint(&self) -> PosRot { self.sample(self.length()) } @@ -926,3 +903,90 @@ impl DubinsPath { } } } + +#[cfg(test)] +mod tests { + + use super::{mod2pi, DubinsPath, NoPathError, PathType, PosRot, SegmentType}; + use core::f32::consts::PI; + use rand::Rng; + use std::mem::size_of; + + const TURN_RADIUS: f32 = 1. / 0.00076; + + #[test] + fn mod2pi_test() { + assert!(mod2pi(-f32::from_bits(1)) >= 0.); + assert_eq!(mod2pi(2. * PI), 0.); + } + + #[test] + fn many_path_correctness() { + #[cfg(feature = "glam")] + fn angle_2d(vec1: f32, vec2: f32) -> f32 { + glam::Vec3A::new(vec1.cos(), vec1.sin(), 0.) + .dot(glam::Vec3A::new(vec2.cos(), vec2.sin(), 0.)) + .clamp(-1., 1.) + .acos() + } + + #[cfg(not(feature = "glam"))] + fn angle_2d(vec1: f32, vec2: f32) -> f32 { + (vec1.cos() * vec1.cos() + vec2.sin() * vec2.sin()).clamp(-1., 1.).acos() + } + + // Test that the path is correct for a number of random configurations. + // If no path is found, just skip. + // If the path is found the sampled endpoint is different from the specified endpoint, then fail. + + let runs = 50_000; + let mut thread_rng = rand::thread_rng(); + let mut error = 0; + + for _ in 0..runs { + let q0 = PosRot::from_f32( + thread_rng.gen_range(-10000_f32..10000.), + thread_rng.gen_range(-10000_f32..10000.), + thread_rng.gen_range((-2. * PI)..(2. * PI)), + ); + let q1 = PosRot::from_f32( + thread_rng.gen_range(-10000_f32..10000.), + thread_rng.gen_range(-10000_f32..10000.), + thread_rng.gen_range((-2. * PI)..(2. * PI)), + ); + + let path = match DubinsPath::shortest_from(q0, q1, TURN_RADIUS) { + Ok(p) => p, + Err(_) => continue, + }; + + let endpoint = path.endpoint(); + + #[cfg(feature = "glam")] + if q1.pos().distance(endpoint.pos()) > 1. || angle_2d(q1.rot(), endpoint.rot()) > 0.1 { + println!("Endpoint is different! {:?} | {q0:?} | {q1:?} | {endpoint:?}", path.path_type); + error += 1; + } + + #[cfg(not(feature = "glam"))] + if (q1.x() - endpoint.x()).abs() > 1. + || (q1.x() - endpoint.x()).abs() > 1. + || angle_2d(q1.rot(), endpoint.rot()) > 0.1 + { + println!("Endpoint is different! {:?} | {q0:?} | {q1:?} | {endpoint:?}", path.path_type); + error += 1; + } + } + + assert_eq!(error, 0) + } + + #[test] + fn size_of_items() { + assert_eq!(size_of::(), 12); + assert_eq!(size_of::(), 32); + assert_eq!(size_of::(), 1); + assert_eq!(size_of::(), 1); + assert_eq!(size_of::(), 0); + } +} diff --git a/tests/tests.rs b/tests/tests.rs deleted file mode 100644 index 912ea3f..0000000 --- a/tests/tests.rs +++ /dev/null @@ -1,195 +0,0 @@ -extern crate dubins_paths; - -use core::f32::consts::PI; -use dubins_paths::{mod2pi, DubinsPath, NoPathError, PathType, PosRot, SegmentType}; -use rand::Rng; -use std::{mem::size_of, panic::panic_any, time::Instant}; - -const TURN_RADIUS: f32 = 1. / 0.00076; - -#[test] -fn mod2pi_test() { - assert!(mod2pi(-f32::from_bits(1)) >= 0.); - assert_eq!(mod2pi(2. * PI), 0.); -} - -#[test] -fn fast_shortest_csc_path() { - let runs = 100_000; - let mut times = Vec::with_capacity(runs); - - for _ in 0..runs { - let start = Instant::now(); - - let q0: PosRot = [2000., 2000., 0.].into(); - let q1: PosRot = [0., 0., PI].into(); - - if let Err(err) = DubinsPath::shortest_in(q0, q1, TURN_RADIUS, &PathType::CCC) { - panic_any(err); - } - - times.push(start.elapsed().as_secs_f32()); - } - - let total_elapsed = times.iter().sum::(); - let elapsed: f32 = total_elapsed / (runs as f32); - let elapsed_ms = elapsed * 1000.; - println!("Ran test in an average of {elapsed} seconds ({elapsed_ms}ms) - total {total_elapsed} seconds"); - assert!(elapsed_ms < 0.002); -} - -#[test] -fn fast_shortest_ccc_path() { - let runs = 100_000; - let mut times = Vec::with_capacity(runs); - - for _ in 0..runs { - let start = Instant::now(); - - let q0: PosRot = [2000., 2000., 0.].into(); - let q1: PosRot = [0., 0., PI].into(); - - if let Err(err) = DubinsPath::shortest_in(q0, q1, TURN_RADIUS, &PathType::CSC) { - panic_any(err); - } - - times.push(start.elapsed().as_secs_f32()); - } - - let total_elapsed = times.iter().sum::(); - let elapsed: f32 = total_elapsed / (runs as f32); - let elapsed_ms = elapsed * 1000.; - println!("Ran test in an average of {elapsed} seconds ({elapsed_ms}ms) - total {total_elapsed} seconds"); - assert!(elapsed_ms < 0.002); -} - -#[test] -fn fast_shortest_path() { - let runs = 100_000; - let mut times = Vec::with_capacity(runs); - - for _ in 0..runs { - let start = Instant::now(); - - let q0: PosRot = [2000., 2000., 0.].into(); - let q1: PosRot = [0., 0., PI].into(); - - if let Err(err) = DubinsPath::shortest_from(q0, q1, TURN_RADIUS) { - panic_any(err); - } - - times.push(start.elapsed().as_secs_f32()); - } - - let total_elapsed = times.iter().sum::(); - let elapsed: f32 = total_elapsed / (runs as f32); - let elapsed_ms = elapsed * 1000.; - println!("Ran test in an average of {elapsed} seconds ({elapsed_ms}ms) - total {total_elapsed} seconds"); - assert!(elapsed_ms < 0.002); -} - -#[test] -fn fast_many_sample() { - const STEP_DISTANCE: f32 = 10.; - - let runs = 50_000; - let mut times = Vec::with_capacity(runs); - - let q0: PosRot = [2000., 2000., 0.].into(); - let q1: PosRot = [0., 0., PI].into(); - - let path = match DubinsPath::shortest_from(q0, q1, TURN_RADIUS) { - Ok(p) => p, - Err(err) => panic_any(err), - }; - - let expected_samples = (path.length() / STEP_DISTANCE).floor() as usize; - let num_samples = path.sample_many(STEP_DISTANCE).len(); - - // it's ok for it to be one less than expected due to floating point errors - assert!(expected_samples == num_samples || expected_samples + 1 == num_samples); - - for _ in 0..runs { - let start = Instant::now(); - - let _ = path.sample_many(STEP_DISTANCE); - - times.push(start.elapsed().as_secs_f32()); - } - - let total_elapsed = times.iter().sum::(); - let elapsed: f32 = total_elapsed / (runs as f32); - let elapsed_ms = elapsed * 1000.; - println!("Ran test in an average of {elapsed} seconds ({elapsed_ms}ms) - total {total_elapsed} seconds"); - assert!(elapsed_ms < 0.1); -} - -#[test] -fn many_path_correctness() { - #[cfg(feature = "glam")] - fn angle_2d(vec1: f32, vec2: f32) -> f32 { - glam::Vec3A::new(vec1.cos(), vec1.sin(), 0.) - .dot(glam::Vec3A::new(vec2.cos(), vec2.sin(), 0.)) - .clamp(-1., 1.) - .acos() - } - - #[cfg(not(feature = "glam"))] - fn angle_2d(vec1: f32, vec2: f32) -> f32 { - (vec1.cos() * vec1.cos() + vec2.sin() * vec2.sin()).clamp(-1., 1.).acos() - } - - // Test that the path is correct for a number of random configurations. - // If no path is found, just skip. - // If the path is found the sampled endpoint is different from the specified endpoint, then fail. - - let runs = 50_000; - let mut thread_rng = rand::thread_rng(); - let mut error = 0; - - for _ in 0..runs { - let q0 = PosRot::from_f32( - thread_rng.gen_range(-10000_f32..10000.), - thread_rng.gen_range(-10000_f32..10000.), - thread_rng.gen_range((-2. * PI)..(2. * PI)), - ); - let q1 = PosRot::from_f32( - thread_rng.gen_range(-10000_f32..10000.), - thread_rng.gen_range(-10000_f32..10000.), - thread_rng.gen_range((-2. * PI)..(2. * PI)), - ); - - let path = match DubinsPath::shortest_from(q0, q1, TURN_RADIUS) { - Ok(p) => p, - Err(_) => continue, - }; - - let endpoint = path.endpoint(); - - #[cfg(feature = "glam")] - if q1.pos().distance(endpoint.pos()) > 1. || angle_2d(q1.rot(), endpoint.rot()) > 0.1 { - println!("Endpoint is different! {:?} | {q0:?} | {q1:?} | {endpoint:?}", path.path_type); - error += 1; - } - - #[cfg(not(feature = "glam"))] - if (q1.x() - endpoint.x()).abs() > 1. - || (q1.x() - endpoint.x()).abs() > 1. - || angle_2d(q1.rot(), endpoint.rot()) > 0.1 - { - println!("Endpoint is different! {:?} | {q0:?} | {q1:?} | {endpoint:?}", path.path_type); - error += 1; - } - } - - assert_eq!(error, 0) -} - -#[test] -fn size_of_items() { - assert_eq!(size_of::(), 12); - assert_eq!(size_of::(), 32); - assert_eq!(size_of::(), 1); - assert_eq!(size_of::(), 1); - assert_eq!(size_of::(), 0); -}