diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index 69c27da..6aa7a0b 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -237,5 +238,37 @@ std::unique_ptr Rotation_Reverse(const Rotation& rotation) { return std::make_unique(rotation.Reverse()); } +std::unique_ptr SRange_new(rust::f64 start, rust::f64 end) { + return std::make_unique(start, end); +} + +std::unique_ptr SRange_GetIntersection(const SRange& s_range, const SRange& other_s_range, rust::f64 tolerance) { + const auto intersection = s_range.GetIntersection(other_s_range, tolerance); + if (intersection) { + return std::make_unique(*intersection); + } + return nullptr; +} + +std::unique_ptr LaneSRange_new(const rust::String& lane_id, const SRange& s_range) { + return std::make_unique(LaneId{std::string(lane_id)}, s_range); +} + +rust::String LaneSRange_lane_id(const LaneSRange& lane_s_range) { + return lane_s_range.lane_id().string(); +} + +std::unique_ptr LaneSRange_s_range(const LaneSRange& lane_s_range) { + return std::make_unique(lane_s_range.s_range()); +} + +std::unique_ptr LaneSRange_GetIntersection(const LaneSRange& lane_s_range, const LaneSRange& other_lane_s_range, rust::f64 tolerance) { + const auto intersection = lane_s_range.GetIntersection(other_lane_s_range, tolerance); + if (intersection) { + return std::make_unique(*intersection); + } + return nullptr; +} + } // namespace api } // namespace maliput diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index d4b5281..dfe6c86 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -178,6 +178,33 @@ pub mod ffi { type HBounds; fn min(self: &HBounds) -> f64; fn max(self: &HBounds) -> f64; + + // SRange bindings definitions + type SRange; + fn SRange_new(s0: f64, s1: f64) -> UniquePtr; + fn s0(self: &SRange) -> f64; + fn s1(self: &SRange) -> f64; + fn set_s0(self: Pin<&mut SRange>, s0: f64); + fn set_s1(self: Pin<&mut SRange>, s1: f64); + fn size(self: &SRange) -> f64; + fn WithS(self: &SRange) -> bool; + fn Intersects(self: &SRange, other: &SRange, tolerance: f64) -> bool; + fn Contains(self: &SRange, s_range: &SRange, tolerance: f64) -> bool; + fn SRange_GetIntersection(s_range: &SRange, other: &SRange, tolerance: f64) -> UniquePtr; + + // LaneSRange bindings definitions + type LaneSRange; + fn LaneSRange_new(lane_id: &String, s_range: &SRange) -> UniquePtr; + fn length(self: &LaneSRange) -> f64; + fn Intersects(self: &LaneSRange, other: &LaneSRange, tolerance: f64) -> bool; + fn Contains(self: &LaneSRange, lane_s_range: &LaneSRange, tolerance: f64) -> bool; + fn LaneSRange_lane_id(lane_s_range: &LaneSRange) -> String; + fn LaneSRange_s_range(lane_s_range: &LaneSRange) -> UniquePtr; + fn LaneSRange_GetIntersection( + lane_s_range: &LaneSRange, + other: &LaneSRange, + tolerance: f64, + ) -> UniquePtr; } impl UniquePtr {} impl UniquePtr {} diff --git a/maliput-sys/tests/api_tests.rs b/maliput-sys/tests/api_tests.rs index c2582e9..6776c76 100644 --- a/maliput-sys/tests/api_tests.rs +++ b/maliput-sys/tests/api_tests.rs @@ -293,4 +293,64 @@ mod api_test { assert!((reversed_rotation.yaw() - PI).abs() < tol); } } + + mod s_range_test { + use maliput_sys::api::ffi::SRange_GetIntersection; + use maliput_sys::api::ffi::SRange_new; + + #[test] + fn s_range_api() { + let mut s_range = SRange_new(1.0, 11.0); + assert_eq!(s_range.s0(), 1.0); + assert_eq!(s_range.s1(), 11.0); + assert_eq!(s_range.size(), 10.0); + s_range.as_mut().expect("").set_s0(2.0); + s_range.as_mut().expect("").set_s1(12.0); + assert_eq!(s_range.s0(), 2.0); + assert_eq!(s_range.s1(), 12.0); + assert!(s_range.WithS()); + + let s_range_2 = SRange_new(5.0, 20.0); + assert!(s_range.Intersects(&s_range_2, 1e-3)); + assert!(!s_range.Contains(&s_range_2, 1e-3)); + + let intersection = SRange_GetIntersection(&s_range, &s_range_2, 1e-3); + assert!(!intersection.is_null()); + assert_eq!(intersection.s0(), 5.0); + assert_eq!(intersection.s1(), 12.0); + + let non_intersected_s_range = SRange_new(150.0, 200.0); + assert!(!s_range.Intersects(&non_intersected_s_range, 1e-3)); + let intersection = SRange_GetIntersection(&s_range, &non_intersected_s_range, 1e-3); + assert!(intersection.is_null()); + } + } + + mod lane_s_range_test { + use maliput_sys::api::ffi::LaneSRange_GetIntersection; + use maliput_sys::api::ffi::LaneSRange_lane_id; + use maliput_sys::api::ffi::LaneSRange_new; + use maliput_sys::api::ffi::LaneSRange_s_range; + use maliput_sys::api::ffi::SRange_new; + + #[test] + fn lane_s_range_api() { + let expected_s_range = SRange_new(1.0, 2.0); + let expected_lane_id = String::from("0_0_1"); + let lane_s_range = LaneSRange_new(&expected_lane_id, &expected_s_range); + assert_eq!(LaneSRange_lane_id(&lane_s_range), expected_lane_id); + assert_eq!(lane_s_range.length(), 1.0); + let s_range = LaneSRange_s_range(&lane_s_range); + assert_eq!(s_range.s0(), expected_s_range.s0()); + assert_eq!(s_range.s1(), expected_s_range.s1()); + let lane_s_range_2 = LaneSRange_new(&expected_lane_id, &SRange_new(1.5, 2.5)); + assert!(lane_s_range.Intersects(&lane_s_range_2, 1e-3)); + assert!(!lane_s_range.Contains(&lane_s_range_2, 1e-3)); + let intersection = LaneSRange_GetIntersection(&lane_s_range, &lane_s_range_2, 1e-3); + assert!(!intersection.is_null()); + let s_range = LaneSRange_s_range(&intersection); + assert_eq!(s_range.s0(), 1.5); + assert_eq!(s_range.s1(), 2.0); + } + } } diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 0087d03..ee0a605 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -866,6 +866,113 @@ impl Rotation { } } +/// Directed, inclusive longitudinal (s value) range from s0 to s1. +/// Wrapper around C++ implementation `maliput::api::SRange`. +pub struct SRange { + s_range: cxx::UniquePtr, +} + +impl SRange { + /// Create a new `SRange` with the given `s0` and `s1`. + pub fn new(s0: f64, s1: f64) -> SRange { + SRange { + s_range: maliput_sys::api::ffi::SRange_new(s0, s1), + } + } + /// Get the s0 of the `SRange`. + pub fn s0(&self) -> f64 { + self.s_range.s0() + } + /// Get the s1 of the `SRange`. + pub fn s1(&self) -> f64 { + self.s_range.s1() + } + /// Set the s0 of the `SRange`. + pub fn set_s0(&mut self, s0: f64) { + self.s_range.as_mut().expect("Underlying SRange is null").set_s0(s0); + } + /// Set the s1 of the `SRange`. + pub fn set_s1(&mut self, s1: f64) { + self.s_range.as_mut().expect("Underlying SRange is null").set_s1(s1); + } + /// Get the size of the `SRange`. + pub fn size(&self) -> f64 { + self.s_range.size() + } + /// Returns true When this SRange is in the direction of +s. + pub fn with_s(&self) -> bool { + self.s_range.WithS() + } + /// Determines whether this SRange intersects with `s_range`. + pub fn intersects(&self, s_range: &SRange, tolerance: f64) -> bool { + self.s_range.Intersects(&s_range.s_range, tolerance) + } + /// Determines whether this SRange contains `s_range`. + pub fn contains(&self, s_range: &SRange, tolerance: f64) -> bool { + self.s_range.Contains(&s_range.s_range, tolerance) + } + /// Get the intersection of this SRange with `s_range`. + /// Returns None if the intersection is empty. + pub fn get_intersection(&self, s_range: &SRange, tolerance: f64) -> Option { + let intersection = maliput_sys::api::ffi::SRange_GetIntersection(&self.s_range, &s_range.s_range, tolerance); + match intersection.is_null() { + true => None, + false => Some(SRange { s_range: intersection }), + } + } +} + +/// Directed longitudinal range of a specific Lane, identified by a LaneId. +/// Wrapper around C++ implementation `maliput::api::LaneSRange`. +pub struct LaneSRange { + lane_s_range: cxx::UniquePtr, +} + +impl LaneSRange { + /// Create a new `LaneSRange` with the given `lane_id` and `s_range`. + pub fn new(lane_id: &String, s_range: &SRange) -> LaneSRange { + LaneSRange { + lane_s_range: maliput_sys::api::ffi::LaneSRange_new(lane_id, &s_range.s_range), + } + } + /// Get the lane id of the `LaneSRange`. + pub fn lane_id(&self) -> String { + maliput_sys::api::ffi::LaneSRange_lane_id(&self.lane_s_range) + } + /// Get the s range of the `LaneSRange`. + pub fn s_range(&self) -> SRange { + SRange { + s_range: maliput_sys::api::ffi::LaneSRange_s_range(&self.lane_s_range), + } + } + /// Get the length of the `LaneSRange`. + pub fn length(&self) -> f64 { + self.lane_s_range.length() + } + /// Determines whether this LaneSRange intersects with `lane_s_range`. + pub fn intersects(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool { + self.lane_s_range.Intersects(&lane_s_range.lane_s_range, tolerance) + } + /// Determines whether this LaneSRange contains `lane_s_range`. + pub fn contains(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool { + self.lane_s_range.Contains(&lane_s_range.lane_s_range, tolerance) + } + /// Get the intersection of this LaneSRange with `lane_s_range`. + pub fn get_intersection(&self, lane_s_range: &LaneSRange, tolerance: f64) -> Option { + let intersection = maliput_sys::api::ffi::LaneSRange_GetIntersection( + &self.lane_s_range, + &lane_s_range.lane_s_range, + tolerance, + ); + match intersection.is_null() { + true => None, + false => Some(LaneSRange { + lane_s_range: intersection, + }), + } + } +} + mod tests { mod lane_position { #[test] @@ -1069,4 +1176,90 @@ mod tests { assert_eq!(matrix.row(2), crate::math::Vector3::new(0.0, 0.0, 1.0)); } } + + mod s_range { + #[test] + fn s_range_new() { + let s_range = crate::api::SRange::new(1.0, 2.0); + assert_eq!(s_range.s0(), 1.0); + assert_eq!(s_range.s1(), 2.0); + } + #[test] + fn s_range_api() { + let s_range_1 = crate::api::SRange::new(1.0, 3.0); + let s_range_2 = crate::api::SRange::new(2.0, 4.0); + assert_eq!(s_range_1.size(), 2.0); + assert!(s_range_1.with_s()); + assert!(s_range_1.intersects(&s_range_2, 0.0)); + assert!(!s_range_1.contains(&s_range_2, 0.0)); + } + #[test] + fn s_range_setters() { + let mut s_range = crate::api::SRange::new(0.0, 4.0); + s_range.set_s0(1.0); + s_range.set_s1(3.0); + assert_eq!(s_range.s0(), 1.0); + assert_eq!(s_range.s1(), 3.0); + } + #[test] + fn s_range_get_intersection_with_intersection() { + let s_range_1 = crate::api::SRange::new(1.0, 3.0); + let s_range_2 = crate::api::SRange::new(2.0, 4.0); + let intersection = s_range_1.get_intersection(&s_range_2, 0.0); + assert!(intersection.is_some()); + let intersection = intersection.unwrap(); + assert_eq!(intersection.s0(), 2.0); + assert_eq!(intersection.s1(), 3.0); + } + #[test] + fn s_range_get_intersection_with_no_intersection() { + let s_range_1 = crate::api::SRange::new(1.0, 2.0); + let s_range_2 = crate::api::SRange::new(3.0, 4.0); + let intersection = s_range_1.get_intersection(&s_range_2, 0.0); + assert!(intersection.is_none()); + } + } + + mod lane_s_range { + #[test] + fn lane_s_range_new() { + let lane_s_range = + crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0)); + assert_eq!(lane_s_range.lane_id(), "lane_test"); + assert_eq!(lane_s_range.s_range().s0(), 1.0); + assert_eq!(lane_s_range.s_range().s1(), 2.0); + assert_eq!(lane_s_range.length(), 1.0); + } + #[test] + fn lane_s_range_api() { + let lane_s_range_1 = + crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0)); + let lane_s_range_2 = + crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 3.0)); + assert!(lane_s_range_1.intersects(&lane_s_range_2, 0.0)); + assert!(!lane_s_range_1.contains(&lane_s_range_2, 0.0)); + } + #[test] + fn lane_s_range_get_intersection_with_intersection() { + let lane_s_range_1 = + crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 3.0)); + let lane_s_range_2 = + crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 4.0)); + let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0); + assert!(intersection.is_some()); + let intersection = intersection.unwrap(); + assert_eq!(intersection.lane_id(), "lane_test"); + assert_eq!(intersection.s_range().s0(), 2.0); + assert_eq!(intersection.s_range().s1(), 3.0); + } + #[test] + fn lane_s_range_get_intersection_with_no_intersection() { + let lane_s_range_1 = + crate::api::LaneSRange::new(&String::from("lane test_1"), &crate::api::SRange::new(1.0, 3.0)); + let lane_s_range_2 = + crate::api::LaneSRange::new(&String::from("lane_test_2"), &crate::api::SRange::new(2.0, 4.0)); + let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0); + assert!(intersection.is_none()); + } + } }