maliput/api/
mod.rs

1// BSD 3-Clause License
2//
3// Copyright (c) 2024, Woven by Toyota.
4// All rights reserved.
5//
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are met:
8//
9// * Redistributions of source code must retain the above copyright notice, this
10//   list of conditions and the following disclaimer.
11//
12// * Redistributions in binary form must reproduce the above copyright notice,
13//   this list of conditions and the following disclaimer in the documentation
14//   and/or other materials provided with the distribution.
15//
16// * Neither the name of the copyright holder nor the names of its
17//   contributors may be used to endorse or promote products derived from
18//   this software without specific prior written permission.
19//
20// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30
31use crate::common::MaliputError;
32use crate::math::Matrix3;
33use crate::math::Quaternion;
34use crate::math::RollPitchYaw;
35use crate::math::Vector3;
36
37pub mod rules;
38
39/// Represents a complete Maliput road network.
40///
41/// A `RoadNetwork` is the main entry point for interacting with a road map in Maliput.
42/// It serves as a container for all the elements that describe a road network,
43/// including its physical layout and the rules of the road.
44///
45/// It provides access to the following key components:
46///
47/// * [`RoadGeometry`]: The geometric description of the road surfaces.
48/// * [`rules::RoadRulebook`]: The set of traffic rules, like speed limits and right-of-way.
49/// * [`rules::TrafficLightBook`]: A catalog of all traffic lights in the network.
50/// * [`IntersectionBook`]: A collection of logical intersections and their states.
51///   TODO: Complete with other books when available (e.g., `RuleRegistry / PhaseRingBook / etc)
52///
53/// More info can be found at https://maliput.readthedocs.io/en/latest/html/deps/maliput/html/maliput_design.html.
54///
55/// # Example
56///
57/// ```rust, no_run
58/// use maliput::api::RoadNetwork;
59/// use std::collections::HashMap;
60///
61/// // Properties to load an OpenDRIVE file using the "maliput_malidrive" backend.
62/// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
63/// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
64/// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
65///
66/// // Create the RoadNetwork by specifying the loader ID and properties.
67/// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
68/// ```
69pub struct RoadNetwork {
70    pub(crate) rn: cxx::UniquePtr<maliput_sys::api::ffi::RoadNetwork>,
71}
72
73impl RoadNetwork {
74    /// Create a new `RoadNetwork` with the given `road_network_loader_id` and `properties`.
75    ///
76    /// # Arguments
77    ///
78    /// * `road_network_loader_id` - The id of the road network loader. It identifies the backend to be used (e.g., "maliput_malidrive").
79    /// * `properties` - The properties of the road network.
80    ///
81    /// # Details
82    /// It relies on `maliput_sys::plugin::ffi::CreateRoadNetwork` to create a new `RoadNetwork`.
83    ///
84    /// # Returns
85    /// A result containing the `RoadNetwork` or a `MaliputError` if the creation fails.
86    pub fn new(
87        road_network_loader_id: &str,
88        properties: &std::collections::HashMap<&str, &str>,
89    ) -> Result<RoadNetwork, MaliputError> {
90        // Translate the properties to ffi types
91        let mut properties_vec = Vec::new();
92        for (key, value) in properties.iter() {
93            properties_vec.push(format!("{}:{}", key, value));
94        }
95        // Append the maliput_malidrive plugin path to MALIPUT_PLUGIN_PATH.
96        // If MALIPUT_PLUGIN_PATH is not set, it will be created.
97        let malidrive_plugin_path = maliput_sdk::get_maliput_malidrive_plugin_path();
98        let new_path = match std::env::var_os("MALIPUT_PLUGIN_PATH") {
99            Some(current_path) => {
100                let mut paths = std::env::split_paths(&current_path).collect::<Vec<_>>();
101                paths.push(malidrive_plugin_path);
102                std::env::join_paths(paths).unwrap()
103            }
104            None => maliput_sdk::get_maliput_malidrive_plugin_path().into(),
105        };
106        std::env::set_var("MALIPUT_PLUGIN_PATH", new_path);
107        let rn = maliput_sys::plugin::ffi::CreateRoadNetwork(&road_network_loader_id.to_string(), &properties_vec)?;
108        Ok(RoadNetwork { rn })
109    }
110
111    /// Get the `RoadGeometry` of the `RoadNetwork`.
112    pub fn road_geometry(&self) -> RoadGeometry {
113        unsafe {
114            RoadGeometry {
115                rg: self.rn.road_geometry().as_ref().expect(""),
116            }
117        }
118    }
119    /// Get the `IntersectionBook` of the `RoadNetwork`.
120    pub fn intersection_book(&mut self) -> IntersectionBook {
121        let intersection_book_ffi = self
122            .rn
123            .as_mut()
124            .expect("Underlying RoadNetwork is null")
125            .intersection_book();
126        IntersectionBook {
127            intersection_book: unsafe {
128                intersection_book_ffi
129                    .as_mut()
130                    .expect("Underlying IntersectionBook is null")
131            },
132        }
133    }
134    /// Get the `TrafficLightBook` of the `RoadNetwork`.
135    pub fn traffic_light_book(&self) -> rules::TrafficLightBook {
136        let traffic_light_book_ffi = self.rn.traffic_light_book();
137        rules::TrafficLightBook {
138            traffic_light_book: unsafe {
139                traffic_light_book_ffi
140                    .as_ref()
141                    .expect("Underlying TrafficLightBook is null")
142            },
143        }
144    }
145    /// Get the `RoadRulebook` of the `RoadNetwork`.
146    pub fn rulebook(&self) -> rules::RoadRulebook {
147        let rulebook_ffi = self.rn.rulebook();
148        rules::RoadRulebook {
149            road_rulebook: unsafe { rulebook_ffi.as_ref().expect("Underlying RoadRulebook is null") },
150        }
151    }
152}
153
154/// Represents the geometry of a road network.
155///
156/// `RoadGeometry` is the top-level container for the road network's geometric
157/// description. It is composed of a set of `Junction`s, which in turn contain
158/// `Segment`s and `Lane`s.
159///
160/// It provides access to the entire road network's geometric structure,
161/// allowing for queries about its components (e.g., finding a `Lane` by its ID)
162/// and for coordinate conversions between the inertial frame and the road network's
163/// intrinsic coordinate systems (lane coordinates).
164///
165/// An instance of `RoadGeometry` is typically obtained from a `RoadNetwork`.
166///
167/// More info can be found at https://maliput.readthedocs.io/en/latest/html/deps/maliput/html/maliput_design.html.
168///
169/// # Example of obtaining a `RoadGeometry`
170///
171/// ```rust, no_run
172/// use maliput::api::RoadNetwork;
173/// use std::collections::HashMap;
174///
175/// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
176/// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
177/// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
178/// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
179/// let road_geometry = road_network.road_geometry();
180/// println!("RoadGeometry ID: {}", road_geometry.id());
181/// ```
182pub struct RoadGeometry<'a> {
183    rg: &'a maliput_sys::api::ffi::RoadGeometry,
184}
185
186impl<'a> RoadGeometry<'a> {
187    /// Returns the id of the RoadGeometry.
188    pub fn id(&self) -> String {
189        maliput_sys::api::ffi::RoadGeometry_id(self.rg)
190    }
191    /// Returns the number of Junctions in the RoadGeometry.
192    ///
193    /// Return value is non-negative.
194    pub fn num_junctions(&self) -> i32 {
195        self.rg.num_junctions()
196    }
197    /// Returns the tolerance guaranteed for linear measurements (positions).
198    pub fn linear_tolerance(&self) -> f64 {
199        self.rg.linear_tolerance()
200    }
201    /// Returns the tolerance guaranteed for angular measurements (orientations).
202    pub fn angular_tolerance(&self) -> f64 {
203        self.rg.angular_tolerance()
204    }
205    /// Returns the number of BranchPoints in the RoadGeometry.
206    ///
207    /// Return value is non-negative.
208    pub fn num_branch_points(&self) -> i32 {
209        self.rg.num_branch_points()
210    }
211    /// Determines the [RoadPosition] corresponding to [InertialPosition] `inertial_position`.
212    ///
213    /// Returns a RoadPositionResult. Its [RoadPosition] is the point in the
214    /// [RoadGeometry]'s manifold which is, in the `Inertial`-frame, closest to
215    /// `inertial_position`. Its InertialPosition is the `Inertial`-frame equivalent of the
216    /// [RoadPosition] and its distance is the Cartesian distance from
217    /// `inertial_position` to the nearest point.
218    ///
219    /// This method guarantees that its result satisfies the condition that
220    /// `result.lane.to_lane_position(result.pos)` is within `linear_tolerance()`
221    /// of the returned [InertialPosition].
222    ///
223    /// The map from [RoadGeometry] to the `Inertial`-frame is not onto (as a bounded
224    /// [RoadGeometry] cannot completely cover the unbounded Cartesian universe).
225    /// If `inertial_position` does represent a point contained within the volume
226    /// of the RoadGeometry, then result distance is guaranteed to be less
227    /// than or equal to `linear_tolerance()`.
228    ///
229    /// The map from [RoadGeometry] to `Inertial`-frame is not necessarily one-to-one.
230    /// Different `(s,r,h)` coordinates from different Lanes, potentially from
231    /// different Segments, may map to the same `(x,y,z)` `Inertial`-frame location.
232    ///
233    /// If `inertial_position` is contained within the volumes of multiple Segments,
234    /// then to_road_position() will choose a [Segment] which yields the minimum
235    /// height `h` value in the result.  If the chosen [Segment] has multiple
236    /// Lanes, then to_road_position() will choose a [Lane] which contains
237    /// `inertial_position` within its `lane_bounds()` if possible, and if that is
238    /// still ambiguous, it will further select a [Lane] which minimizes the
239    /// absolute value of the lateral `r` coordinate in the result.
240    ///
241    /// # Arguments
242    /// * `inertial_position` - The [InertialPosition] to convert into a [RoadPosition].
243    ///
244    /// # Return
245    /// A [RoadPositionResult] with the nearest [RoadPosition], the corresponding [InertialPosition]
246    /// to that [RoadPosition] and the distance between the input and output [InertialPosition]s.
247    pub fn to_road_position(&self, inertial_position: &InertialPosition) -> Result<RoadPositionResult, MaliputError> {
248        let rpr = maliput_sys::api::ffi::RoadGeometry_ToRoadPosition(self.rg, &inertial_position.ip)?;
249        Ok(RoadPositionResult {
250            road_position: RoadPosition {
251                rp: maliput_sys::api::ffi::RoadPositionResult_road_position(&rpr),
252            },
253            nearest_position: InertialPosition {
254                ip: maliput_sys::api::ffi::RoadPositionResult_nearest_position(&rpr),
255            },
256            distance: maliput_sys::api::ffi::RoadPositionResult_distance(&rpr),
257        })
258    }
259
260    /// TODO(#197): Implement find_road_positions
261    /// Obtains all [RoadPosition]s within a radius of the inertial_position.
262    ///
263    /// Only Lanes whose segment regions include points that are within radius of
264    /// inertial position are included in the search. For each of these Lanes,
265    /// include the [RoadPosition] or [RoadPosition]s with the minimum distance to
266    /// inertial position in the returned result.
267    ///
268    /// # Arguments
269    ///
270    /// * `inertial_position` - The [InertialPosition] to search around.
271    /// * `radius` - The radius around the [InertialPosition] to search for [RoadPosition]s.
272    ///
273    /// # Return
274    ///
275    /// A vector of [RoadPositionResult]s.
276    pub fn find_road_positions(
277        &self,
278        _inertial_position: &[InertialPosition],
279        _radius: f64,
280    ) -> Result<Vec<RoadPositionResult>, MaliputError> {
281        Err(MaliputError::Other(
282            "find_road_positions is not implemented yet".to_string(),
283        ))
284    }
285
286    /// Get the lane matching given `lane_id`.
287    ///
288    /// # Arguments
289    /// * `lane_id` - The id of the lane.
290    ///
291    /// # Return
292    /// The lane with the given id.
293    /// If no lane is found with the given id, return None.
294    pub fn get_lane(&self, lane_id: &String) -> Option<Lane> {
295        let lane = maliput_sys::api::ffi::RoadGeometry_GetLane(self.rg, lane_id);
296        if lane.lane.is_null() {
297            return None;
298        }
299        Some(Lane {
300            lane: unsafe { lane.lane.as_ref().expect("") },
301        })
302    }
303    /// Get all lanes of the `RoadGeometry`.
304    /// Returns a vector of `Lane`.
305    /// # Example
306    /// ```rust, no_run
307    /// use maliput::api::RoadNetwork;
308    /// use std::collections::HashMap;
309    ///
310    /// let package_location = std::env::var("CARGO_MANIFEST_DIR").unwrap();
311    /// let xodr_path = format!("{}/data/xodr/TShapeRoad.xodr", package_location);
312    /// let road_network_properties = HashMap::from([("road_geometry_id", "my_rg_from_rust"), ("opendrive_file", xodr_path.as_str())]);
313    /// let road_network = RoadNetwork::new("maliput_malidrive", &road_network_properties).unwrap();
314    /// let road_geometry = road_network.road_geometry();
315    /// let lanes = road_geometry.get_lanes();
316    /// for lane in lanes {
317    ///    println!("lane_id: {}", lane.id());
318    /// }
319    /// ```
320    pub fn get_lanes(&self) -> Vec<Lane> {
321        let lanes = maliput_sys::api::ffi::RoadGeometry_GetLanes(self.rg);
322        lanes
323            .into_iter()
324            .map(|l| Lane {
325                lane: unsafe { l.lane.as_ref().expect("") },
326            })
327            .collect::<Vec<Lane>>()
328    }
329    /// Get the segment matching given `segment_id`.
330    ///
331    /// # Arguments
332    /// * `segment_id` - The id of the segment.
333    ///
334    /// # Return
335    /// The segment with the given id.
336    /// If no segment is found with the given id, return None.
337    pub fn get_segment(&self, segment_id: &String) -> Option<Segment> {
338        let segment = maliput_sys::api::ffi::RoadGeometry_GetSegment(self.rg, segment_id);
339        if segment.is_null() {
340            return None;
341        }
342        unsafe {
343            Some(Segment {
344                segment: segment.as_ref().expect(""),
345            })
346        }
347    }
348    /// Get the junction matching given `junction_id`.
349    ///
350    /// # Arguments
351    /// * `junction_id` - The id of the junction.
352    ///
353    /// # Return
354    /// The junction with the given id.
355    /// If no junction is found with the given id, return None.
356    pub fn get_junction(&self, junction_id: &String) -> Option<Junction> {
357        let junction = maliput_sys::api::ffi::RoadGeometry_GetJunction(self.rg, junction_id);
358        if junction.is_null() {
359            return None;
360        }
361        unsafe {
362            Some(Junction {
363                junction: junction.as_ref().expect(""),
364            })
365        }
366    }
367    /// Get the branch point matching given `branch_point_id`.
368    ///
369    /// # Arguments
370    /// * `branch_point_id` - The id of the branch point.
371    ///
372    /// # Return
373    /// The branch point with the given id.
374    /// If no branch point is found with the given id, return None.
375    pub fn get_branch_point(&self, branch_point_id: &String) -> Option<BranchPoint> {
376        let branch_point = maliput_sys::api::ffi::RoadGeometry_GetBranchPoint(self.rg, branch_point_id);
377        if branch_point.is_null() {
378            return None;
379        }
380        unsafe {
381            Some(BranchPoint {
382                branch_point: branch_point.as_ref().expect(""),
383            })
384        }
385    }
386    /// Execute a custom command on the backend.
387    ///
388    /// # Details
389    /// The documentation of the custom command should be provided by the backend: https://github.com/maliput/maliput_malidrive/blob/main/src/maliput_malidrive/base/road_geometry.h
390    ///
391    /// # Arguments
392    /// * `command` - The command to execute.
393    ///
394    /// # Return
395    /// The result of the command.
396    // pub fn backend_custom_command(&self, command: &String) -> String {
397    pub fn backend_custom_command(&self, command: &String) -> Result<String, MaliputError> {
398        Ok(maliput_sys::api::ffi::RoadGeometry_BackendCustomCommand(
399            self.rg, command,
400        )?)
401    }
402    /// Obtains the Geo Reference info of this RoadGeometry.
403    ///
404    /// # Return
405    /// A string containing the Geo Reference projection, if any.
406    pub fn geo_reference_info(&self) -> String {
407        maliput_sys::api::ffi::RoadGeometry_GeoReferenceInfo(self.rg)
408    }
409}
410
411/// A 3-dimensional position in a `Lane`-frame, consisting of three components:
412///
413/// * s is longitudinal position, as arc-length along a Lane's reference line.
414/// * r is lateral position, perpendicular to the reference line at s. +r is to
415///   to the left when traveling in the direction of +s.
416/// * h is height above the road surface.
417///
418/// # Example
419///
420/// ```rust, no_run
421/// use maliput::api::LanePosition;
422///
423/// let lane_pos = LanePosition::new(1.0, 2.0, 3.0);
424/// println!("lane_pos = {}", lane_pos);
425/// assert_eq!(lane_pos.s(), 1.0);
426/// assert_eq!(lane_pos.r(), 2.0);
427/// assert_eq!(lane_pos.h(), 3.0);
428/// ```
429pub struct LanePosition {
430    lp: cxx::UniquePtr<maliput_sys::api::ffi::LanePosition>,
431}
432
433impl LanePosition {
434    /// Create a new `LanePosition` with the given `s`, `r`, and `h` components.
435    pub fn new(s: f64, r: f64, h: f64) -> LanePosition {
436        LanePosition {
437            lp: maliput_sys::api::ffi::LanePosition_new(s, r, h),
438        }
439    }
440    /// Get the `s` component of the `LanePosition`.
441    pub fn s(&self) -> f64 {
442        self.lp.s()
443    }
444    /// Get the `r` component of the `LanePosition`.
445    pub fn r(&self) -> f64 {
446        self.lp.r()
447    }
448    /// Get the `h` component of the `LanePosition`.
449    pub fn h(&self) -> f64 {
450        self.lp.h()
451    }
452
453    /// Returns all components as 3-vector `[s, r, h]`.
454    pub fn srh(&self) -> Vector3 {
455        let srh = self.lp.srh();
456        Vector3::new(srh.x(), srh.y(), srh.z())
457    }
458
459    /// Set the `s` component of the `LanePosition`.
460    pub fn set_s(&mut self, s: f64) {
461        self.lp.as_mut().expect("Underlying LanePosition is null").set_s(s);
462    }
463
464    /// Set the `r` component of the `LanePosition`.
465    pub fn set_r(&mut self, r: f64) {
466        self.lp.as_mut().expect("Underlying LanePosition is null").set_r(r);
467    }
468
469    /// Set the `h` component of the `LanePosition`.
470    pub fn set_h(&mut self, h: f64) {
471        self.lp.as_mut().expect("Underlying LanePosition is null").set_h(h);
472    }
473
474    /// Set all components from 3-vector `[s, r, h]`.
475    pub fn set_srh(&mut self, srh: &Vector3) {
476        let ffi_vec = maliput_sys::math::ffi::Vector3_new(srh.x(), srh.y(), srh.z());
477        self.lp
478            .as_mut()
479            .expect("Underlying LanePosition is null")
480            .set_srh(&ffi_vec);
481    }
482}
483
484impl PartialEq for LanePosition {
485    fn eq(&self, other: &Self) -> bool {
486        self.srh() == other.srh()
487    }
488}
489
490impl Eq for LanePosition {}
491
492impl std::fmt::Display for LanePosition {
493    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
494        write!(f, "{}", maliput_sys::api::ffi::LanePosition_to_str(&self.lp))
495    }
496}
497
498impl std::fmt::Debug for LanePosition {
499    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
500        f.debug_struct("LanePosition")
501            .field("s", &self.s())
502            .field("r", &self.r())
503            .field("h", &self.h())
504            .finish()
505    }
506}
507
508/// A position in 3-dimensional geographical Cartesian space, i.e., in the
509/// `Inertial`-frame, consisting of three components x, y, and z.
510///
511/// # Example
512///
513/// ```rust, no_run
514/// use maliput::api::InertialPosition;
515///
516/// let inertial_pos = InertialPosition::new(1.0, 2.0, 3.0);
517/// println!("inertial_pos = {}", inertial_pos);
518/// assert_eq!(inertial_pos.x(), 1.0);
519/// assert_eq!(inertial_pos.y(), 2.0);
520/// assert_eq!(inertial_pos.z(), 3.0);
521/// ```
522pub struct InertialPosition {
523    ip: cxx::UniquePtr<maliput_sys::api::ffi::InertialPosition>,
524}
525
526impl InertialPosition {
527    /// Create a new `InertialPosition` with the given `x`, `y`, and `z` components.
528    pub fn new(x: f64, y: f64, z: f64) -> InertialPosition {
529        InertialPosition {
530            ip: maliput_sys::api::ffi::InertialPosition_new(x, y, z),
531        }
532    }
533    /// Get the `x` component of the `InertialPosition`.
534    pub fn x(&self) -> f64 {
535        self.ip.x()
536    }
537    /// Get the `y` component of the `InertialPosition`.
538    pub fn y(&self) -> f64 {
539        self.ip.y()
540    }
541    /// Get the `z` component of the `InertialPosition`.
542    pub fn z(&self) -> f64 {
543        self.ip.z()
544    }
545
546    /// Returns all components as 3-vector `[x, y, z]`.
547    pub fn xyz(&self) -> Vector3 {
548        let xyz = self.ip.xyz();
549        Vector3::new(xyz.x(), xyz.y(), xyz.z())
550    }
551
552    /// Set the `x` component of the `InertialPosition`.
553    pub fn set_x(&mut self, x: f64) {
554        self.ip.as_mut().expect("Underlying InertialPosition is null").set_x(x);
555    }
556
557    /// Set the `y` component of the `InertialPosition`.
558    pub fn set_y(&mut self, y: f64) {
559        self.ip.as_mut().expect("Underlying InertialPosition is null").set_y(y);
560    }
561
562    /// Set the `z` component of the `InertialPosition`.
563    pub fn set_z(&mut self, z: f64) {
564        self.ip.as_mut().expect("Underlying InertialPosition is null").set_z(z);
565    }
566
567    /// Set all components from 3-vector `[x, y, z]`.
568    pub fn set_xyz(&mut self, xyz: &Vector3) {
569        let ffi_vec = maliput_sys::math::ffi::Vector3_new(xyz.x(), xyz.y(), xyz.z());
570        self.ip
571            .as_mut()
572            .expect("Underlying InertialPosition is null")
573            .set_xyz(&ffi_vec);
574    }
575
576    /// Get the length of `InertialPosition`.
577    pub fn length(&self) -> f64 {
578        self.ip.length()
579    }
580
581    /// Get the distance between two `InertialPosition`.
582    pub fn distance(&self, other: &InertialPosition) -> f64 {
583        self.ip.Distance(&other.ip)
584    }
585}
586
587impl PartialEq for InertialPosition {
588    fn eq(&self, other: &Self) -> bool {
589        maliput_sys::api::ffi::InertialPosition_operator_eq(&self.ip, &other.ip)
590    }
591}
592
593impl Eq for InertialPosition {}
594
595impl std::fmt::Display for InertialPosition {
596    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
597        write!(f, "{}", maliput_sys::api::ffi::InertialPosition_to_str(&self.ip))
598    }
599}
600
601impl std::fmt::Debug for InertialPosition {
602    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
603        f.debug_struct("InertialPosition")
604            .field("x", &self.x())
605            .field("y", &self.y())
606            .field("z", &self.z())
607            .finish()
608    }
609}
610
611impl std::ops::Add for InertialPosition {
612    type Output = InertialPosition;
613
614    fn add(self, other: InertialPosition) -> InertialPosition {
615        InertialPosition {
616            ip: maliput_sys::api::ffi::InertialPosition_operator_sum(&self.ip, &other.ip),
617        }
618    }
619}
620
621impl std::ops::Sub for InertialPosition {
622    type Output = InertialPosition;
623
624    fn sub(self, other: InertialPosition) -> InertialPosition {
625        InertialPosition {
626            ip: maliput_sys::api::ffi::InertialPosition_operator_sub(&self.ip, &other.ip),
627        }
628    }
629}
630
631impl std::ops::Mul<f64> for InertialPosition {
632    type Output = InertialPosition;
633
634    fn mul(self, scalar: f64) -> InertialPosition {
635        InertialPosition {
636            ip: maliput_sys::api::ffi::InertialPosition_operator_mul_scalar(&self.ip, scalar),
637        }
638    }
639}
640
641/// Bounds in the lateral dimension (r component) of a `Lane`-frame, consisting
642/// of a pair of minimum and maximum r value.  The bounds must straddle r = 0,
643/// i.e., the minimum must be <= 0 and the maximum must be >= 0.
644pub struct RBounds {
645    min: f64,
646    max: f64,
647}
648
649impl RBounds {
650    /// Create a new `RBounds` with the given `min` and `max` values.
651    pub fn new(min: f64, max: f64) -> RBounds {
652        RBounds { min, max }
653    }
654    /// Get the `min` value of the `RBounds`.
655    pub fn min(&self) -> f64 {
656        self.min
657    }
658    /// Get the `max` value of the `RBounds`.
659    pub fn max(&self) -> f64 {
660        self.max
661    }
662    /// Set the `min` value of the `RBounds`.
663    pub fn set_min(&mut self, min: f64) {
664        self.min = min;
665    }
666    /// Set the `max` value of the `RBounds`.
667    pub fn set_max(&mut self, max: f64) {
668        self.max = max;
669    }
670}
671
672/// Bounds in the elevation dimension (`h` component) of a `Lane`-frame,
673/// consisting of a pair of minimum and maximum `h` value.  The bounds
674/// must straddle `h = 0`, i.e., the minimum must be `<= 0` and the
675/// maximum must be `>= 0`.
676pub struct HBounds {
677    min: f64,
678    max: f64,
679}
680
681impl HBounds {
682    /// Create a new `HBounds` with the given `min` and `max` values.
683    pub fn new(min: f64, max: f64) -> HBounds {
684        HBounds { min, max }
685    }
686    /// Get the `min` value of the `HBounds`.
687    pub fn min(&self) -> f64 {
688        self.min
689    }
690    /// Get the `max` value of the `HBounds`.
691    pub fn max(&self) -> f64 {
692        self.max
693    }
694    /// Set the `min` value of the `HBounds`.
695    pub fn set_min(&mut self, min: f64) {
696        self.min = min;
697    }
698    /// Set the `max` value of the `HBounds`.
699    pub fn set_max(&mut self, max: f64) {
700        self.max = max;
701    }
702}
703
704/// Isometric velocity vector in a `Lane`-frame.
705///
706/// sigma_v, rho_v, and eta_v are the components of velocity in a
707/// (sigma, rho, eta) coordinate system.  (sigma, rho, eta) have the same
708/// orientation as the (s, r, h) at any given point in space, however they
709/// form an isometric system with a Cartesian distance metric.  Hence,
710/// IsoLaneVelocity represents a "real" physical velocity vector (albeit
711/// with an orientation relative to the road surface).
712#[derive(Default, Copy, Clone, Debug, PartialEq)]
713pub struct IsoLaneVelocity {
714    pub sigma_v: f64,
715    pub rho_v: f64,
716    pub eta_v: f64,
717}
718
719impl IsoLaneVelocity {
720    /// Create a new `IsoLaneVelocity` with the given `sigma_v`, `rho_v`, and `eta_v` components.
721    pub fn new(sigma_v: f64, rho_v: f64, eta_v: f64) -> IsoLaneVelocity {
722        IsoLaneVelocity { sigma_v, rho_v, eta_v }
723    }
724}
725
726/// A Lane represents a lane of travel in a road network.  A Lane defines
727/// a curvilinear coordinate system covering the road surface, with a
728/// longitudinal 's' coordinate that expresses the arc-length along a
729/// central reference curve.  The reference curve nominally represents
730/// an ideal travel trajectory along the Lane.
731///
732/// Lanes are grouped by Segment.  All Lanes belonging to a Segment
733/// represent the same road surface, but with different coordinate
734/// parameterizations (e.g., each Lane has its own reference curve).
735pub struct Lane<'a> {
736    lane: &'a maliput_sys::api::ffi::Lane,
737}
738
739impl<'a> Lane<'a> {
740    /// Returns the id of the Lane.
741    ///
742    /// The id is a unique identifier for the Lane within the RoadGeometry.
743    ///
744    /// # Returns
745    /// A `String` containing the id of the Lane.
746    pub fn id(&self) -> String {
747        maliput_sys::api::ffi::Lane_id(self.lane)
748    }
749    /// Returns the index of this Lane within the Segment which owns it.
750    pub fn index(&self) -> i32 {
751        self.lane.index()
752    }
753    /// Returns the Segment to which this Lane belongs.
754    ///
755    /// # Returns
756    /// A [`Segment`] containing the Segment to which this Lane belongs.
757    pub fn segment(&self) -> Segment<'a> {
758        unsafe {
759            Segment {
760                segment: self.lane.segment().as_ref().expect(""),
761            }
762        }
763    }
764    /// Returns the adjacent lane to the left, if one exists.
765    ///
766    /// "Left" is defined as the direction of `+r` in the `(s, r, h)` lane-coordinate frame.
767    ///
768    /// # Returns
769    ///
770    /// An [`Option<Lane>`] containing the left lane, or `None` if this is the
771    /// leftmost lane.
772    pub fn to_left(&self) -> Option<Lane> {
773        let lane = self.lane.to_left();
774        if lane.is_null() {
775            None
776        } else {
777            unsafe {
778                Some(Lane {
779                    lane: lane.as_ref().expect(""),
780                })
781            }
782        }
783    }
784    /// Returns the adjacent lane to the right, if one exists.
785    ///
786    /// "Right" is defined as the direction of `-r` in the `(s, r, h)` lane-coordinate
787    /// frame.
788    ///
789    /// # Returns
790    ///
791    /// An [`Option<Lane>`] containing the right lane, or `None` if this is the
792    /// rightmost lane.
793    pub fn to_right(&self) -> Option<Lane> {
794        let lane = self.lane.to_right();
795        if lane.is_null() {
796            None
797        } else {
798            unsafe {
799                Some(Lane {
800                    lane: lane.as_ref().expect(""),
801                })
802            }
803        }
804    }
805    /// Returns the arc-length of the Lane along its reference curve.
806    ///
807    /// The value of length() is also the maximum s-coordinate for this Lane;
808    /// i.e., the domain of s is [0, length()].
809    ///
810    /// # Returns
811    ///
812    /// The length of the Lane in meters.
813    pub fn length(&self) -> f64 {
814        self.lane.length()
815    }
816
817    /// Get the orientation of the `Lane` at the given `LanePosition`.
818    pub fn get_orientation(&self, lane_position: &LanePosition) -> Result<Rotation, MaliputError> {
819        Ok(Rotation {
820            r: maliput_sys::api::ffi::Lane_GetOrientation(self.lane, lane_position.lp.as_ref().expect(""))?,
821        })
822    }
823    /// # Brief
824    /// Get the [InertialPosition] of the [Lane] at the given [LanePosition].
825    ///
826    /// # Notes
827    /// Note there is no constraint for the `r` coordinate, as it can be outside the lane boundaries.
828    /// In that scenario, the resultant inertial position represents a point in the `s-r` plane at the given `s` and `h`
829    /// coordinates. It's on the user side to verify, if needed, that the lane position is within lane boundaries.
830    /// Bare in mind that the inertial position will be a point in the `s-r` plane, but *not* necessarily on the road surface.
831    ///
832    /// # Arguments
833    /// * `lane_position` - A maliput [LanePosition].
834    ///
835    /// # Precondition
836    /// The s component of `lane_position` must be in domain [0, Lane::length()].
837    ///
838    /// # Return
839    /// The [InertialPosition] corresponding to the input [LanePosition].
840    pub fn to_inertial_position(&self, lane_position: &LanePosition) -> Result<InertialPosition, MaliputError> {
841        Ok(InertialPosition {
842            ip: maliput_sys::api::ffi::Lane_ToInertialPosition(self.lane, lane_position.lp.as_ref().expect(""))?,
843        })
844    }
845    /// Determines the LanePosition corresponding to InertialPosition `inertial_position`.
846    /// The LanePosition is expected to be contained within the lane's boundaries.
847    /// See [Lane::to_segment_position] method.
848    ///
849    /// This method guarantees that its result satisfies the condition that
850    /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
851    ///  of `result.nearest_position`.
852    ///
853    /// # Arguments
854    /// * `inertial_position` - A [InertialPosition] to get a [LanePosition] from.
855    ///
856    /// # Return
857    /// A [LanePositionResult] with the closest [LanePosition], the corresponding [InertialPosition] to that [LanePosition]
858    /// and the distance between the input and output [InertialPosition]s.
859    pub fn to_lane_position(&self, inertial_position: &InertialPosition) -> Result<LanePositionResult, MaliputError> {
860        let lpr = maliput_sys::api::ffi::Lane_ToLanePosition(self.lane, inertial_position.ip.as_ref().expect(""))?;
861        Ok(LanePositionResult {
862            lane_position: LanePosition {
863                lp: maliput_sys::api::ffi::LanePositionResult_road_position(&lpr),
864            },
865            nearest_position: InertialPosition {
866                ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&lpr),
867            },
868            distance: maliput_sys::api::ffi::LanePositionResult_distance(&lpr),
869        })
870    }
871    /// Determines the [LanePosition] corresponding to [InertialPosition] `inertial_position`.
872    /// The [LanePosition] is expected to be contained within the segment's boundaries.
873    /// See [Lane::to_lane_position] method.
874    ///
875    /// This method guarantees that its result satisfies the condition that
876    /// `to_inertial_position(result.lane_position)` is within `linear_tolerance()`
877    ///  of `result.nearest_position`.
878    ///
879    /// # Arguments
880    /// * `inertial_position` - A [InertialPosition] to get a SegmentPosition from.
881    ///
882    /// # Return
883    /// A [LanePositionResult] with the closest [LanePosition] within the segment, the corresponding
884    /// [InertialPosition] to that [LanePosition] and the distance between the input and output
885    /// [InertialPosition]s.
886    pub fn to_segment_position(
887        &self,
888        inertial_position: &InertialPosition,
889    ) -> Result<LanePositionResult, MaliputError> {
890        let spr = maliput_sys::api::ffi::Lane_ToSegmentPosition(self.lane, inertial_position.ip.as_ref().expect(""))?;
891        Ok(LanePositionResult {
892            lane_position: LanePosition {
893                lp: maliput_sys::api::ffi::LanePositionResult_road_position(&spr),
894            },
895            nearest_position: InertialPosition {
896                ip: maliput_sys::api::ffi::LanePositionResult_nearest_position(&spr),
897            },
898            distance: maliput_sys::api::ffi::LanePositionResult_distance(&spr),
899        })
900    }
901    /// Returns the nominal lateral (r) bounds for the lane as a function of s.
902    ///
903    /// These are the lateral bounds for a position that is considered to be
904    /// "staying in the lane".
905    ///
906    /// See also [Lane::segment_bounds] that defines the whole surface.
907    ///
908    /// # Arguments
909    /// * `s` - The longitudinal position along the lane's reference line.
910    ///
911    /// # Returns
912    /// A [RBounds] containing the lateral bounds of the lane at the given `s.
913    ///
914    /// # Errors
915    /// If lane bounds cannot be computed, an error is returned. This can happen if the
916    /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]).
917    pub fn lane_bounds(&self, s: f64) -> Result<RBounds, MaliputError> {
918        let bounds = maliput_sys::api::ffi::Lane_lane_bounds(self.lane, s)?;
919        Ok(RBounds::new(bounds.min(), bounds.max()))
920    }
921    /// Returns the lateral segment (r) bounds of the lane as a function of s.
922    ///
923    /// These are the lateral bounds for a position that is considered to be
924    /// "on segment", reflecting the physical extent of the surface of the
925    /// lane's segment.
926    ///
927    /// See also [Lane::lane_bounds] that defines what's considered to be "staying
928    /// in the lane".
929    ///
930    /// # Arguments
931    /// * `s` - The longitudinal position along the lane's reference line.
932    ///
933    /// # Returns
934    /// A [RBounds] containing the lateral segment bounds of the lane at the given `
935    /// s`.
936    ///
937    /// # Errors
938    /// If segment bounds cannot be computed, an error is returned. This can happen if the
939    /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]).
940    pub fn segment_bounds(&self, s: f64) -> Result<RBounds, MaliputError> {
941        let bounds = maliput_sys::api::ffi::Lane_segment_bounds(self.lane, s)?;
942        Ok(RBounds::new(bounds.min(), bounds.max()))
943    }
944    /// Returns the elevation (`h`) bounds of the lane as a function of `(s, r)`.
945    ///
946    /// These are the elevation bounds for a position that is considered to be
947    /// within the Lane's volume modeled by the RoadGeometry.
948    ///
949    /// `s` is within [0, `length()`] of this Lane and `r` is within
950    /// `lane_bounds(s)`.
951    ///
952    /// # Arguments
953    /// * `s` - The longitudinal position along the lane's reference line.
954    /// * `r` - The lateral position perpendicular to the reference line at `s`.
955    ///
956    /// # Returns
957    /// A [HBounds] containing the elevation bounds of the lane at the given `(s, r)`.
958    ///
959    /// # Errors
960    /// If elevation bounds cannot be computed, an error is returned. This can happen if the
961    /// `s` value is out of bounds (i.e., not in the range [0, Lane::length()]) or if `r` is not within the
962    /// lane bounds at `s`.
963    pub fn elevation_bounds(&self, s: f64, r: f64) -> Result<HBounds, MaliputError> {
964        let bounds = maliput_sys::api::ffi::Lane_elevation_bounds(self.lane, s, r)?;
965        Ok(HBounds::new(bounds.min(), bounds.max()))
966    }
967    /// Computes derivatives of [LanePosition] given a velocity vector `velocity`.
968    /// `velocity` is a isometric velocity vector oriented in the `Lane`-frame
969    /// at `position`.
970    ///
971    /// # Arguments
972    /// * `lane_position` - A [LanePosition] at which to evaluate the derivatives.
973    /// * `velocity` - An [IsoLaneVelocity] representing the velocity vector in the `Lane`-frame
974    ///   at `lane_position`.
975    ///
976    /// # Returns
977    /// Returns `Lane`-frame derivatives packed into a [LanePosition] struct.
978    pub fn eval_motion_derivatives(&self, lane_position: &LanePosition, velocity: &IsoLaneVelocity) -> LanePosition {
979        LanePosition {
980            lp: maliput_sys::api::ffi::Lane_EvalMotionDerivatives(
981                self.lane,
982                lane_position.lp.as_ref().expect(""),
983                velocity.sigma_v,
984                velocity.rho_v,
985                velocity.eta_v,
986            ),
987        }
988    }
989    /// Returns the lane's [BranchPoint] for the specified end.
990    ///
991    /// # Argument
992    /// * `end` - This lane's start or end [LaneEnd].
993    ///
994    /// # Return
995    /// The lane's [BranchPoint] for the specified end.
996    pub fn get_branch_point(&self, end: &LaneEnd) -> Result<BranchPoint, MaliputError> {
997        if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
998            return Err(MaliputError::AssertionError(format!(
999                "LaneEnd must be an end of this lane {:?}",
1000                end
1001            )));
1002        }
1003        Ok(BranchPoint {
1004            branch_point: unsafe {
1005                maliput_sys::api::ffi::Lane_GetBranchPoint(self.lane, end == &LaneEnd::Start(self.clone()))
1006                    .as_ref()
1007                    .expect("Underlying BranchPoint is null")
1008            },
1009        })
1010    }
1011    /// Returns the set of [LaneEnd]'s which connect with this lane on the
1012    /// same side of the [BranchPoint] at `end`. At a minimum,
1013    /// this set will include this [Lane].
1014    ///
1015    /// # Arguments
1016    /// * `end` - This lane's start or end [LaneEnd].
1017    ///
1018    /// # Return
1019    /// A [LaneEndSet] with all the [LaneEnd]s at the same side of the [BranchPoint] at `end`.
1020    pub fn get_confluent_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1021        if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
1022            return Err(MaliputError::AssertionError(format!(
1023                "LaneEnd must be an end of this lane {:?}",
1024                end
1025            )));
1026        }
1027        Ok(LaneEndSet {
1028            lane_end_set: unsafe {
1029                maliput_sys::api::ffi::Lane_GetConfluentBranches(self.lane, end == &LaneEnd::Start(self.clone()))?
1030                    .as_ref()
1031                    .expect("Underlying LaneEndSet is null")
1032            },
1033        })
1034    }
1035    /// Returns the set of [LaneEnd]s which continue onward from this lane at the
1036    /// [BranchPoint] at `end`.
1037    ///
1038    /// # Arguments
1039    /// * `end` - This lane's start or end [LaneEnd].
1040    ///
1041    /// # Return
1042    /// A [LaneEndSet] with all the [LaneEnd]s at the opposite side of the [BranchPoint] at `end`.
1043    pub fn get_ongoing_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1044        if end != &LaneEnd::Start(self.clone()) && end != &LaneEnd::Finish(self.clone()) {
1045            return Err(MaliputError::AssertionError(format!(
1046                "LaneEnd must be an end of this lane {:?}",
1047                end
1048            )));
1049        }
1050        Ok(LaneEndSet {
1051            lane_end_set: unsafe {
1052                maliput_sys::api::ffi::Lane_GetOngoingBranches(self.lane, end == &LaneEnd::Start(self.clone()))?
1053                    .as_ref()
1054                    .expect("Underlying LaneEndSet is null")
1055            },
1056        })
1057    }
1058    /// Returns the default ongoing LaneEnd connected at `end`,
1059    /// or None if no default branch has been established.
1060    ///
1061    /// # Arguments
1062    /// * `end` - This lane's start or end [LaneEnd].
1063    ///
1064    /// # Return
1065    /// An `Option<LaneEnd>` containing the default branch if it exists, or None
1066    /// if no default branch has been established.
1067    pub fn get_default_branch(&self, end: &LaneEnd) -> Option<LaneEnd> {
1068        assert! {
1069            end == &LaneEnd::Start(self.clone()) || end == &LaneEnd::Finish(self.clone()),
1070            "LaneEnd must be an end of this lane {:?}",
1071           end
1072        }
1073        let lane_end = maliput_sys::api::ffi::Lane_GetDefaultBranch(self.lane, end == &LaneEnd::Start(self.clone()));
1074        match lane_end.is_null() {
1075            true => None,
1076            false => {
1077                let lane_end_ref: &maliput_sys::api::ffi::LaneEnd =
1078                    lane_end.as_ref().expect("Underlying LaneEnd is null");
1079                let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end_ref);
1080                let lane_ref = unsafe {
1081                    maliput_sys::api::ffi::LaneEnd_lane(lane_end_ref)
1082                        .as_ref()
1083                        .expect("Underlying LaneEnd is null")
1084                };
1085                match is_start {
1086                    true => Some(LaneEnd::Start(Lane { lane: lane_ref })),
1087                    false => Some(LaneEnd::Finish(Lane { lane: lane_ref })),
1088                }
1089            }
1090        }
1091    }
1092    /// Evaluates if the `Lane` contains the given `LanePosition`.
1093    ///
1094    /// # Arguments
1095    /// * `lane_position` - A [LanePosition] to check if it is contained within the `Lane`.
1096    ///
1097    /// # Returns
1098    /// A boolean indicating whether the `Lane` contains the `LanePosition`.
1099    pub fn contains(&self, lane_position: &LanePosition) -> bool {
1100        self.lane.Contains(lane_position.lp.as_ref().expect(""))
1101    }
1102}
1103
1104/// Copy trait for Lane.
1105/// A reference to the Lane is copied.
1106impl Clone for Lane<'_> {
1107    fn clone(&self) -> Self {
1108        Lane { lane: self.lane }
1109    }
1110}
1111
1112/// A Segment represents a bundle of adjacent Lanes which share a
1113/// continuously traversable road surface. Every [LanePosition] on a
1114/// given [Lane] of a Segment has a corresponding [LanePosition] on each
1115/// other [Lane], all with the same height-above-surface h, that all
1116/// map to the same GeoPoint in 3-space.
1117///
1118/// Segments are grouped by [Junction].
1119pub struct Segment<'a> {
1120    segment: &'a maliput_sys::api::ffi::Segment,
1121}
1122
1123impl<'a> Segment<'a> {
1124    /// Returns the id of the Segment.
1125    /// The id is a unique identifier for the Segment within the RoadGeometry.
1126    ///
1127    /// # Returns
1128    /// A `String` containing the id of the Segment.
1129    pub fn id(&self) -> String {
1130        maliput_sys::api::ffi::Segment_id(self.segment)
1131    }
1132    /// Returns the [Junction] to which this Segment belongs.
1133    ///
1134    /// # Returns
1135    /// An [`Result<Junction, MaliputError>`] containing the Junction to which this Segment belongs.
1136    /// If the Segment does not belong to a Junction, an error is returned.
1137    pub fn junction(&self) -> Result<Junction, MaliputError> {
1138        let junction = self.segment.junction()?;
1139        if junction.is_null() {
1140            return Err(MaliputError::AssertionError(
1141                "Segment does not belong to a Junction".to_string(),
1142            ));
1143        }
1144        unsafe {
1145            Ok(Junction {
1146                junction: junction.as_ref().expect(""),
1147            })
1148        }
1149    }
1150    /// Returns the number of lanes in the Segment.
1151    ///
1152    /// # Returns
1153    /// The number of lanes in the Segment.
1154    pub fn num_lanes(&self) -> i32 {
1155        self.segment.num_lanes()
1156    }
1157    /// Returns the lane at the given `index`.
1158    ///
1159    /// # Arguments
1160    /// * `index` - The index of the lane to retrieve.
1161    ///
1162    /// # Returns
1163    /// A [`Lane`] containing the lane at the given index.
1164    pub fn lane(&self, index: i32) -> Result<Lane, MaliputError> {
1165        if index < 0 || index >= self.num_lanes() {
1166            return Err(MaliputError::AssertionError(format!(
1167                "Index {} is out of bounds for Segment with {} lanes",
1168                index,
1169                self.num_lanes()
1170            )));
1171        }
1172        let lane = self.segment.lane(index)?;
1173        unsafe {
1174            Ok(Lane {
1175                lane: lane.as_ref().expect(""),
1176            })
1177        }
1178    }
1179}
1180
1181/// A Junction is a closed set of [Segment]s which have physically
1182/// coplanar road surfaces, in the sense that [RoadPosition]s with the
1183/// same h value (height above surface) in the domains of two [Segment]s
1184/// map to the same [InertialPosition].  The [Segment]s need not be directly
1185/// connected to one another in the network topology.
1186///
1187/// Junctions are grouped by [RoadGeometry].
1188pub struct Junction<'a> {
1189    junction: &'a maliput_sys::api::ffi::Junction,
1190}
1191
1192impl<'a> Junction<'a> {
1193    /// Returns the id of the Junction.
1194    /// The id is a unique identifier for the Junction within the RoadGeometry.
1195    ///
1196    /// # Returns
1197    /// A `String` containing the id of the Junction.
1198    pub fn id(&self) -> String {
1199        maliput_sys::api::ffi::Junction_id(self.junction)
1200    }
1201    /// Returns the [RoadGeometry] to which this Junction belongs.
1202    ///
1203    /// # Returns
1204    /// A [`RoadGeometry`] containing the RoadGeometry to which this Junction belongs.
1205    pub fn road_geometry(&self) -> RoadGeometry {
1206        unsafe {
1207            RoadGeometry {
1208                rg: self.junction.road_geometry().as_ref().expect(""),
1209            }
1210        }
1211    }
1212    /// Returns the number of segments in the Junction.
1213    ///
1214    /// # Returns
1215    /// The number of segments in the Junction.
1216    pub fn num_segments(&self) -> i32 {
1217        self.junction.num_segments()
1218    }
1219    /// Returns the segment at the given `index`.
1220    ///
1221    /// # Arguments
1222    /// * `index` - The index of the segment to retrieve.
1223    ///
1224    /// # Returns
1225    /// A [Result<Segment, MaliputError>] containing the segment at the given index.
1226    /// If the index is out of bounds, an error is returned.
1227    pub fn segment(&self, index: i32) -> Result<Segment, MaliputError> {
1228        unsafe {
1229            Ok(Segment {
1230                segment: self.junction.segment(index)?.as_ref().expect(""),
1231            })
1232        }
1233    }
1234}
1235
1236/// A position in the road network compound by a specific lane and a lane-frame position in that lane.
1237/// This position is defined by a [Lane] and a [LanePosition].
1238pub struct RoadPosition {
1239    rp: cxx::UniquePtr<maliput_sys::api::ffi::RoadPosition>,
1240}
1241
1242impl RoadPosition {
1243    /// Create a new `RoadPosition` with the given `lane` and `lane_pos`.
1244    ///
1245    /// # Arguments
1246    /// * `lane` - A reference to a [Lane] that this `RoadPosition` is associated with.
1247    /// * `lane_pos` - A reference to a [LanePosition] that defines the position within the lane.
1248    ///
1249    /// # Returns
1250    /// A new `RoadPosition` instance.
1251    pub fn new(lane: &Lane, lane_pos: &LanePosition) -> RoadPosition {
1252        unsafe {
1253            RoadPosition {
1254                rp: maliput_sys::api::ffi::RoadPosition_new(lane.lane, &lane_pos.lp),
1255            }
1256        }
1257    }
1258    /// Computes the [InertialPosition] corresponding to this `RoadPosition`.
1259    ///
1260    /// # Notes
1261    /// This is an indirection to [Lane::to_inertial_position] method.
1262    ///
1263    /// # Returns
1264    /// An [InertialPosition] corresponding to this `RoadPosition`.
1265    pub fn to_inertial_position(&self) -> InertialPosition {
1266        InertialPosition {
1267            ip: maliput_sys::api::ffi::RoadPosition_ToInertialPosition(&self.rp),
1268        }
1269    }
1270    /// Gets the [Lane] associated with this `RoadPosition`.
1271    ///
1272    /// # Returns
1273    /// A [Lane] that this `RoadPosition` is associated with.
1274    pub fn lane(&self) -> Lane {
1275        unsafe {
1276            Lane {
1277                lane: maliput_sys::api::ffi::RoadPosition_lane(&self.rp).as_ref().expect(""),
1278            }
1279        }
1280    }
1281    /// Gets the [LanePosition] associated with this `RoadPosition`.
1282    ///
1283    /// # Returns
1284    /// A [LanePosition] that defines the position within the lane for this `RoadPosition`.
1285    pub fn pos(&self) -> LanePosition {
1286        LanePosition {
1287            lp: maliput_sys::api::ffi::RoadPosition_pos(&self.rp),
1288        }
1289    }
1290}
1291
1292/// Represents the result of a RoadPosition query.
1293/// This struct contains the `RoadPosition`, the nearest `InertialPosition` to that `RoadPosition`,
1294/// and the distance between the input `InertialPosition` and the nearest `InertialPosition`.
1295///
1296/// This struct is typically used as return type for the methods: [RoadGeometry::to_road_position] and [RoadGeometry::find_road_positions].
1297pub struct RoadPositionResult {
1298    /// The candidate RoadPosition returned by the query.
1299    pub road_position: RoadPosition,
1300    /// The nearest InertialPosition to the candidate RoadPosition.
1301    /// This is the position in the inertial frame that is closest to the candidate RoadPosition
1302    pub nearest_position: InertialPosition,
1303    /// The distance between the input InertialPosition and the nearest InertialPosition.
1304    pub distance: f64,
1305}
1306
1307impl RoadPositionResult {
1308    /// Create a new `RoadPositionResult` with the given `road_position`, `nearest_position`, and `distance`.
1309    pub fn new(road_position: RoadPosition, nearest_position: InertialPosition, distance: f64) -> RoadPositionResult {
1310        RoadPositionResult {
1311            road_position,
1312            nearest_position,
1313            distance,
1314        }
1315    }
1316}
1317
1318/// Represents the result of a LanePosition query.
1319/// This struct contains the `LanePosition`, the nearest `InertialPosition` to that `LanePosition`,
1320/// and the distance between the input `InertialPosition` and the nearest `InertialPosition`.
1321///
1322/// This struct is typically used as return type for the methods: [Lane::to_lane_position] and [Lane::to_segment_position].
1323pub struct LanePositionResult {
1324    /// The candidate LanePosition within the Lane' lane-bounds or segment-bounds
1325    /// depending if [Lane::to_lane_position] or [Lane::to_segment_position] respectively, was called.
1326    /// The LanePosition is closest to a `inertial_position` supplied to [Lane::to_lane_position]
1327    /// (measured by the Cartesian metric in the `Inertial`-frame).
1328    pub lane_position: LanePosition,
1329    /// The position that exactly corresponds to `lane_position`.
1330    pub nearest_position: InertialPosition,
1331    /// The Cartesian distance between `nearest_position` and the
1332    /// `inertial_position` supplied to [Lane::to_lane_position] / [Lane::to_segment_position].
1333    pub distance: f64,
1334}
1335
1336impl LanePositionResult {
1337    /// Create a new `LanePositionResult` with the given `lane_position`, `nearest_position`, and `distance`.
1338    pub fn new(lane_position: LanePosition, nearest_position: InertialPosition, distance: f64) -> LanePositionResult {
1339        LanePositionResult {
1340            lane_position,
1341            nearest_position,
1342            distance,
1343        }
1344    }
1345}
1346
1347/// A 3-dimensional rotation in the road network.
1348/// This struct represents a rotation in the road network, which can be defined
1349/// using a quaternion or roll-pitch-yaw angles.
1350/// It provides methods to create a rotation, convert between representations,
1351/// and apply the rotation to an inertial position.
1352pub struct Rotation {
1353    r: cxx::UniquePtr<maliput_sys::api::ffi::Rotation>,
1354}
1355
1356impl Default for Rotation {
1357    fn default() -> Self {
1358        Self::new()
1359    }
1360}
1361
1362impl Rotation {
1363    /// Create a new `Rotation`.
1364    pub fn new() -> Rotation {
1365        Rotation {
1366            r: maliput_sys::api::ffi::Rotation_new(),
1367        }
1368    }
1369    /// Create a new `Rotation` from a `Quaternion`.
1370    pub fn from_quat(q: &Quaternion) -> Rotation {
1371        let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
1372        Rotation {
1373            r: maliput_sys::api::ffi::Rotation_from_quat(&q_ffi),
1374        }
1375    }
1376    /// Create a new `Rotation` from a `RollPitchYaw`.
1377    pub fn from_rpy(rpy: &RollPitchYaw) -> Rotation {
1378        let rpy_ffi = maliput_sys::math::ffi::RollPitchYaw_new(rpy.roll_angle(), rpy.pitch_angle(), rpy.yaw_angle());
1379        Rotation {
1380            r: maliput_sys::api::ffi::Rotation_from_rpy(&rpy_ffi),
1381        }
1382    }
1383    /// Get the roll of the `Rotation`.
1384    pub fn roll(&self) -> f64 {
1385        self.r.roll()
1386    }
1387    /// Get the pitch of the `Rotation`.
1388    pub fn pitch(&self) -> f64 {
1389        self.r.pitch()
1390    }
1391    /// Get the yaw of the `Rotation`.
1392    pub fn yaw(&self) -> f64 {
1393        self.r.yaw()
1394    }
1395    /// Get a quaternion representation of the `Rotation`.
1396    pub fn quat(&self) -> Quaternion {
1397        let q_ffi = self.r.quat();
1398        Quaternion::new(q_ffi.w(), q_ffi.x(), q_ffi.y(), q_ffi.z())
1399    }
1400    /// Get a roll-pitch-yaw representation of the `Rotation`.
1401    pub fn rpy(&self) -> RollPitchYaw {
1402        let rpy_ffi = maliput_sys::api::ffi::Rotation_rpy(&self.r);
1403        RollPitchYaw::new(rpy_ffi.roll_angle(), rpy_ffi.pitch_angle(), rpy_ffi.yaw_angle())
1404    }
1405    /// Set the `Rotation` from a `Quaternion`.
1406    pub fn set_quat(&mut self, q: &Quaternion) {
1407        let q_ffi = maliput_sys::math::ffi::Quaternion_new(q.w(), q.x(), q.y(), q.z());
1408        maliput_sys::api::ffi::Rotation_set_quat(self.r.pin_mut(), &q_ffi);
1409    }
1410    /// Get the matrix representation of the `Rotation`.
1411    pub fn matrix(&self) -> Matrix3 {
1412        let matrix_ffi: cxx::UniquePtr<maliput_sys::math::ffi::Matrix3> =
1413            maliput_sys::api::ffi::Rotation_matrix(&self.r);
1414        let row_0 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 0);
1415        let row_1 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 1);
1416        let row_2 = maliput_sys::math::ffi::Matrix3_row(matrix_ffi.as_ref().expect(""), 2);
1417        Matrix3::new(
1418            Vector3::new(row_0.x(), row_0.y(), row_0.z()),
1419            Vector3::new(row_1.x(), row_1.y(), row_1.z()),
1420            Vector3::new(row_2.x(), row_2.y(), row_2.z()),
1421        )
1422    }
1423    /// Get the distance between two `Rotation`.
1424    pub fn distance(&self, other: &Rotation) -> f64 {
1425        self.r.Distance(&other.r)
1426    }
1427    /// Apply the `Rotation` to an `InertialPosition`.
1428    pub fn apply(&self, v: &InertialPosition) -> InertialPosition {
1429        InertialPosition {
1430            ip: maliput_sys::api::ffi::Rotation_Apply(&self.r, &v.ip),
1431        }
1432    }
1433    /// Get the reverse of the `Rotation`.
1434    pub fn reverse(&self) -> Rotation {
1435        Rotation {
1436            r: maliput_sys::api::ffi::Rotation_Reverse(&self.r),
1437        }
1438    }
1439}
1440
1441/// Directed, inclusive longitudinal (s value) range from s0 to s1.
1442pub struct SRange {
1443    s_range: cxx::UniquePtr<maliput_sys::api::ffi::SRange>,
1444}
1445
1446impl SRange {
1447    /// Creates a new `SRange` with the given `s0` and `s1`.
1448    ///
1449    /// # Arguments
1450    /// * `s0` - The starting value of the range.
1451    /// * `s1` - The ending value of the range.
1452    ///
1453    /// # Returns
1454    /// A new `SRange` instance.
1455    pub fn new(s0: f64, s1: f64) -> SRange {
1456        SRange {
1457            s_range: maliput_sys::api::ffi::SRange_new(s0, s1),
1458        }
1459    }
1460    /// Returns the s0 of the `SRange`.
1461    ///
1462    /// # Returns
1463    /// The starting value of the range.
1464    pub fn s0(&self) -> f64 {
1465        self.s_range.s0()
1466    }
1467    /// Returns the s1 of the `SRange`.
1468    ///
1469    /// # Returns
1470    /// The ending value of the range.
1471    pub fn s1(&self) -> f64 {
1472        self.s_range.s1()
1473    }
1474    /// Sets the s0 of the `SRange`.
1475    ///
1476    /// # Arguments
1477    /// * `s0` - The new starting value of the range.
1478    pub fn set_s0(&mut self, s0: f64) {
1479        self.s_range.as_mut().expect("Underlying SRange is null").set_s0(s0);
1480    }
1481    /// Sets the s1 of the `SRange`.
1482    ///
1483    /// # Arguments
1484    /// * `s1` - The new ending value of the range.
1485    pub fn set_s1(&mut self, s1: f64) {
1486        self.s_range.as_mut().expect("Underlying SRange is null").set_s1(s1);
1487    }
1488    /// Get the size of the `SRange`.
1489    ///
1490    /// # Returns
1491    /// The size of the range, which is the difference between s1 and s0.
1492    pub fn size(&self) -> f64 {
1493        self.s_range.size()
1494    }
1495    /// Defines whether this SRange is in the direction of +s (i.e., s1() > s0()).
1496    ///
1497    /// # Returns
1498    /// A boolean indicating whether the SRange is in the direction of +s.
1499    pub fn with_s(&self) -> bool {
1500        self.s_range.WithS()
1501    }
1502    /// Determines whether this SRange intersects with `s_range`.
1503    ///
1504    /// # Arguments
1505    /// * `s_range` - Another `SRange` to check for intersection.
1506    /// * `tolerance` - A tolerance value to consider when checking for intersection.
1507    ///
1508    /// # Returns
1509    /// A boolean indicating whether this SRange intersects with `s_range`.
1510    pub fn intersects(&self, s_range: &SRange, tolerance: f64) -> bool {
1511        self.s_range.Intersects(&s_range.s_range, tolerance)
1512    }
1513    /// Determines whether this SRange contains `s_range`.
1514    ///
1515    /// # Arguments
1516    /// * `s_range` - Another `SRange` to check if it is contained within this SRange.
1517    /// * `tolerance` - A tolerance value to consider when checking for containment.
1518    pub fn contains(&self, s_range: &SRange, tolerance: f64) -> bool {
1519        self.s_range.Contains(&s_range.s_range, tolerance)
1520    }
1521    /// Get the intersection of this SRange with `s_range`.
1522    ///
1523    /// # Arguments
1524    /// * `s_range` - Another `SRange` to get the intersection with.
1525    /// * `tolerance` - A tolerance value to consider when checking for intersection.
1526    ///
1527    /// # Returns
1528    /// An `Option<SRange>` containing the intersection of this SRange with `s_range`.
1529    /// If the intersection is empty, it returns None.
1530    pub fn get_intersection(&self, s_range: &SRange, tolerance: f64) -> Option<SRange> {
1531        let intersection = maliput_sys::api::ffi::SRange_GetIntersection(&self.s_range, &s_range.s_range, tolerance);
1532        match intersection.is_null() {
1533            true => None,
1534            false => Some(SRange { s_range: intersection }),
1535        }
1536    }
1537}
1538
1539impl std::fmt::Debug for SRange {
1540    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1541        write!(f, "SRange {{ s0: {}, s1: {} }}", self.s0(), self.s1())
1542    }
1543}
1544
1545/// Directed longitudinal range of a specific Lane, identified by a LaneId.
1546/// Similar to [SRange], but associated with a specific Lane.
1547pub struct LaneSRange {
1548    pub(crate) lane_s_range: cxx::UniquePtr<maliput_sys::api::ffi::LaneSRange>,
1549}
1550
1551impl LaneSRange {
1552    /// Creates a new `LaneSRange` with the given `lane_id` and `s_range`.
1553    /// # Arguments
1554    /// * `lane_id` - A `String` representing the id of the lane.
1555    /// * `s_range` - A reference to an [SRange] that defines the longitudinal range of the lane.
1556    ///
1557    /// # Returns
1558    /// A new `LaneSRange` instance.
1559    pub fn new(lane_id: &String, s_range: &SRange) -> LaneSRange {
1560        LaneSRange {
1561            lane_s_range: maliput_sys::api::ffi::LaneSRange_new(lane_id, &s_range.s_range),
1562        }
1563    }
1564    /// Returns the lane id of the `LaneSRange`.
1565    ///
1566    /// # Returns
1567    /// A `String` containing the id of the lane associated with this `LaneSRange`.
1568    pub fn lane_id(&self) -> String {
1569        maliput_sys::api::ffi::LaneSRange_lane_id(&self.lane_s_range)
1570    }
1571    /// Returns the [SRange] of the `LaneSRange`.
1572    ///
1573    /// # Returns
1574    /// An [SRange] containing the longitudinal range of the lane associated with this `LaneSRange`.
1575    pub fn s_range(&self) -> SRange {
1576        SRange {
1577            s_range: maliput_sys::api::ffi::LaneSRange_s_range(&self.lane_s_range),
1578        }
1579    }
1580    /// Returns the length of the `LaneSRange`.
1581    ///
1582    /// This is equivalent to `s_range.size()`.
1583    ///
1584    /// # Returns
1585    /// A `f64` representing the length of the `LaneSRange`.
1586    pub fn length(&self) -> f64 {
1587        self.lane_s_range.length()
1588    }
1589    /// Determines whether this LaneSRange intersects with `lane_s_range`.
1590    ///
1591    /// # Arguments
1592    /// * `lane_s_range` - Another `LaneSRange` to check for intersection.
1593    /// * `tolerance` - A tolerance value to consider when checking for intersection.
1594    ///
1595    /// # Returns
1596    /// A boolean indicating whether this LaneSRange intersects with `lane_s_range`.
1597    pub fn intersects(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool {
1598        self.lane_s_range.Intersects(&lane_s_range.lane_s_range, tolerance)
1599    }
1600    /// Determines whether this LaneSRange contains `lane_s_range`.
1601    ///
1602    /// # Arguments
1603    /// * `lane_s_range` - Another `LaneSRange` to check if it is contained within this LaneSRange.
1604    /// * `tolerance` - A tolerance value to consider when checking for containment.
1605    ///
1606    /// # Returns
1607    /// A boolean indicating whether this LaneSRange contains `lane_s_range`.
1608    /// This checks if the `s_range` of `lane_s_range` is fully contained
1609    /// within the `s_range` of this `LaneSRange`, considering the lane id.
1610    /// If the lane id does not match, it returns false.
1611    pub fn contains(&self, lane_s_range: &LaneSRange, tolerance: f64) -> bool {
1612        self.lane_s_range.Contains(&lane_s_range.lane_s_range, tolerance)
1613    }
1614    /// Computes the intersection of this `LaneSRange` with `lane_s_range`.
1615    ///
1616    /// # Arguments
1617    /// * `lane_s_range` - Another `LaneSRange` to get the intersection with.
1618    /// * `tolerance` - A tolerance value to consider when checking for intersection.
1619    ///
1620    /// # Returns
1621    /// An `Option<LaneSRange>` containing the intersection of this `LaneSRange` with `lane_s_range`.
1622    /// If the lane ids do not match, it returns None.
1623    /// If the intersection is empty, it returns None.
1624    pub fn get_intersection(&self, lane_s_range: &LaneSRange, tolerance: f64) -> Option<LaneSRange> {
1625        let intersection = maliput_sys::api::ffi::LaneSRange_GetIntersection(
1626            &self.lane_s_range,
1627            &lane_s_range.lane_s_range,
1628            tolerance,
1629        );
1630        match intersection.is_null() {
1631            true => None,
1632            false => Some(LaneSRange {
1633                lane_s_range: intersection,
1634            }),
1635        }
1636    }
1637}
1638
1639impl std::fmt::Debug for LaneSRange {
1640    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1641        write!(
1642            f,
1643            "LaneSRange {{ lane_id: {}, s_range: {:?} }}",
1644            self.lane_id(),
1645            self.s_range()
1646        )
1647    }
1648}
1649
1650/// A route, possibly spanning multiple (end-to-end) lanes.
1651///
1652/// The sequence of [LaneSRange]s should be contiguous by either presenting
1653/// laterally adjacent [LaneSRange]s, or consecutive [LaneSRange]s. (In other words,
1654/// taken as a Lane-space path with r=0 and h=0, it should present a
1655/// G1-continuous curve.)
1656pub struct LaneSRoute {
1657    lane_s_route: cxx::UniquePtr<maliput_sys::api::ffi::LaneSRoute>,
1658}
1659
1660impl LaneSRoute {
1661    /// Creates a new `LaneSRoute` with the given `ranges`.
1662    ///
1663    /// # Arguments
1664    /// * `ranges` - A vector of [LaneSRange] to create the [LaneSRoute].
1665    ///
1666    /// # Returns
1667    /// A new `LaneSRoute` instance containing the provided ranges.
1668    pub fn new(ranges: Vec<LaneSRange>) -> LaneSRoute {
1669        let mut lane_s_ranges_cpp = cxx::CxxVector::new();
1670        for range in &ranges {
1671            lane_s_ranges_cpp
1672                .as_mut()
1673                .unwrap()
1674                .push(maliput_sys::api::ffi::ConstLaneSRangeRef {
1675                    lane_s_range: &range.lane_s_range,
1676                });
1677        }
1678        LaneSRoute {
1679            lane_s_route: maliput_sys::api::ffi::LaneSRoute_new(&lane_s_ranges_cpp),
1680        }
1681    }
1682
1683    /// Returns the sequence of [LaneSRange]s.
1684    ///
1685    /// # Returns
1686    /// A vector of [LaneSRange]s that make up this [LaneSRoute].
1687    pub fn ranges(&self) -> Vec<LaneSRange> {
1688        let mut ranges = Vec::new();
1689        let lane_s_ranges = self.lane_s_route.ranges();
1690        for range in lane_s_ranges {
1691            ranges.push(LaneSRange {
1692                lane_s_range: maliput_sys::api::ffi::LaneSRange_new(
1693                    &maliput_sys::api::ffi::LaneSRange_lane_id(range),
1694                    maliput_sys::api::ffi::LaneSRange_s_range(range).as_ref().expect(""),
1695                ),
1696            })
1697        }
1698        ranges
1699    }
1700
1701    /// Computes the accumulated length of all [LaneSRange]s.
1702    ///
1703    /// # Returns
1704    /// A `f64` representing the total length of the [LaneSRoute].
1705    pub fn length(&self) -> f64 {
1706        self.lane_s_route.length()
1707    }
1708
1709    /// Determines whether this LaneSRoute intersects with `other`.
1710    ///
1711    /// # Arguments
1712    /// * `other` - The other LaneSRoute to check for intersection.
1713    /// * `tolerance` - The tolerance to use for intersection checks.
1714    ///
1715    /// # Returns
1716    /// * `true` if the two LaneSRoute intersect, `false` otherwise.
1717    pub fn intersects(&self, other: &LaneSRoute, tolerance: f64) -> bool {
1718        self.lane_s_route.Intersects(&other.lane_s_route, tolerance)
1719    }
1720}
1721
1722impl std::fmt::Debug for LaneSRoute {
1723    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1724        write!(f, "LaneSRoute {{ ranges: {:?} }}", self.ranges())
1725    }
1726}
1727
1728/// A specific endpoint of a specific Lane.
1729pub enum LaneEnd<'a> {
1730    /// The start of the Lane. ("s == 0")
1731    Start(Lane<'a>),
1732    /// The end of the Lane. ("s == length")
1733    Finish(Lane<'a>),
1734}
1735
1736impl LaneEnd<'_> {
1737    /// Gets the Lane of the `LaneEnd`.
1738    ///
1739    /// # Returns
1740    /// A reference to the [Lane] associated with this `LaneEnd`.
1741    /// This will return the Lane for both Start and Finish variants.
1742    pub fn lane(&self) -> &Lane {
1743        match self {
1744            LaneEnd::Start(lane) => lane,
1745            LaneEnd::Finish(lane) => lane,
1746        }
1747    }
1748}
1749
1750impl PartialEq for LaneEnd<'_> {
1751    fn eq(&self, other: &Self) -> bool {
1752        match self {
1753            LaneEnd::Start(lane) => match other {
1754                LaneEnd::Start(other_lane) => lane.id() == other_lane.id(),
1755                _ => false,
1756            },
1757            LaneEnd::Finish(lane) => match other {
1758                LaneEnd::Finish(other_lane) => lane.id() == other_lane.id(),
1759                _ => false,
1760            },
1761        }
1762    }
1763}
1764
1765impl Eq for LaneEnd<'_> {}
1766
1767impl std::fmt::Display for LaneEnd<'_> {
1768    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1769        match self {
1770            LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
1771            LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
1772        }
1773    }
1774}
1775
1776impl std::fmt::Debug for LaneEnd<'_> {
1777    fn fmt(&self, f: &mut std::fmt::Formatter) -> std::fmt::Result {
1778        match self {
1779            LaneEnd::Start(lane) => write!(f, "LaneEnd::Start({})", lane.id()),
1780            LaneEnd::Finish(lane) => write!(f, "LaneEnd::Finish({})", lane.id()),
1781        }
1782    }
1783}
1784
1785/// A set of LaneEnds.
1786pub struct LaneEndSet<'a> {
1787    lane_end_set: &'a maliput_sys::api::ffi::LaneEndSet,
1788}
1789
1790impl<'a> LaneEndSet<'a> {
1791    /// Obtains the size of the LaneEndSet.
1792    ///
1793    /// # Returns
1794    /// The number of LaneEnds in the set.
1795    pub fn size(&self) -> i32 {
1796        self.lane_end_set.size()
1797    }
1798    /// Gets the [LaneEnd] at the given index.
1799    ///
1800    /// # Arguments
1801    /// * `index` - The index of the LaneEnd to retrieve.
1802    ///
1803    /// # Returns
1804    /// A [Result<LaneEnd, MaliputError>] containing the LaneEnd at the given index.
1805    /// If the index is out of bounds, an error is returned.
1806    pub fn get(&self, index: i32) -> Result<LaneEnd, MaliputError> {
1807        let lane_end = self.lane_end_set.get(index)?;
1808        // Obtain end type and lane reference.
1809        let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end);
1810        let lane_ref = unsafe {
1811            maliput_sys::api::ffi::LaneEnd_lane(lane_end)
1812                .as_ref()
1813                .expect("Underlying LaneEnd is null")
1814        };
1815        // Create a LaneEnd enum variant.
1816        Ok(match is_start {
1817            true => LaneEnd::Start(Lane { lane: lane_ref }),
1818            false => LaneEnd::Finish(Lane { lane: lane_ref }),
1819        })
1820    }
1821
1822    /// Converts the LaneEndSet to a map of lane-id to LaneEnd.
1823    ///
1824    /// # Returns
1825    /// A `HashMap<String, LaneEnd>` where the key is the lane id and
1826    /// the value is the corresponding LaneEnd.
1827    pub fn to_lane_map(&self) -> std::collections::HashMap<String, LaneEnd> {
1828        (0..self.size())
1829            .map(|i| {
1830                let end = self.get(i).unwrap();
1831                (end.lane().id(), end)
1832            })
1833            .collect()
1834    }
1835}
1836
1837/// A BranchPoint is a node in the network of a RoadGeometry at which
1838/// Lanes connect to one another.  A BranchPoint is a collection of LaneEnds
1839/// specifying the Lanes (and, in particular, which ends of the Lanes) are
1840/// connected at the BranchPoint.
1841///
1842/// LaneEnds participating in a BranchPoint are grouped into two sets,
1843/// arbitrarily named "A-side" and "B-side". LaneEnds on the same "side"
1844/// have coincident into-the-lane tangent vectors, which are anti-parallel
1845/// to those of LaneEnds on the other side.
1846pub struct BranchPoint<'a> {
1847    branch_point: &'a maliput_sys::api::ffi::BranchPoint,
1848}
1849
1850impl<'a> BranchPoint<'a> {
1851    /// Returns the id of the BranchPoint.
1852    ///
1853    /// # Returns
1854    /// A `String` containing the id of the BranchPoint.
1855    pub fn id(&self) -> String {
1856        maliput_sys::api::ffi::BranchPoint_id(self.branch_point)
1857    }
1858    /// Returns the [RoadGeometry] to which this BranchPoint belongs.
1859    ///
1860    /// # Returns
1861    /// A [RoadGeometry] containing the RoadGeometry to which this BranchPoint belongs.
1862    pub fn road_geometry(&self) -> RoadGeometry {
1863        unsafe {
1864            RoadGeometry {
1865                rg: self.branch_point.road_geometry().as_ref().expect(""),
1866            }
1867        }
1868    }
1869    /// Returns the set of [LaneEnd]s on the same side as the given [LaneEnd].
1870    /// E.g: For a T-junction, this would return the set of LaneEnds on the merging side.
1871    ///
1872    /// # Arguments
1873    /// * `end` - This branch's start or end [LaneEnd].
1874    ///
1875    /// # Return
1876    /// A [LaneEndSet] of [LaneEnd]s on the same side as the given [LaneEnd].
1877    pub fn get_confluent_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1878        let lane_end_set_ptr = self.branch_point.GetConfluentBranches(
1879            BranchPoint::from_lane_end_to_ffi(end)
1880                .as_ref()
1881                .expect("Underlying LaneEnd is null"),
1882        )?;
1883        Ok(LaneEndSet {
1884            lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1885        })
1886    }
1887    /// Returns the set of [LaneEnd]s on the opposite side as the given [LaneEnd].
1888    /// E.g: For a T-junction, this would return the [LaneEnd]s which end flows into the junction.
1889    ///
1890    /// # Arguments
1891    /// * `end` - This branch's start or end [LaneEnd].
1892    ///
1893    /// # Return
1894    /// A [LaneEndSet] of [LaneEnd]s on the opposite side as the given [LaneEnd].
1895    pub fn get_ongoing_branches(&self, end: &LaneEnd) -> Result<LaneEndSet, MaliputError> {
1896        let lane_end_set_ptr = self.branch_point.GetOngoingBranches(
1897            BranchPoint::from_lane_end_to_ffi(end)
1898                .as_ref()
1899                .expect("Underlying LaneEnd is null"),
1900        )?;
1901        Ok(LaneEndSet {
1902            lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1903        })
1904    }
1905    /// Returns the default ongoing branch (if any) for the given `end`.
1906    /// This typically represents what would be considered "continuing
1907    /// through-traffic" from `end` (e.g., as opposed to a branch executing
1908    /// a turn).
1909    ///
1910    /// If `end` has no default-branch at this BranchPoint, the return
1911    /// value will be None.
1912    ///
1913    /// # Arguments
1914    /// * `end` - The [LaneEnd] for which to get the default branch.
1915    ///
1916    /// # Returns
1917    /// An `Option<LaneEnd>` containing the default branch if it exists.
1918    /// If no default branch exists, it returns None.
1919    pub fn get_default_branch(&self, end: &LaneEnd) -> Option<LaneEnd> {
1920        let lane_end = maliput_sys::api::ffi::BranchPoint_GetDefaultBranch(
1921            self.branch_point,
1922            BranchPoint::from_lane_end_to_ffi(end)
1923                .as_ref()
1924                .expect("Underlying LaneEnd is null"),
1925        );
1926        match lane_end.is_null() {
1927            true => None,
1928            false => {
1929                let lane_end_ref: &maliput_sys::api::ffi::LaneEnd =
1930                    lane_end.as_ref().expect("Underlying LaneEnd is null");
1931                let is_start = maliput_sys::api::ffi::LaneEnd_is_start(lane_end_ref);
1932                let lane_ref = unsafe {
1933                    maliput_sys::api::ffi::LaneEnd_lane(lane_end_ref)
1934                        .as_ref()
1935                        .expect("Underlying LaneEnd is null")
1936                };
1937                match is_start {
1938                    true => Some(LaneEnd::Start(Lane { lane: lane_ref })),
1939                    false => Some(LaneEnd::Finish(Lane { lane: lane_ref })),
1940                }
1941            }
1942        }
1943    }
1944    /// Returns the set of LaneEnds grouped together on the "A-side".
1945    ///
1946    /// # Returns
1947    /// A [LaneEndSet] containing the LaneEnds on the "A-side" of the BranchPoint.
1948    pub fn get_a_side(&self) -> LaneEndSet {
1949        let lane_end_set_ptr = self.branch_point.GetASide();
1950        LaneEndSet {
1951            lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1952        }
1953    }
1954    /// Returns the set of LaneEnds grouped together on the "B-side".
1955    ///
1956    /// # Returns
1957    /// A [LaneEndSet] containing the LaneEnds on the "B-side" of the BranchPoint.
1958    /// This is the opposite side of the "A-side".
1959    pub fn get_b_side(&self) -> LaneEndSet {
1960        let lane_end_set_ptr = self.branch_point.GetBSide();
1961        LaneEndSet {
1962            lane_end_set: unsafe { lane_end_set_ptr.as_ref().expect("Underlying LaneEndSet is null") },
1963        }
1964    }
1965    /// Converts LaneEnd enum to LaneEnd ffi.
1966    fn from_lane_end_to_ffi(end: &LaneEnd) -> cxx::UniquePtr<maliput_sys::api::ffi::LaneEnd> {
1967        match end {
1968            LaneEnd::Start(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, true) },
1969            LaneEnd::Finish(lane) => unsafe { maliput_sys::api::ffi::LaneEnd_new(lane.lane, false) },
1970        }
1971    }
1972}
1973
1974/// An abstract convenience class that aggregates information pertaining to an
1975/// intersection. Its primary purpose is to serve as a single source of this
1976/// information and to remove the need for users to query numerous disparate
1977/// data structures and state providers.
1978pub struct Intersection<'a> {
1979    intersection: &'a mut maliput_sys::api::ffi::Intersection,
1980}
1981
1982impl<'a> Intersection<'a> {
1983    /// Returns the id of the `Intersection` as a string.
1984    pub fn id(&self) -> String {
1985        maliput_sys::api::ffi::Intersection_id(self.intersection)
1986    }
1987}
1988
1989/// A book of Intersections.
1990pub struct IntersectionBook<'a> {
1991    intersection_book: &'a mut maliput_sys::api::ffi::IntersectionBook,
1992}
1993
1994impl<'a> IntersectionBook<'a> {
1995    /// Returns all Intersections in the book.
1996    ///
1997    /// # Returns
1998    /// A vector of [Intersection]s containing all Intersections in the book.
1999    pub fn get_intersections(&mut self) -> Vec<Intersection> {
2000        let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) };
2001        let intersections_cpp = maliput_sys::api::ffi::IntersectionBook_GetIntersections(book_pin);
2002        unsafe {
2003            intersections_cpp
2004                .into_iter()
2005                .map(|intersection| Intersection {
2006                    intersection: intersection
2007                        .intersection
2008                        .as_mut()
2009                        .expect("Underlying Intersection is null"),
2010                })
2011                .collect::<Vec<Intersection>>()
2012        }
2013    }
2014
2015    /// Gets the specified Intersection.
2016    ///
2017    /// # Arguments
2018    ///   * `id` - The id of the Intersection to get.
2019    ///
2020    /// # Returns
2021    ///   * An `Option<Intersection>`
2022    ///     * Some(Intersection) - The Intersection with the specified id.
2023    ///     * None - If the Intersection with the specified id does not exist.
2024    pub fn get_intersection(&mut self, id: &str) -> Option<Intersection> {
2025        let book_pin = unsafe { std::pin::Pin::new_unchecked(&mut *self.intersection_book) };
2026        let intersection_option = unsafe {
2027            maliput_sys::api::ffi::IntersectionBook_GetIntersection(book_pin, &String::from(id))
2028                .intersection
2029                .as_mut()
2030        };
2031        match &intersection_option {
2032            None => None,
2033            Some(_) => Some(Intersection {
2034                intersection: intersection_option.expect("Underlying Intersection is null"),
2035            }),
2036        }
2037    }
2038}
2039
2040mod tests {
2041    mod lane_position {
2042        #[test]
2043        fn lane_position_new() {
2044            let lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2045            assert_eq!(lane_pos.s(), 1.0);
2046            assert_eq!(lane_pos.r(), 2.0);
2047            assert_eq!(lane_pos.h(), 3.0);
2048        }
2049
2050        #[test]
2051        fn equality() {
2052            let v = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2053            let w = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2054            assert_eq!(v, w);
2055            let z = crate::api::LanePosition::new(4.0, 5.0, 6.0);
2056            assert_ne!(v, z);
2057        }
2058
2059        #[test]
2060        fn set_s() {
2061            let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2062            lane_pos.set_s(4.0);
2063            assert_eq!(lane_pos.s(), 4.0);
2064        }
2065
2066        #[test]
2067        fn set_r() {
2068            let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2069            lane_pos.set_r(4.0);
2070            assert_eq!(lane_pos.r(), 4.0);
2071        }
2072
2073        #[test]
2074        fn set_h() {
2075            let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2076            lane_pos.set_h(4.0);
2077            assert_eq!(lane_pos.h(), 4.0);
2078        }
2079
2080        #[test]
2081        fn set_srh() {
2082            use crate::math::Vector3;
2083            let mut lane_pos = crate::api::LanePosition::new(1.0, 2.0, 3.0);
2084            let vector = Vector3::new(4.0, 5.0, 6.0);
2085            lane_pos.set_srh(&vector);
2086            assert_eq!(lane_pos.s(), 4.0);
2087            assert_eq!(lane_pos.r(), 5.0);
2088            assert_eq!(lane_pos.h(), 6.0);
2089        }
2090    }
2091
2092    mod inertial_position {
2093
2094        #[test]
2095        fn inertial_position_new() {
2096            let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2097            assert_eq!(inertial_pos.x(), 1.0);
2098            assert_eq!(inertial_pos.y(), 2.0);
2099            assert_eq!(inertial_pos.z(), 3.0);
2100        }
2101
2102        #[test]
2103        fn equality() {
2104            let v = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2105            let w = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2106            assert_eq!(v, w);
2107            let z = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2108            assert_ne!(v, z);
2109        }
2110
2111        #[test]
2112        fn set_x() {
2113            let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2114            inertial_pos.set_x(4.0);
2115            assert_eq!(inertial_pos.x(), 4.0);
2116        }
2117
2118        #[test]
2119        fn set_y() {
2120            let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2121            inertial_pos.set_y(4.0);
2122            assert_eq!(inertial_pos.y(), 4.0);
2123        }
2124
2125        #[test]
2126        fn set_z() {
2127            let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2128            inertial_pos.set_z(4.0);
2129            assert_eq!(inertial_pos.z(), 4.0);
2130        }
2131
2132        #[test]
2133        fn set_xyz() {
2134            use crate::math::Vector3;
2135            let mut inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2136            let vector = Vector3::new(4.0, 5.0, 6.0);
2137            inertial_pos.set_xyz(&vector);
2138            assert_eq!(inertial_pos.x(), 4.0);
2139            assert_eq!(inertial_pos.y(), 5.0);
2140            assert_eq!(inertial_pos.z(), 6.0);
2141        }
2142
2143        #[test]
2144        fn xyz() {
2145            let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2146            assert_eq!(inertial_pos.xyz(), crate::math::Vector3::new(1.0, 2.0, 3.0));
2147        }
2148
2149        #[test]
2150        fn length() {
2151            let inertial_pos = crate::api::InertialPosition::new(3.0, 0.0, 4.0);
2152            assert_eq!(inertial_pos.length(), 5.0);
2153        }
2154
2155        #[test]
2156        fn distance() {
2157            let inertial_pos1 = crate::api::InertialPosition::new(1.0, 1.0, 1.0);
2158            let inertial_pos2 = crate::api::InertialPosition::new(5.0, 1.0, 1.0);
2159            assert_eq!(inertial_pos1.distance(&inertial_pos2), 4.0);
2160        }
2161
2162        #[test]
2163        fn str() {
2164            let inertial_pos = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2165            assert_eq!(inertial_pos.to_string(), "(x = 1, y = 2, z = 3)");
2166        }
2167
2168        #[test]
2169        fn add_operation() {
2170            let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2171            let inertial_pos2 = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2172            let inertial_pos3 = inertial_pos1 + inertial_pos2;
2173            assert_eq!(inertial_pos3.x(), 5.0);
2174            assert_eq!(inertial_pos3.y(), 7.0);
2175            assert_eq!(inertial_pos3.z(), 9.0);
2176        }
2177
2178        #[test]
2179        fn sub_operation() {
2180            let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2181            let inertial_pos2 = crate::api::InertialPosition::new(4.0, 5.0, 6.0);
2182            let inertial_pos3 = inertial_pos1 - inertial_pos2;
2183            assert_eq!(inertial_pos3.x(), -3.0);
2184            assert_eq!(inertial_pos3.y(), -3.0);
2185            assert_eq!(inertial_pos3.z(), -3.0);
2186        }
2187
2188        #[test]
2189        fn mul_scalar_operation() {
2190            let inertial_pos1 = crate::api::InertialPosition::new(1.0, 2.0, 3.0);
2191            let inertial_pos2 = inertial_pos1 * 2.0;
2192            assert_eq!(inertial_pos2.x(), 2.0);
2193            assert_eq!(inertial_pos2.y(), 4.0);
2194            assert_eq!(inertial_pos2.z(), 6.0);
2195        }
2196    }
2197    mod rotation {
2198        #[test]
2199        fn rotation_new() {
2200            let rotation = crate::api::Rotation::new();
2201            assert_eq!(rotation.roll(), 0.0);
2202            assert_eq!(rotation.pitch(), 0.0);
2203            assert_eq!(rotation.yaw(), 0.0);
2204        }
2205
2206        #[test]
2207        fn from_quat() {
2208            let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
2209            let rotation = crate::api::Rotation::from_quat(&quat);
2210            assert_eq!(rotation.roll(), 0.0);
2211            assert_eq!(rotation.pitch(), 0.0);
2212            assert_eq!(rotation.yaw(), 0.0);
2213        }
2214
2215        #[test]
2216        fn from_rpy() {
2217            let rpy = crate::math::RollPitchYaw::new(0.0, 0.0, 0.0);
2218            let rotation = crate::api::Rotation::from_rpy(&rpy);
2219            assert_eq!(rotation.roll(), 0.0);
2220            assert_eq!(rotation.pitch(), 0.0);
2221            assert_eq!(rotation.yaw(), 0.0);
2222        }
2223
2224        #[test]
2225        fn set_quat() {
2226            let mut rotation = crate::api::Rotation::new();
2227            let quat = crate::math::Quaternion::new(1.0, 0.0, 0.0, 0.0);
2228            rotation.set_quat(&quat);
2229            assert_eq!(rotation.roll(), 0.0);
2230            assert_eq!(rotation.pitch(), 0.0);
2231            assert_eq!(rotation.yaw(), 0.0);
2232        }
2233
2234        #[test]
2235        fn matrix() {
2236            let rotation = crate::api::Rotation::new();
2237            let matrix = rotation.matrix();
2238            assert_eq!(matrix.row(0), crate::math::Vector3::new(1.0, 0.0, 0.0));
2239            assert_eq!(matrix.row(1), crate::math::Vector3::new(0.0, 1.0, 0.0));
2240            assert_eq!(matrix.row(2), crate::math::Vector3::new(0.0, 0.0, 1.0));
2241        }
2242    }
2243
2244    mod s_range {
2245        #[test]
2246        fn s_range_new() {
2247            let s_range = crate::api::SRange::new(1.0, 2.0);
2248            assert_eq!(s_range.s0(), 1.0);
2249            assert_eq!(s_range.s1(), 2.0);
2250        }
2251        #[test]
2252        fn s_range_api() {
2253            let s_range_1 = crate::api::SRange::new(1.0, 3.0);
2254            let s_range_2 = crate::api::SRange::new(2.0, 4.0);
2255            assert_eq!(s_range_1.size(), 2.0);
2256            assert!(s_range_1.with_s());
2257            assert!(s_range_1.intersects(&s_range_2, 0.0));
2258            assert!(!s_range_1.contains(&s_range_2, 0.0));
2259        }
2260        #[test]
2261        fn s_range_setters() {
2262            let mut s_range = crate::api::SRange::new(0.0, 4.0);
2263            s_range.set_s0(1.0);
2264            s_range.set_s1(3.0);
2265            assert_eq!(s_range.s0(), 1.0);
2266            assert_eq!(s_range.s1(), 3.0);
2267        }
2268        #[test]
2269        fn s_range_get_intersection_with_intersection() {
2270            let s_range_1 = crate::api::SRange::new(1.0, 3.0);
2271            let s_range_2 = crate::api::SRange::new(2.0, 4.0);
2272            let intersection = s_range_1.get_intersection(&s_range_2, 0.0);
2273            assert!(intersection.is_some());
2274            let intersection = intersection.unwrap();
2275            assert_eq!(intersection.s0(), 2.0);
2276            assert_eq!(intersection.s1(), 3.0);
2277        }
2278        #[test]
2279        fn s_range_get_intersection_with_no_intersection() {
2280            let s_range_1 = crate::api::SRange::new(1.0, 2.0);
2281            let s_range_2 = crate::api::SRange::new(3.0, 4.0);
2282            let intersection = s_range_1.get_intersection(&s_range_2, 0.0);
2283            assert!(intersection.is_none());
2284        }
2285    }
2286
2287    mod lane_s_range {
2288        #[test]
2289        fn lane_s_range_new() {
2290            let lane_s_range =
2291                crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0));
2292            assert_eq!(lane_s_range.lane_id(), "lane_test");
2293            assert_eq!(lane_s_range.s_range().s0(), 1.0);
2294            assert_eq!(lane_s_range.s_range().s1(), 2.0);
2295            assert_eq!(lane_s_range.length(), 1.0);
2296        }
2297        #[test]
2298        fn lane_s_range_api() {
2299            let lane_s_range_1 =
2300                crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 2.0));
2301            let lane_s_range_2 =
2302                crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 3.0));
2303            assert!(lane_s_range_1.intersects(&lane_s_range_2, 0.0));
2304            assert!(!lane_s_range_1.contains(&lane_s_range_2, 0.0));
2305        }
2306        #[test]
2307        fn lane_s_range_get_intersection_with_intersection() {
2308            let lane_s_range_1 =
2309                crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(1.0, 3.0));
2310            let lane_s_range_2 =
2311                crate::api::LaneSRange::new(&String::from("lane_test"), &crate::api::SRange::new(2.0, 4.0));
2312            let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0);
2313            assert!(intersection.is_some());
2314            let intersection = intersection.unwrap();
2315            assert_eq!(intersection.lane_id(), "lane_test");
2316            assert_eq!(intersection.s_range().s0(), 2.0);
2317            assert_eq!(intersection.s_range().s1(), 3.0);
2318        }
2319        #[test]
2320        fn lane_s_range_get_intersection_with_no_intersection() {
2321            let lane_s_range_1 =
2322                crate::api::LaneSRange::new(&String::from("lane test_1"), &crate::api::SRange::new(1.0, 3.0));
2323            let lane_s_range_2 =
2324                crate::api::LaneSRange::new(&String::from("lane_test_2"), &crate::api::SRange::new(2.0, 4.0));
2325            let intersection = lane_s_range_1.get_intersection(&lane_s_range_2, 0.0);
2326            assert!(intersection.is_none());
2327        }
2328    }
2329
2330    mod lane_s_route {
2331        // Helper function to create a LaneSRoute
2332        // with two LaneSRange.
2333        // # Arguments
2334        // * `s0_0` - The s0 of the first LaneSRange.
2335        // * `s1_0` - The s1 of the first LaneSRange.
2336        // * `s0_1` - The s0 of the second LaneSRange.
2337        // * `s1_1` - The s1 of the second LaneSRange.
2338        fn _get_lane_s_route(s0_0: f64, s1_0: f64, s0_1: f64, s1_1: f64) -> crate::api::LaneSRoute {
2339            let ranges = vec![
2340                crate::api::LaneSRange::new(&String::from("lane_test_1"), &crate::api::SRange::new(s0_0, s1_0)),
2341                crate::api::LaneSRange::new(&String::from("lane_test_2"), &crate::api::SRange::new(s0_1, s1_1)),
2342            ];
2343            crate::api::LaneSRoute::new(ranges)
2344        }
2345        #[test]
2346        fn lane_s_route_new() {
2347            let lane_s_route = _get_lane_s_route(0., 10., 0., 15.);
2348            assert!(!lane_s_route.lane_s_route.is_null());
2349            let ranges = lane_s_route.ranges();
2350            assert_eq!(ranges.len(), 2);
2351            assert_eq!(ranges[0].lane_id(), "lane_test_1");
2352            assert_eq!(ranges[1].lane_id(), "lane_test_2");
2353        }
2354        #[test]
2355        fn lane_s_route_length() {
2356            let lane_s_route = _get_lane_s_route(0., 10., 0., 15.);
2357            assert_eq!(lane_s_route.length(), 25.0);
2358        }
2359        #[test]
2360        fn lane_s_route_intersects() {
2361            let lane_s_route = _get_lane_s_route(0., 10., 0., 10.);
2362            let lane_s_route_that_intersects = _get_lane_s_route(5., 9., 5., 9.);
2363            let lane_s_route_that_not_intersects = _get_lane_s_route(11., 20., 11., 20.);
2364            assert!(lane_s_route.intersects(&lane_s_route_that_intersects, 0.0));
2365            assert!(!lane_s_route.intersects(&lane_s_route_that_not_intersects, 0.0));
2366        }
2367    }
2368}