diff --git a/Cargo.lock b/Cargo.lock index 2dc3e54396..434f66c317 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -917,6 +917,8 @@ dependencies = [ "linear_algebra", "nalgebra", "projection", + "serde", + "serialize_hierarchy", "thiserror", "types", ] diff --git a/crates/calibration/Cargo.toml b/crates/calibration/Cargo.toml index f739c86f2e..9dbdd054b9 100644 --- a/crates/calibration/Cargo.toml +++ b/crates/calibration/Cargo.toml @@ -12,5 +12,7 @@ levenberg-marquardt = { workspace = true } linear_algebra = { workspace = true } nalgebra = { workspace = true } projection = { workspace = true } +serde = { workspace = true } +serialize_hierarchy = { workspace = true } thiserror = { workspace = true } types = { workspace = true } diff --git a/crates/calibration/src/lines.rs b/crates/calibration/src/lines.rs index cd7c6a2d75..f8d7b2e80d 100644 --- a/crates/calibration/src/lines.rs +++ b/crates/calibration/src/lines.rs @@ -2,9 +2,11 @@ use coordinate_systems::{Ground, Pixel}; use geometry::line::{Line, Line2}; use linear_algebra::Point2; use projection::Projection; +use serde::{Deserialize, Serialize}; +use serialize_hierarchy::SerializeHierarchy; use types::camera_matrix::CameraMatrix; -#[derive(Clone)] +#[derive(Clone, Default, Debug, Deserialize, Serialize, SerializeHierarchy)] pub struct Lines { pub border_line: Line2, pub goal_box_line: Line2, @@ -39,9 +41,9 @@ fn project_line_and_map_error( line: Line2, which: &str, ) -> Result, LinesError> { - Ok(Line( - project_point_and_map_error(matrix, line.0, format!("{which} point 0"))?, - project_point_and_map_error(matrix, line.1, format!("{which} point 1"))?, + Ok(Line::new( + project_point_and_map_error(matrix, line.first, format!("{which} point 0"))?, + project_point_and_map_error(matrix, line.second, format!("{which} point 1"))?, )) } diff --git a/crates/calibration/src/measurement.rs b/crates/calibration/src/measurement.rs index 5e9cf83bb7..362e606fb2 100644 --- a/crates/calibration/src/measurement.rs +++ b/crates/calibration/src/measurement.rs @@ -1,9 +1,11 @@ use coordinate_systems::Pixel; +use serde::{Deserialize, Serialize}; +use serialize_hierarchy::SerializeHierarchy; use types::{camera_matrix::CameraMatrix, camera_position::CameraPosition}; use crate::lines::Lines; -#[derive(Clone)] +#[derive(Clone, Debug, Default, Deserialize, Serialize, SerializeHierarchy)] pub struct Measurement { pub position: CameraPosition, pub matrix: CameraMatrix, diff --git a/crates/calibration/src/residuals.rs b/crates/calibration/src/residuals.rs index 9d6a7105a9..6af27299da 100644 --- a/crates/calibration/src/residuals.rs +++ b/crates/calibration/src/residuals.rs @@ -58,13 +58,13 @@ impl Residuals { .signed_acute_angle_to_orthogonal(projected_lines.connecting_line); let distance_between_parallel_line_start_points = projected_lines .border_line - .distance_to_point(projected_lines.goal_box_line.0); + .distance_to_point(projected_lines.goal_box_line.first); let distance_between_parallel_line_center_points = projected_lines .border_line .distance_to_point(projected_lines.goal_box_line.center()); let distance_between_parallel_line_end_points = projected_lines .border_line - .distance_to_point(projected_lines.goal_box_line.1); + .distance_to_point(projected_lines.goal_box_line.second); Ok(Residuals { border_to_connecting_angle, diff --git a/crates/control/src/behavior/defend.rs b/crates/control/src/behavior/defend.rs index 1a0e9f9d7a..2fb8623ed9 100644 --- a/crates/control/src/behavior/defend.rs +++ b/crates/control/src/behavior/defend.rs @@ -266,11 +266,11 @@ fn block_on_line( ) -> Pose2 { let is_ball_in_front_of_defense_line = defense_line_x < ball_position.x(); if is_ball_in_front_of_defense_line { - let defense_line = Line( + let defense_line = Line::new( point![defense_line_x, defense_line_y_range.start], point![defense_line_x, defense_line_y_range.end], ); - let ball_target_line = Line(ball_position, target); + let ball_target_line = Line::new(ball_position, target); let intersection_point = defense_line.intersection(&ball_target_line); let defense_position = point![ intersection_point.x(), diff --git a/crates/control/src/behavior/intercept_ball.rs b/crates/control/src/behavior/intercept_ball.rs index f57b0a99a9..3a400b575b 100644 --- a/crates/control/src/behavior/intercept_ball.rs +++ b/crates/control/src/behavior/intercept_ball.rs @@ -62,7 +62,7 @@ pub fn execute( return None; } - let ball_line = Line( + let ball_line = Line::new( ball.ball_in_ground, ball.ball_in_ground + ball.ball_in_ground_velocity, ); diff --git a/crates/control/src/camera_matrix_calculator.rs b/crates/control/src/camera_matrix_calculator.rs index 5bb205cb53..1aba10a960 100644 --- a/crates/control/src/camera_matrix_calculator.rs +++ b/crates/control/src/camera_matrix_calculator.rs @@ -164,10 +164,10 @@ fn project_penalty_area_on_images( .ok()?; Some(vec![ - Line(penalty_top_left, penalty_top_right), - Line(penalty_bottom_left, penalty_bottom_right), - Line(penalty_bottom_left, penalty_top_left), - Line(penalty_bottom_right, penalty_top_right), - Line(corner_left, corner_right), + Line::new(penalty_top_left, penalty_top_right), + Line::new(penalty_bottom_left, penalty_bottom_right), + Line::new(penalty_bottom_left, penalty_top_left), + Line::new(penalty_bottom_right, penalty_top_right), + Line::new(corner_left, corner_right), ]) } diff --git a/crates/control/src/localization.rs b/crates/control/src/localization.rs index 00b187f8c9..0b3f7fdff8 100644 --- a/crates/control/src/localization.rs +++ b/crates/control/src/localization.rs @@ -322,11 +322,11 @@ impl Localization { let correspondence_points_1 = field_mark_correspondence.correspondence_points.1; [ - Line( + Line::new( correspondence_points_0.measured, correspondence_points_0.reference, ), - Line( + Line::new( correspondence_points_1.measured, correspondence_points_1.reference, ), @@ -541,7 +541,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( let goal_depth = field_dimensions.goal_depth; vec![ FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 - goal_depth, -goal_width / 2.0 @@ -554,7 +554,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 - goal_depth, -goal_width / 2.0 @@ -564,7 +564,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 - goal_depth, goal_width / 2.0 @@ -574,7 +574,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 + goal_depth, -goal_width / 2.0 @@ -584,7 +584,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![field_dimensions.length / 2.0, -goal_width / 2.0], point![ field_dimensions.length / 2.0 + goal_depth, @@ -594,7 +594,7 @@ pub fn goal_support_structure_line_marks_from_field_dimensions( direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![field_dimensions.length / 2.0, goal_width / 2.0], point![field_dimensions.length / 2.0 + goal_depth, goal_width / 2.0], ), @@ -889,39 +889,40 @@ fn get_translation_and_rotation_measurement( }; let measured_line_in_field = match field_mark_line_direction { Direction::PositiveX - if field_mark_correspondence.measured_line_in_field.1.x() - < field_mark_correspondence.measured_line_in_field.0.x() => + if field_mark_correspondence.measured_line_in_field.second.x() + < field_mark_correspondence.measured_line_in_field.first.x() => { - Line( - field_mark_correspondence.measured_line_in_field.1, - field_mark_correspondence.measured_line_in_field.0, + Line::new( + field_mark_correspondence.measured_line_in_field.second, + field_mark_correspondence.measured_line_in_field.first, ) } Direction::PositiveY - if field_mark_correspondence.measured_line_in_field.1.y() - < field_mark_correspondence.measured_line_in_field.0.y() => + if field_mark_correspondence.measured_line_in_field.second.y() + < field_mark_correspondence.measured_line_in_field.first.y() => { - Line( - field_mark_correspondence.measured_line_in_field.1, - field_mark_correspondence.measured_line_in_field.0, + Line::new( + field_mark_correspondence.measured_line_in_field.second, + field_mark_correspondence.measured_line_in_field.first, ) } _ => field_mark_correspondence.measured_line_in_field, }; - let measured_line_in_field_vector = measured_line_in_field.1 - measured_line_in_field.0; + let measured_line_in_field_vector = + measured_line_in_field.second - measured_line_in_field.first; let signed_distance_to_line = measured_line_in_field.signed_distance_to_point(ground_to_field.as_pose().position()); match field_mark_line_direction { Direction::PositiveX => { nalgebra::vector![ - field_mark_line.0.y() + signed_distance_to_line, + field_mark_line.first.y() + signed_distance_to_line, (-measured_line_in_field_vector.y()).atan2(measured_line_in_field_vector.x()) + ground_to_field.orientation().angle() ] } Direction::PositiveY => { nalgebra::vector![ - field_mark_line.0.x() - signed_distance_to_line, + field_mark_line.first.x() - signed_distance_to_line, measured_line_in_field_vector .x() .atan2(measured_line_in_field_vector.y()) @@ -1024,9 +1025,9 @@ mod tests { fn fitting_line_results_in_zero_measurement() { let ground_to_field = Isometry2::identity(); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![0.0, 0.0], point![0.0, 1.0]), + measured_line_in_field: Line::new(point![0.0, 0.0], point![0.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1045,9 +1046,9 @@ mod tests { assert_relative_eq!(update, Vector2::zeros()); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![0.0, 1.0], point![0.0, 0.0]), + measured_line_in_field: Line::new(point![0.0, 1.0], point![0.0, 0.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1066,9 +1067,9 @@ mod tests { assert_relative_eq!(update, Vector2::zeros()); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![0.0, 0.0], point![1.0, 0.0]), + measured_line_in_field: Line::new(point![0.0, 0.0], point![1.0, 0.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1087,9 +1088,9 @@ mod tests { assert_relative_eq!(update, Vector2::zeros()); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 0.0], point![0.0, 0.0]), + measured_line_in_field: Line::new(point![1.0, 0.0], point![0.0, 0.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1112,9 +1113,9 @@ mod tests { fn translated_line_results_in_translation_measurement() { let ground_to_field = Isometry2::identity(); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 0.0], point![1.0, 1.0]), + measured_line_in_field: Line::new(point![1.0, 0.0], point![1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1133,9 +1134,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 1.0], point![1.0, 0.0]), + measured_line_in_field: Line::new(point![1.0, 1.0], point![1.0, 0.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1154,9 +1155,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, 0.0], point![-1.0, 1.0]), + measured_line_in_field: Line::new(point![-1.0, 0.0], point![-1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1175,9 +1176,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, 1.0], point![-1.0, 0.0]), + measured_line_in_field: Line::new(point![-1.0, 1.0], point![-1.0, 0.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1196,9 +1197,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![0.0, 1.0], point![1.0, 1.0]), + measured_line_in_field: Line::new(point![0.0, 1.0], point![1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1217,9 +1218,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 1.0], point![0.0, 1.0]), + measured_line_in_field: Line::new(point![1.0, 1.0], point![0.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1238,9 +1239,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![0.0, -1.0], point![1.0, -1.0]), + measured_line_in_field: Line::new(point![0.0, -1.0], point![1.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1259,9 +1260,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![1.0, 0.0]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, -1.0], point![0.0, -1.0]), + measured_line_in_field: Line::new(point![1.0, -1.0], point![0.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1284,9 +1285,9 @@ mod tests { fn rotated_line_results_in_rotation_measurement() { let ground_to_field = Isometry2::identity(); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, -1.0], point![1.0, 1.0]), + measured_line_in_field: Line::new(point![-1.0, -1.0], point![1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1305,9 +1306,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 1.0], point![-1.0, -1.0]), + measured_line_in_field: Line::new(point![1.0, 1.0], point![-1.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1326,9 +1327,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, 1.0], point![1.0, -1.0]), + measured_line_in_field: Line::new(point![-1.0, 1.0], point![1.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1347,9 +1348,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, -FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, -1.0], point![-1.0, 1.0]), + measured_line_in_field: Line::new(point![1.0, -1.0], point![-1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![0.0, -3.0], point![0.0, 3.0]), + line: Line::new(point![0.0, -3.0], point![0.0, 3.0]), direction: Direction::PositiveY, }, correspondence_points: ( @@ -1368,9 +1369,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, -FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, -1.0], point![1.0, 1.0]), + measured_line_in_field: Line::new(point![-1.0, -1.0], point![1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1389,9 +1390,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, -FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, 1.0], point![-1.0, -1.0]), + measured_line_in_field: Line::new(point![1.0, 1.0], point![-1.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1410,9 +1411,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, -FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![-1.0, 1.0], point![1.0, -1.0]), + measured_line_in_field: Line::new(point![-1.0, 1.0], point![1.0, -1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1431,9 +1432,9 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, FRAC_PI_4]); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(point![1.0, -1.0], point![-1.0, 1.0]), + measured_line_in_field: Line::new(point![1.0, -1.0], point![-1.0, 1.0]), field_mark: FieldMark::Line { - line: Line(point![-3.0, 0.0], point![3.0, 0.0]), + line: Line::new(point![-3.0, 0.0], point![3.0, 0.0]), direction: Direction::PositiveX, }, correspondence_points: ( @@ -1456,9 +1457,9 @@ mod tests { fn correct_correspondence_points() { let line_length_acceptance_factor = 1.5; - let measured_lines_in_field = [Line(point![0.0, 0.0], point![1.0, 0.0])]; + let measured_lines_in_field = [Line::new(point![0.0, 0.0], point![1.0, 0.0])]; let field_marks = [FieldMark::Line { - line: Line(point![0.0, 0.0], point![1.0, 0.0]), + line: Line::new(point![0.0, 0.0], point![1.0, 0.0]), direction: Direction::PositiveX, }]; let correspondences = get_field_mark_correspondence( @@ -1485,9 +1486,9 @@ mod tests { point![1.0, 0.0] ); - let measured_lines_in_field = [Line(point![0.0, 0.0], point![1.0, 0.0])]; + let measured_lines_in_field = [Line::new(point![0.0, 0.0], point![1.0, 0.0])]; let field_marks = [FieldMark::Line { - line: Line(point![0.0, 1.0], point![1.0, 1.0]), + line: Line::new(point![0.0, 1.0], point![1.0, 1.0]), direction: Direction::PositiveX, }]; let correspondences = get_field_mark_correspondence( @@ -1514,9 +1515,9 @@ mod tests { point![1.0, 1.0] ); - let measured_lines_in_field = [Line(point![0.0, 0.0], point![1.0, 0.0])]; + let measured_lines_in_field = [Line::new(point![0.0, 0.0], point![1.0, 0.0])]; let field_marks = [FieldMark::Line { - line: Line(point![0.0, 0.0], point![1.0, 0.0]), + line: Line::new(point![0.0, 0.0], point![1.0, 0.0]), direction: Direction::PositiveX, }]; let correspondences = get_field_mark_correspondence( @@ -1548,7 +1549,7 @@ mod tests { fn circle_mark_correspondence_translates() { let ground_to_field = Isometry2::identity(); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1568,7 +1569,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, 0.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1588,7 +1589,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, -1.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1608,7 +1609,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![0.0, 1.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1628,7 +1629,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, 0.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1648,7 +1649,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![1.0, 0.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, @@ -1668,7 +1669,7 @@ mod tests { assert_relative_eq!(update, nalgebra::vector![-1.0, -1.0], epsilon = 0.0001); let field_mark_correspondence = FieldMarkCorrespondence { - measured_line_in_field: Line(Point2::origin(), Point2::origin()), + measured_line_in_field: Line::new(Point2::origin(), Point2::origin()), field_mark: FieldMark::Circle { center: Point2::origin(), radius: 0.0, diff --git a/crates/geometry/src/line.rs b/crates/geometry/src/line.rs index e1f7dd2978..8ffc96e21e 100644 --- a/crates/geometry/src/line.rs +++ b/crates/geometry/src/line.rs @@ -8,30 +8,38 @@ use linear_algebra::{ center, distance, distance_squared, point, vector, Point, Point2, Rotation2, Transform, Vector2, }; use serde::{Deserialize, Serialize}; +use serialize_hierarchy::SerializeHierarchy; -#[derive(Copy, Clone, Debug, Deserialize, Serialize)] -pub struct Line( - pub Point, - pub Point, -); +#[derive(Copy, Clone, Default, Debug, Deserialize, Serialize, SerializeHierarchy)] +#[serde(bound = "")] +pub struct Line { + pub first: Point, + pub second: Point, +} + +impl Line { + pub fn new(first: Point, second: Point) -> Self { + Self { first, second } + } +} pub type Line2 = Line; pub type Line3 = Line; impl Line2 { pub fn signed_acute_angle(&self, other: Self) -> f32 { - let self_direction = self.1 - self.0; - let other_direction = other.1 - other.0; + let self_direction = self.second - self.first; + let other_direction = other.second - other.first; signed_acute_angle(self_direction, other_direction) } pub fn angle(&self, other: Self) -> f32 { - (self.1 - self.0).angle(other.1 - other.0) + (self.second - self.first).angle(other.second - other.first) } pub fn signed_acute_angle_to_orthogonal(&self, other: Self) -> f32 { - let self_direction = self.1 - self.0; - let other_direction = other.1 - other.0; + let self_direction = self.second - self.first; + let other_direction = other.second - other.first; let orthogonal_other_direction = vector![other_direction.y(), -other_direction.x()]; signed_acute_angle(self_direction, orthogonal_other_direction) } @@ -41,47 +49,47 @@ impl Line2 { } pub fn slope(&self) -> f32 { - let difference = self.0 - self.1; + let difference = self.first - self.second; difference.y() / difference.x() } pub fn y_axis_intercept(&self) -> f32 { - self.0.y() - (self.0.x() * self.slope()) + self.first.y() - (self.first.x() * self.slope()) } pub fn is_above(&self, point: Point2) -> bool { - let rise = (point.x() - self.0.x()) * self.slope(); - point.y() >= rise + self.0.y() + let rise = (point.x() - self.first.x()) * self.slope(); + point.y() >= rise + self.first.y() } pub fn signed_distance_to_point(&self, point: Point2) -> f32 { - let line_vector = self.1 - self.0; + let line_vector = self.second - self.first; let normal_vector = vector![-line_vector.y(), line_vector.x()].normalize(); - normal_vector.dot(point.coords()) - normal_vector.dot(self.0.coords()) + normal_vector.dot(point.coords()) - normal_vector.dot(self.first.coords()) } pub fn project_onto_segment(&self, point: Point2) -> Point2 { - let difference_on_line = self.1 - self.0; - let difference_to_point = point - self.0; + let difference_on_line = self.second - self.first; + let difference_to_point = point - self.first; let t = difference_to_point.dot(difference_on_line) / difference_on_line.norm_squared(); if t <= 0.0 { - self.0 + self.first } else if t >= 1.0 { - self.1 + self.second } else { - self.0 + difference_on_line * t + self.first + difference_on_line * t } } pub fn intersection(&self, other: &Line2) -> Point2 { - let x1 = self.0.x(); - let y1 = self.0.y(); - let x2 = self.1.x(); - let y2 = self.1.y(); - let x3 = other.0.x(); - let y3 = other.0.y(); - let x4 = other.1.x(); - let y4 = other.1.y(); + let x1 = self.first.x(); + let y1 = self.first.y(); + let x2 = self.second.x(); + let y2 = self.second.y(); + let x3 = other.first.x(); + let y3 = other.first.y(); + let x4 = other.second.x(); + let y4 = other.second.y(); point!( ((((x1 * y2) - (y1 * x2)) * (x3 - x4)) - ((x1 - x2) * ((x3 * y4) - (y3 * x4)))) @@ -105,23 +113,23 @@ fn signed_acute_angle(first: Vector2, second: Vector2) -> f impl Line { pub fn project_point(&self, point: Point) -> Point { - let difference_on_line = self.1 - self.0; - let difference_to_point = point - self.0; - self.0 + let difference_on_line = self.second - self.first; + let difference_to_point = point - self.first; + self.first + (difference_on_line * difference_on_line.dot(difference_to_point) / difference_on_line.norm_squared()) } pub fn squared_distance_to_segment(&self, point: Point) -> f32 { - let difference_on_line = self.1 - self.0; - let difference_to_point = point - self.0; + let difference_on_line = self.second - self.first; + let difference_to_point = point - self.first; let t = difference_to_point.dot(difference_on_line) / difference_on_line.norm_squared(); if t <= 0.0 { - (point - self.0).norm_squared() + (point - self.first).norm_squared() } else if t >= 1.0 { - (point - self.1).norm_squared() + (point - self.second).norm_squared() } else { - (point - (self.0 + difference_on_line * t)).norm_squared() + (point - (self.first + difference_on_line * t)).norm_squared() } } @@ -135,11 +143,11 @@ impl Line { } pub fn length(&self) -> f32 { - distance(self.0, self.1) + distance(self.first, self.second) } pub fn center(&self) -> Point { - center(self.0, self.1) + center(self.first, self.second) } } @@ -151,13 +159,17 @@ where type Output = Line; fn mul(self, right: Line) -> Self::Output { - Line(self * right.0, self * right.1) + Line { + first: self * right.first, + second: self * right.second, + } } } impl PartialEq for Line { fn eq(&self, other: &Self) -> bool { - (self.0 == other.0 && self.1 == other.1) || (self.0 == other.1 && self.1 == other.0) + (self.first == other.first && self.second == other.second) + || (self.first == other.second && self.second == other.first) } } @@ -169,7 +181,8 @@ impl AbsDiffEq for Line { } fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - self.0.abs_diff_eq(&other.0, epsilon) && self.1.abs_diff_eq(&other.1, epsilon) + self.first.abs_diff_eq(&other.first, epsilon) + && self.second.abs_diff_eq(&other.second, epsilon) } } @@ -184,8 +197,10 @@ impl RelativeEq for Line { epsilon: Self::Epsilon, max_relative: Self::Epsilon, ) -> bool { - self.0.relative_eq(&other.0, epsilon, max_relative) - && self.1.relative_eq(&other.1, epsilon, max_relative) + self.first.relative_eq(&other.first, epsilon, max_relative) + && self + .second + .relative_eq(&other.second, epsilon, max_relative) } } @@ -213,69 +228,69 @@ mod tests { let sixty_degree = 60.0_f32.to_radians(); let cases = [ Case { - self_line: Line(point![0.0, 0.0], point![42.0, 0.0]), - other_line: Line(point![0.0, 0.0], point![42.0, 0.0]), + self_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), + other_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), expected_angle: 0.0, }, Case { - self_line: Line(point![0.0, 0.0], point![42.0, 0.0]), - other_line: Line(point![0.0, 0.0], point![42.0, 42.0]), + self_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), + other_line: Line::new(point![0.0, 0.0], point![42.0, 42.0]), expected_angle: FRAC_PI_4, }, Case { - self_line: Line(point![0.0, 0.0], point![42.0, 42.0]), - other_line: Line(point![0.0, 0.0], point![42.0, 0.0]), + self_line: Line::new(point![0.0, 0.0], point![42.0, 42.0]), + other_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), expected_angle: -FRAC_PI_4, }, Case { - self_line: Line(point![0.0, 0.0], point![42.0, 0.0]), - other_line: Line(point![0.0, 0.0], point![42.0, -42.0]), + self_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), + other_line: Line::new(point![0.0, 0.0], point![42.0, -42.0]), expected_angle: -FRAC_PI_4, }, Case { - self_line: Line(point![0.0, 0.0], point![42.0, -42.0]), - other_line: Line(point![0.0, 0.0], point![42.0, 0.0]), + self_line: Line::new(point![0.0, 0.0], point![42.0, -42.0]), + other_line: Line::new(point![0.0, 0.0], point![42.0, 0.0]), expected_angle: FRAC_PI_4, }, Case { - self_line: Line( + self_line: Line::new( point![0.0, 0.0], point![(-thirty_degree).cos(), (-thirty_degree).sin()], ), - other_line: Line( + other_line: Line::new( point![0.0, 0.0], point![thirty_degree.cos(), thirty_degree.sin()], ), expected_angle: sixty_degree, }, Case { - self_line: Line( + self_line: Line::new( point![0.0, 0.0], point![thirty_degree.cos(), thirty_degree.sin()], ), - other_line: Line( + other_line: Line::new( point![0.0, 0.0], point![(-thirty_degree).cos(), (-thirty_degree).sin()], ), expected_angle: -sixty_degree, }, Case { - self_line: Line( + self_line: Line::new( point![0.0, 0.0], point![(-sixty_degree).cos(), (-sixty_degree).sin()], ), - other_line: Line( + other_line: Line::new( point![0.0, 0.0], point![sixty_degree.cos(), sixty_degree.sin()], ), expected_angle: -sixty_degree, }, Case { - self_line: Line( + self_line: Line::new( point![0.0, 0.0], point![sixty_degree.cos(), sixty_degree.sin()], ), - other_line: Line( + other_line: Line::new( point![0.0, 0.0], point![(-sixty_degree).cos(), (-sixty_degree).sin()], ), @@ -291,18 +306,18 @@ mod tests { expected_angle: case.expected_angle, }, Case { - self_line: Line(case.self_line.1, case.self_line.0), + self_line: Line::new(case.self_line.second, case.self_line.first), other_line: case.other_line, expected_angle: case.expected_angle, }, Case { self_line: case.self_line, - other_line: Line(case.other_line.1, case.other_line.0), + other_line: Line::new(case.other_line.second, case.other_line.first), expected_angle: case.expected_angle, }, Case { - self_line: Line(case.self_line.1, case.self_line.0), - other_line: Line(case.other_line.1, case.other_line.0), + self_line: Line::new(case.self_line.second, case.self_line.first), + other_line: Line::new(case.other_line.second, case.other_line.first), expected_angle: case.expected_angle, }, ] diff --git a/crates/types/src/field_marks.rs b/crates/types/src/field_marks.rs index 012a619e0c..a27c5aff0a 100644 --- a/crates/types/src/field_marks.rs +++ b/crates/types/src/field_marks.rs @@ -33,10 +33,10 @@ impl FieldMark { direction: _, } => { let measured_line = match [ - distance(measured_line.0, reference_line.0), - distance(measured_line.0, reference_line.1), - distance(measured_line.1, reference_line.0), - distance(measured_line.1, reference_line.1), + distance(measured_line.first, reference_line.first), + distance(measured_line.first, reference_line.second), + distance(measured_line.second, reference_line.first), + distance(measured_line.second, reference_line.second), ] .iter() .enumerate() @@ -44,51 +44,52 @@ impl FieldMark { .unwrap() .0 { - 1 | 2 => Line(measured_line.1, measured_line.0), + 1 | 2 => Line::new(measured_line.second, measured_line.first), _ => measured_line, }; - let measured_direction = (measured_line.0 - measured_line.1).normalize(); - let reference_direction = (reference_line.0 - reference_line.1).normalize(); + let measured_direction = (measured_line.first - measured_line.second).normalize(); + let reference_direction = + (reference_line.first - reference_line.second).normalize(); let projected_point_on_measured_line = - measured_line.project_onto_segment(reference_line.0); + measured_line.project_onto_segment(reference_line.first); let projected_point_on_reference_line = - reference_line.project_onto_segment(measured_line.0); + reference_line.project_onto_segment(measured_line.first); let measured_distance = - distance(projected_point_on_measured_line, reference_line.0); + distance(projected_point_on_measured_line, reference_line.first); let reference_distance = - distance(measured_line.0, projected_point_on_reference_line); + distance(measured_line.first, projected_point_on_reference_line); let correspondence_0 = if measured_distance < reference_distance { CorrespondencePoints { measured: projected_point_on_measured_line, - reference: reference_line.0, + reference: reference_line.first, } } else { CorrespondencePoints { - measured: measured_line.0, + measured: measured_line.first, reference: projected_point_on_reference_line, } }; let projected_point_on_measured_line = - measured_line.project_onto_segment(reference_line.1); + measured_line.project_onto_segment(reference_line.second); let projected_point_on_reference_line = - reference_line.project_onto_segment(measured_line.1); + reference_line.project_onto_segment(measured_line.second); let measured_distance = - distance(projected_point_on_measured_line, reference_line.1); + distance(projected_point_on_measured_line, reference_line.second); let reference_distance = - distance(measured_line.1, projected_point_on_reference_line); + distance(measured_line.second, projected_point_on_reference_line); let correspondence_1 = if measured_distance < reference_distance { CorrespondencePoints { measured: projected_point_on_measured_line, - reference: reference_line.1, + reference: reference_line.second, } } else { CorrespondencePoints { - measured: measured_line.1, + measured: measured_line.second, reference: projected_point_on_reference_line, } }; @@ -100,24 +101,24 @@ impl FieldMark { } } FieldMark::Circle { center, radius } => { - let center_to_0 = measured_line.0 - center; - let center_to_1 = measured_line.1 - center; + let center_to_0 = measured_line.first - center; + let center_to_1 = measured_line.second - center; - let correspondence_0_measured = measured_line.0; + let correspondence_0_measured = measured_line.first; let correspondence_0_reference = if center_to_0 == Vector2::zeros() { point![center.x() + radius, center.y()] } else { center + center_to_0.normalize() * radius }; - let correspondence_1_measured = measured_line.1; + let correspondence_1_measured = measured_line.second; let correspondence_1_reference = if center_to_1 == Vector2::zeros() { point![center.x() + radius, center.y()] } else { center + center_to_1.normalize() * radius }; - let measured_direction = (measured_line.0 - measured_line.1).normalize(); + let measured_direction = (measured_line.first - measured_line.second).normalize(); let center_vector = (correspondence_0_reference - center) + (correspondence_1_reference - center); let center_vector_rotated_by_90_degree = @@ -159,14 +160,14 @@ pub struct CorrespondencePoints { pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> Vec { vec![ FieldMark::Line { - line: Line( + line: Line::new( point![-field_dimensions.length / 2.0, field_dimensions.width / 2.0], point![field_dimensions.length / 2.0, field_dimensions.width / 2.0], ), direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, -field_dimensions.width / 2.0 @@ -176,7 +177,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, -field_dimensions.width / 2.0 @@ -186,14 +187,14 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![field_dimensions.length / 2.0, -field_dimensions.width / 2.0], point![field_dimensions.length / 2.0, field_dimensions.width / 2.0], ), direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, field_dimensions.penalty_area_width / 2.0 @@ -206,7 +207,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, -field_dimensions.penalty_area_width / 2.0 @@ -219,7 +220,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 + field_dimensions.penalty_area_length, -field_dimensions.penalty_area_width / 2.0 @@ -232,7 +233,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, field_dimensions.goal_box_area_width / 2.0 @@ -245,7 +246,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0, -field_dimensions.goal_box_area_width / 2.0 @@ -258,7 +259,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 + field_dimensions.goal_box_area_length, -field_dimensions.goal_box_area_width / 2.0 @@ -271,7 +272,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.penalty_area_length, field_dimensions.penalty_area_width / 2.0 @@ -284,7 +285,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.penalty_area_length, -field_dimensions.penalty_area_width / 2.0 @@ -297,7 +298,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.penalty_area_length, -field_dimensions.penalty_area_width / 2.0 @@ -310,7 +311,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.goal_box_area_length, field_dimensions.goal_box_area_width / 2.0 @@ -323,7 +324,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.goal_box_area_length, -field_dimensions.goal_box_area_width / 2.0 @@ -336,7 +337,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.goal_box_area_length, -field_dimensions.goal_box_area_width / 2.0 @@ -349,7 +350,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![0.0, -field_dimensions.width / 2.0], point![0.0, field_dimensions.width / 2.0], ), @@ -360,7 +361,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> radius: field_dimensions.center_circle_diameter / 2.0, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 + field_dimensions.penalty_marker_distance - field_dimensions.penalty_marker_size / 2.0, @@ -376,7 +377,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ -field_dimensions.length / 2.0 + field_dimensions.penalty_marker_distance, -field_dimensions.penalty_marker_size / 2.0 @@ -389,7 +390,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveY, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.penalty_marker_distance @@ -405,7 +406,7 @@ pub fn field_marks_from_field_dimensions(field_dimensions: &FieldDimensions) -> direction: Direction::PositiveX, }, FieldMark::Line { - line: Line( + line: Line::new( point![ field_dimensions.length / 2.0 - field_dimensions.penalty_marker_distance, -field_dimensions.penalty_marker_size / 2.0 diff --git a/crates/types/src/limb.rs b/crates/types/src/limb.rs index c5d26ec7a4..b068a95dbf 100644 --- a/crates/types/src/limb.rs +++ b/crates/types/src/limb.rs @@ -23,7 +23,8 @@ pub fn is_above_limbs(pixel_position: Point2, projected_limbs: &[Limb]) - } // 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()]) + !Line::new(points[0], points[1]) + .is_above(point![pixel_position.x(), pixel_position.y()]) } None => true, } diff --git a/crates/vision/src/field_border_detection.rs b/crates/vision/src/field_border_detection.rs index 32c2f4c4b7..8c15c29802 100644 --- a/crates/vision/src/field_border_detection.rs +++ b/crates/vision/src/field_border_detection.rs @@ -143,7 +143,7 @@ fn best_fit_line(points: &[Point2]) -> Line2 { let half_size = points.len() / 2; let line_start = find_center_of_group(&points[0..half_size]); let line_end = find_center_of_group(&points[half_size..points.len()]); - Line(line_start, line_end) + Line::new(line_start, line_end) } fn find_center_of_group(group: &[Point2]) -> Point2 { @@ -161,13 +161,13 @@ fn is_orthogonal( angle_threshold: f32, ) -> Result { let projected_lines = [ - Line( - camera_matrix.pixel_to_ground(lines[0].0)?, - camera_matrix.pixel_to_ground(lines[0].1)?, + Line::new( + camera_matrix.pixel_to_ground(lines[0].first)?, + camera_matrix.pixel_to_ground(lines[0].second)?, ), - Line( - camera_matrix.pixel_to_ground(lines[1].0)?, - camera_matrix.pixel_to_ground(lines[1].1)?, + Line::new( + camera_matrix.pixel_to_ground(lines[1].first)?, + camera_matrix.pixel_to_ground(lines[1].second)?, ), ]; Ok(projected_lines[0].is_orthogonal(projected_lines[1], angle_threshold)) diff --git a/crates/vision/src/line_detection.rs b/crates/vision/src/line_detection.rs index 90f7c5ce66..f116b50673 100644 --- a/crates/vision/src/line_detection.rs +++ b/crates/vision/src/line_detection.rs @@ -153,7 +153,7 @@ impl LineDetection { break; }; - let line_in_ground = Line(start_point_in_robot, end_point_in_robot); + let line_in_ground = Line::new(start_point_in_robot, end_point_in_robot); let line_length_in_robot = line_in_ground.length(); let is_too_short = *context.check_line_length && line_length_in_robot < context.allowed_line_length_in_field.start; @@ -177,7 +177,7 @@ impl LineDetection { lines_in_ground.push(line_in_ground); if context.lines_in_image.is_subscribed() { - image_lines.push(Line(start_point_in_ground, end_point_in_ground)); + image_lines.push(Line::new(start_point_in_ground, end_point_in_ground)); } } let line_data = LineData { @@ -189,9 +189,9 @@ impl LineDetection { image_lines .into_iter() .map(|line| { - Line( - context.camera_matrix.ground_to_pixel(line.0).unwrap(), - context.camera_matrix.ground_to_pixel(line.1).unwrap(), + Line::new( + context.camera_matrix.ground_to_pixel(line.first).unwrap(), + context.camera_matrix.ground_to_pixel(line.second).unwrap(), ) }) .collect() @@ -201,9 +201,9 @@ impl LineDetection { .into_iter() .map(|(line, discard_reason)| { ( - Line( - context.camera_matrix.ground_to_pixel(line.0).unwrap(), - context.camera_matrix.ground_to_pixel(line.1).unwrap(), + Line::new( + context.camera_matrix.ground_to_pixel(line.first).unwrap(), + context.camera_matrix.ground_to_pixel(line.second).unwrap(), ), discard_reason, ) diff --git a/crates/vision/src/ransac.rs b/crates/vision/src/ransac.rs index 8469902f75..f61fb6b82e 100644 --- a/crates/vision/src/ransac.rs +++ b/crates/vision/src/ransac.rs @@ -45,7 +45,7 @@ impl Ransac { let mut points = self .unused_points .choose_multiple(&mut self.random_number_generator, 2); - let line = Line(*points.next().unwrap(), *points.next().unwrap()); + let line = Line::new(*points.next().unwrap(), *points.next().unwrap()); let score: f32 = self .unused_points .iter() @@ -105,7 +105,7 @@ mod test { let result = ransac.next_line(10, 5.0, 5.0); assert_relative_eq!( result.line.expect("No line found"), - Line(point![15.0, 15.0], point![30.0, 30.0]) + Line::new(point![15.0, 15.0], point![30.0, 30.0]) ); assert_relative_eq!(result.used_points[0], point![15.0, 15.0]); assert_relative_eq!(result.used_points[1], point![30.0, 30.0]); diff --git a/docs/framework/macros.md b/docs/framework/macros.md index da7a43b9fa..a4f0bf6e33 100644 --- a/docs/framework/macros.md +++ b/docs/framework/macros.md @@ -31,9 +31,13 @@ TODO: Elaborate - Extracts data from cycle context and returns none for all main outputs if the input was none - `require_some!(...) => match ... { Some(...) => ..., None => return MainOutputs::none() }` - SerializeHierarchy + - Goal: Enable use of *field paths* to conveniently access inner fields inside a hierarchy. + - Definition: *Field path* a compact representations for the *path* of a field in hierarchial data. + - i.e. For `{ a: { b: 10, c: "someString", d: { c: 666 } } }`, field path `a.d.c` refers to `c: 666`. + - We use this notation extensively inside the framework and tooling (i.e. Twix) - Trait - Mostly used by Communication for (de-)serialization - - Adds support for field paths + - Adds support for field paths (Representation of the 'path' of a field in hierarchial data.) - Allows to (de-)serialize into/from field paths: `fn serialize_hierarchy(field_path)`, `fn deserialize_hierarchy(field_path, data)` - Allows to check if a field paths exists - Allows to generate a hierarchy object diff --git a/tools/twix/src/panels/image/overlays/line_detection.rs b/tools/twix/src/panels/image/overlays/line_detection.rs index 38c3274eef..46a97d2ea2 100644 --- a/tools/twix/src/panels/image/overlays/line_detection.rs +++ b/tools/twix/src/panels/image/overlays/line_detection.rs @@ -54,10 +54,10 @@ impl Overlay for LineDetection { LineDiscardReason::LineTooLong => Color32::BROWN, LineDiscardReason::TooFarAway => Color32::BLACK, }; - painter.line_segment(line.0, line.1, Stroke::new(3.0, color)); + painter.line_segment(line.first, line.second, Stroke::new(3.0, color)); } for line in lines_in_image { - painter.line_segment(line.0, line.1, Stroke::new(3.0, Color32::BLUE)); + painter.line_segment(line.first, line.second, Stroke::new(3.0, Color32::BLUE)); } Ok(()) } diff --git a/tools/twix/src/panels/image/overlays/penalty_boxes.rs b/tools/twix/src/panels/image/overlays/penalty_boxes.rs index 93b0043945..8a60c888b5 100644 --- a/tools/twix/src/panels/image/overlays/penalty_boxes.rs +++ b/tools/twix/src/panels/image/overlays/penalty_boxes.rs @@ -37,7 +37,7 @@ impl Overlay for PenaltyBoxes { let penalty_boxes_lines_in_image: Vec> = self.penalty_boxes.require_latest()?; for line in penalty_boxes_lines_in_image { - painter.line_segment(line.0, line.1, Stroke::new(3.0, Color32::BLACK)); + painter.line_segment(line.first, line.second, Stroke::new(3.0, Color32::BLACK)); } Ok(()) } diff --git a/tools/twix/src/panels/map/layers/line_correspondences.rs b/tools/twix/src/panels/map/layers/line_correspondences.rs index adbb699490..b2c4fff152 100644 --- a/tools/twix/src/panels/map/layers/line_correspondences.rs +++ b/tools/twix/src/panels/map/layers/line_correspondences.rs @@ -44,7 +44,7 @@ impl Layer for LineCorrespondences { } }; for line in lines { - painter.line_segment(line.0, line.1, Stroke::new(0.02, Color32::YELLOW)); + painter.line_segment(line.first, line.second, Stroke::new(0.02, Color32::YELLOW)); } Ok(()) } diff --git a/tools/twix/src/panels/map/layers/lines.rs b/tools/twix/src/panels/map/layers/lines.rs index 6e9e095124..d1ec6db93e 100644 --- a/tools/twix/src/panels/map/layers/lines.rs +++ b/tools/twix/src/panels/map/layers/lines.rs @@ -52,8 +52,8 @@ impl Layer for Lines { .collect(); for line in lines { painter.line_segment( - ground_to_field * line.0, - ground_to_field * line.1, + ground_to_field * line.first, + ground_to_field * line.second, Stroke::new(0.04, Color32::RED), ); }