builder: implement orientation according to coordinates

python/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "liblrs_python"
 description = "Python bindings for liblrs: a library to work with linear referencing systems"
-version = "0.1.6"
+version = "0.1.7"
 edition = "2021"
 license = "MIT"
 repository = "https://github.com/osrd-project/liblrs/"
@@ -13,4 +13,4 @@ crate-type = ["cdylib"]
 liblrs = { path = ".." }
 geo-types = "*"
 # "abi3-py38" tells pyo3 (and maturin) to build using the stable ABI with minimum Python version 3.8
-pyo3 = { version = "0.22.1", features = ["abi3-py38"]}
+pyo3 = { version = "0.22.1", features = ["abi3-py38"] }

python/liblrs_python.pyi

@@ -127,6 +127,14 @@ class Builder:
         """
         ...
 
+    def orient_along_points(self, traversal_index:int, first_point:Point, last_point:Point) -> None:
+        r"""Orient the traversal according to two points
+
+        In the end, the first coordinate must be closer to the begining than the second
+        If both points are so far from the curve that they are projected to a end, we consider the offset to the curve
+        """
+        ...
+
 
 class LrmScaleMeasure:
     r"""

python/src/lib.rs

@@ -57,6 +57,15 @@ impl From<geo_types::Point> for Point {
     }
 }
 
+impl From<Point> for geo_types::Point {
+    fn from(val: Point) -> Self {
+        geo_types::point! {
+            x: val.x,
+            y: val.y,
+        }
+    }
+}
+
 impl From<geo_types::Coord> for Point {
     fn from(value: geo_types::Coord) -> Self {
         Self {
@@ -413,7 +422,7 @@ impl Builder {
     /// Return None if the curve of the traversal is not defined
     pub fn project(&self, lrm_index: usize, point: Point) -> Option<f64> {
         self.inner
-            .project(lrm_index, geo_types::point! {x: point.x, y: point.y})
+            .project(lrm_index, point.into())
             .map(|p| p.distance_along_curve)
             .ok()
     }
@@ -424,4 +433,19 @@ impl Builder {
     pub fn reverse(&mut self, lrm_index: usize) {
         self.inner.reverse(lrm_index)
     }
+
+    /// Orient the traversal according to two points
+    ///
+    /// In the end, the first coordinate must be closer to the begining than the second
+    /// If both points are so far from the curve that they are projected to a end, we consider the offset to the curve
+    pub fn orient_along_points(
+        &mut self,
+        traversal_index: usize,
+        first_point: Point,
+        last_point: Point,
+    ) {
+        self.inner
+            .orient_along_points(traversal_index, first_point.into(), last_point.into())
+            .unwrap()
+    }
 }

src/builder.rs

@@ -227,10 +227,10 @@ impl<'fbb> Builder<'fbb> {
         for segment in segments {
             let start_node = self.temp_segments[segment.segment_index].start_node_index as usize;
             let end_node = self.temp_segments[segment.segment_index].end_node_index as usize;
-            if nodes_of_traversal.is_empty() {
-                nodes_of_traversal.push(end_node);
-            }
             if segment.reversed {
+                if nodes_of_traversal.is_empty() {
+                    nodes_of_traversal.push(end_node);
+                }
                 nodes_of_traversal.push(start_node);
                 for &coord in self.temp_segments[segment.segment_index]
                     .geometry
@@ -506,4 +506,112 @@ impl<'fbb> Builder<'fbb> {
     pub fn get_node_coord(&self, node_index: usize) -> Coord {
         self.nodes_coords[node_index]
     }
+
+    /// Orient the traversal according to two points
+    ///
+    /// In the end, the first coordinate must be closer to the begining than the second
+    /// If both points are so far from the curve that they are projected to a end, we consider the offset to the curve
+    pub fn orient_along_points(
+        &mut self,
+        traversal_index: usize,
+        first_point: geo::Point,
+        last_point: geo::Point,
+    ) -> Result<(), CurveError> {
+        let first_projected = self.project(traversal_index, first_point)?;
+        let last_projected = self.project(traversal_index, last_point)?;
+        if (first_projected.distance_along_curve > last_projected.distance_along_curve)
+            || (first_projected.distance_along_curve == last_projected.distance_along_curve
+                && first_projected.offset.abs() > last_projected.offset.abs())
+        {
+            self.reverse(traversal_index);
+        }
+        Ok(())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use geo::{coord, point};
+
+    fn build_traversal(builder: &mut Builder) -> usize {
+        let s1 = builder.add_segment("s1", &[coord! {x: 0., y: 0.}, coord! {x:1., y: 0.}], 0, 1);
+        let s2 = builder.add_segment("s2", &[coord! {x: 1., y: 0.}, coord! {x:2., y: 0.}], 1, 2);
+        let sot1 = super::SegmentOfTraversal {
+            segment_index: s1,
+            reversed: false,
+        };
+        let sot2 = super::SegmentOfTraversal {
+            segment_index: s2,
+            reversed: false,
+        };
+        builder.add_traversal("traversal", &[sot1, sot2])
+    }
+
+    #[test]
+    fn traversal_nodes_order() {
+        // Nominal case
+        let mut b = Builder::new();
+        let traversal = build_traversal(&mut b);
+        assert_eq!(b.nodes_of_traversal[traversal], [0, 1, 2]);
+
+        // One reversed edge
+        let s1 = b.add_segment("s1", &[coord! {x: 0., y: 0.}, coord! {x:1., y: 0.}], 10, 11);
+        let s2 = b.add_segment("s2", &[coord! {x: 1., y: 0.}, coord! {x:2., y: 0.}], 10, 12);
+        let sot1 = super::SegmentOfTraversal {
+            segment_index: s1,
+            reversed: true,
+        };
+        let sot2 = super::SegmentOfTraversal {
+            segment_index: s2,
+            reversed: false,
+        };
+        let traversal = b.add_traversal("traversal", &[sot1, sot2]);
+        assert_eq!(b.nodes_of_traversal[traversal], [11, 10, 12]);
+    }
+
+    #[test]
+    fn orientation_on_curve() {
+        let mut b = Builder::new();
+        let traversal = build_traversal(&mut b);
+
+        let point_a = point! {x: 1., y: 1.};
+        let point_b = point! {x: 2., y: -1.};
+
+        b.orient_along_points(traversal, point_a, point_b).unwrap();
+        assert_eq!(b.temp_traversal[traversal].segments[0].segment_index, 0);
+        assert_eq!(b.nodes_of_traversal[0][0], 0);
+
+        b.orient_along_points(traversal, point_b, point_a).unwrap();
+        assert_eq!(b.temp_traversal[0].segments[0].segment_index, 1);
+        assert_eq!(b.nodes_of_traversal[0][0], 2);
+    }
+
+    #[test]
+    fn orientation_before_curve() {
+        let mut b = Builder::new();
+        let traversal = build_traversal(&mut b);
+
+        // a-b---[the curve]
+        let point_a = point! {x: -1., y: 0.};
+        let point_b = point! {x: -2., y: 0.};
+        b.orient_along_points(traversal, point_a, point_b).unwrap();
+        assert_eq!(b.temp_traversal[traversal].segments[0].segment_index, 0);
+        b.orient_along_points(traversal, point_b, point_a).unwrap();
+        assert_eq!(b.temp_traversal[traversal].segments[0].segment_index, 1);
+    }
+
+    #[test]
+    fn orientation_after_curve() {
+        let mut b = Builder::new();
+        let traversal = build_traversal(&mut b);
+
+        // [the curve]---a-b
+        let point_a = point! {x: 5., y: 0.};
+        let point_b = point! {x: 7., y: 0.};
+        b.orient_along_points(traversal, point_a, point_b).unwrap();
+        assert_eq!(b.temp_traversal[traversal].segments[0].segment_index, 0);
+        b.orient_along_points(traversal, point_b, point_a).unwrap();
+        assert_eq!(b.temp_traversal[traversal].segments[0].segment_index, 1);
+    }
 }