Fix segments ending on limbs

Author: h3ndrk

crates/geometry/src/line.rs

@@ -37,8 +37,13 @@ impl<Frame> Line2<Frame> {
         self.0.y() - (self.0.x() * self.slope())
     }
 
+    pub fn project_onto_segment_in_x_axis(&self, point: Point2<Frame>) -> f32 {
+        let rise = (point.x() - self.0.x()) * self.slope();
+        rise + self.0.y()
+    }
+
     pub fn is_above(&self, point: Point2<Frame>) -> bool {
-        self.signed_distance_to_point(point) >= 0.0
+        point.y() >= self.project_onto_segment_in_x_axis(point)
     }
 
     pub fn signed_distance_to_point(&self, point: Point2<Frame>) -> f32 {

crates/types/src/limb.rs

@@ -4,7 +4,7 @@ use geometry::line::Line;
 use serde::{Deserialize, Serialize};
 
 use coordinate_systems::Pixel;
-use linear_algebra::{point, Point2};
+use linear_algebra::Point2;
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 
 #[derive(
@@ -14,21 +14,35 @@ pub struct Limb {
     pub pixel_polygon: Vec<Point2<Pixel>>,
 }
 
-pub fn is_above_limbs(pixel_position: Point2<Pixel>, projected_limbs: &[Limb]) -> bool {
-    projected_limbs.iter().all(|limb| {
-        match limb.pixel_polygon.as_slice().windows(2).find(|points| {
-            points[0].x() <= pixel_position.x() && points[1].x() >= pixel_position.x()
-        }) {
-            Some(points) => {
-                if points[0].x() == points[1].x() {
-                    return (pixel_position.y()) < f32::min(points[0].y(), points[1].y());
-                }
-
-                // since Y is pointing downwards, "is above" is actually !Line::is_above()
-                !Line(points[0], points[1]).is_above(point![pixel_position.x(), pixel_position.y()])
-            }
-            None => true,
-        }
+pub fn project_onto_limbs(position: Point2<Pixel>, projected_limbs: &[Limb]) -> Option<f32> {
+    projected_limbs
+        .iter()
+        .flat_map(|limb| {
+            limb.pixel_polygon
+                .as_slice()
+                .windows(2)
+                .filter_map(|points| {
+                    let is_outside_of_segment =
+                        position.x() < points[0].x() || position.x() > points[1].x();
+                    if is_outside_of_segment {
+                        return None;
+                    }
+
+                    let is_vertical_segment = points[0].x() == points[1].x();
+                    if is_vertical_segment {
+                        return Some(f32::min(points[0].y(), points[1].y()));
+                    }
+
+                    Some(Line(points[0], points[1]).project_onto_segment_in_x_axis(position))
+                })
+                .min_by(f32::total_cmp)
+        })
+        .min_by(f32::total_cmp)
+}
+
+pub fn is_above_limbs(position: Point2<Pixel>, projected_limbs: &[Limb]) -> bool {
+    project_onto_limbs(position, projected_limbs).map_or(true, |projected_position_y| {
+        position.y() < projected_position_y
     })
 }
 
@@ -38,3 +52,46 @@ pub fn is_above_limbs(pixel_position: Point2<Pixel>, projected_limbs: &[Limb]) -
 pub struct ProjectedLimbs {
     pub limbs: Vec<Limb>,
 }
+
+#[cfg(test)]
+mod tests {
+    use linear_algebra::point;
+
+    use super::*;
+
+    #[test]
+    fn left_limb_is_ignored() {
+        let position = point![2.0, 0.0];
+        let projected_limbs = vec![Limb {
+            pixel_polygon: vec![point![0.0, 0.0], point![1.0, 1.0]],
+        }];
+        assert!(is_above_limbs(position, &projected_limbs));
+    }
+
+    #[test]
+    fn right_limb_is_ignored() {
+        let position = point![2.0, 0.0];
+        let projected_limbs = vec![Limb {
+            pixel_polygon: vec![point![3.0, 0.0], point![4.0, 1.0]],
+        }];
+        assert!(is_above_limbs(position, &projected_limbs));
+    }
+
+    #[test]
+    fn too_high_limb_leads_to_point_being_below() {
+        let position = point![2.0, 0.0];
+        let projected_limbs = vec![Limb {
+            pixel_polygon: vec![point![1.0, 10.0], point![3.0, 11.0]],
+        }];
+        assert!(is_above_limbs(position, &projected_limbs));
+    }
+
+    #[test]
+    fn low_limb_leads_to_point_being_above() {
+        let position = point![2.0, 10.0];
+        let projected_limbs = vec![Limb {
+            pixel_polygon: vec![point![1.0, 0.0], point![3.0, 1.0]],
+        }];
+        assert!(!is_above_limbs(position, &projected_limbs));
+    }
+}

crates/vision/src/image_segmenter.rs

@@ -16,7 +16,8 @@ use types::{
     color::{Hsv, Intensity, RgChromaticity, Rgb, YCbCr444},
     field_color::FieldColorParameters,
     image_segments::{Direction, EdgeType, ImageSegments, ScanGrid, ScanLine, Segment},
-    limb::{is_above_limbs, Limb, ProjectedLimbs},
+    limb::project_onto_limbs,
+    limb::{Limb, ProjectedLimbs},
     parameters::{EdgeDetectionSourceParameters, MedianModeParameters},
     ycbcr422_image::YCbCr422Image,
 };
@@ -386,14 +387,18 @@ fn new_vertical_scan_line(
         if let Some(segment) =
             detect_edge(&mut state, y as u16, edge_detection_value, edge_threshold)
         {
-            if segment_is_below_limbs(position as u16, &segment, projected_limbs) {
-                fix_previous_edge_type(&mut segments);
-                break;
-            }
             segments.push(set_field_color_in_segment(
                 set_color_in_segment(segment, position, Direction::Vertical, image),
                 field_color,
             ));
+            let projected_y_on_limb =
+                project_onto_limbs(point![position as f32, segment.end as f32], projected_limbs);
+            let is_below_limbs = projected_y_on_limb
+                .is_some_and(|projected_y_on_limb| segment.end as f32 > projected_y_on_limb);
+            if is_below_limbs {
+                fix_previous_segment(&mut segments, projected_y_on_limb.unwrap() as u16);
+                break;
+            }
         }
     }
 
@@ -405,11 +410,23 @@ fn new_vertical_scan_line(
         color: Default::default(),
         field_color: Intensity::Low,
     };
-    if !segment_is_below_limbs(position as u16, &last_segment, projected_limbs) {
+
+    if segments.last().map_or(true, |segment| {
+        segment.end_edge_type != EdgeType::LimbBorder
+    }) {
         segments.push(set_field_color_in_segment(
             set_color_in_segment(last_segment, position, Direction::Vertical, image),
             field_color,
         ));
+        let projected_y_on_limb = project_onto_limbs(
+            point![position as f32, last_segment.end as f32],
+            projected_limbs,
+        );
+        let is_below_limbs = projected_y_on_limb
+            .is_some_and(|projected_y_on_limb| last_segment.end as f32 > projected_y_on_limb);
+        if is_below_limbs {
+            fix_previous_segment(&mut segments, projected_y_on_limb.unwrap() as u16);
+        }
     }
 
     ScanLine {
@@ -519,20 +536,10 @@ fn average_image_pixels(
     sum.average()
 }
 
-fn segment_is_below_limbs(
-    scan_line_position: u16,
-    segment: &Segment,
-    projected_limbs: &[Limb],
-) -> bool {
-    !is_above_limbs(
-        point![scan_line_position as f32, segment.end as f32],
-        projected_limbs,
-    )
-}
-
-fn fix_previous_edge_type(segments: &mut [Segment]) {
+fn fix_previous_segment(segments: &mut [Segment], y_on_limb: u16) {
     if let Some(previous_segment) = segments.last_mut() {
         previous_segment.end_edge_type = EdgeType::LimbBorder;
+        previous_segment.end = y_on_limb;
     }
 }