diff --git a/crates/bevyhavior_simulator/src/robot.rs b/crates/bevyhavior_simulator/src/robot.rs index 6f63b75061..1d93acfa53 100644 --- a/crates/bevyhavior_simulator/src/robot.rs +++ b/crates/bevyhavior_simulator/src/robot.rs @@ -19,7 +19,7 @@ use buffered_watch::Receiver; use control::localization::generate_initial_pose; use coordinate_systems::{Field, Ground, Head}; use framework::{future_queue, Producer, RecordingTrigger}; -use geometry::line_segment::LineSegment; +use geometry::{direction::Rotate90Degrees, line_segment::LineSegment}; use linear_algebra::{vector, Isometry2, Orientation2, Point2, Rotation2, Vector2}; use parameters::directory::deserialize; use projection::camera_matrix::CameraMatrix; @@ -234,9 +234,9 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut let target = match path[0] { PathSegment::LineSegment(LineSegment(_start, end)) => end.coords(), - PathSegment::Arc(arc) => arc - .direction - .rotate_vector_90_degrees(arc.start - arc.circle.center), + PathSegment::Arc(arc) => { + (arc.start - arc.circle.center).rotate_90_degrees(arc.direction) + } }; let orientation = match orientation_mode { diff --git a/crates/control/src/motion/step_planner.rs b/crates/control/src/motion/step_planner.rs index ccd8505f01..929a5ed567 100644 --- a/crates/control/src/motion/step_planner.rs +++ b/crates/control/src/motion/step_planner.rs @@ -1,5 +1,6 @@ use color_eyre::{eyre::eyre, Result}; use coordinate_systems::{Ground, UpcomingSupport}; +use geometry::direction::Rotate90Degrees; use serde::{Deserialize, Serialize}; use context_attribute::context; @@ -105,9 +106,7 @@ impl StepPlanner { Pose2::from_parts(line_segment.1, rotation) } PathSegment::Arc(arc) => { - let direction = arc - .direction - .rotate_vector_90_degrees(arc.start - arc.circle.center); + let direction = (arc.start - arc.circle.center).rotate_90_degrees(arc.direction); Pose2::from_parts( arc.start + direction * 1.0, Orientation2::from_vector(direction), diff --git a/crates/control/src/path_planner.rs b/crates/control/src/path_planner.rs index e36b7b274a..09fdc95d62 100644 --- a/crates/control/src/path_planner.rs +++ b/crates/control/src/path_planner.rs @@ -1,5 +1,10 @@ use color_eyre::{eyre::eyre, Result}; -use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment}; +use geometry::{ + arc::Arc, + circle::Circle, + direction::{Direction, Rotate90Degrees}, + line_segment::LineSegment, +}; use linear_algebra::{distance, point, vector, Isometry2, Orientation2, Point2}; use log::warn; use ordered_float::NotNan; @@ -55,9 +60,8 @@ impl PathPlanner { MotionCommand::Walk { path, .. } => path.first().map(|segment| { let direction = match segment { PathSegment::LineSegment(line_segment) => line_segment.1.coords(), - PathSegment::Arc(arc) => arc - .direction - .rotate_vector_90_degrees(arc.start - arc.circle.center) + PathSegment::Arc(arc) => (arc.start - arc.circle.center) + .rotate_90_degrees(arc.direction) .normalize(), }; if direction.norm_squared() < f32::EPSILON { diff --git a/crates/geometry/src/direction.rs b/crates/geometry/src/direction.rs index 98a0fd358b..783be44a59 100644 --- a/crates/geometry/src/direction.rs +++ b/crates/geometry/src/direction.rs @@ -21,12 +21,16 @@ pub enum Direction { Colinear, } -impl Direction { - pub fn rotate_vector_90_degrees(&self, subject: Vector2) -> Vector2 { - match self { - Direction::Clockwise => vector![subject.y(), -subject.x()], - Direction::Counterclockwise => vector![-subject.y(), subject.x()], - Direction::Colinear => subject, +pub trait Rotate90Degrees { + fn rotate_90_degrees(&self, direction: Direction) -> Self; +} + +impl Rotate90Degrees for Vector2 { + fn rotate_90_degrees(&self, direction: Direction) -> Self { + match direction { + Direction::Clockwise => vector![self.y(), -self.x()], + Direction::Counterclockwise => vector![-self.y(), self.x()], + Direction::Colinear => *self, } } } diff --git a/crates/geometry/src/line.rs b/crates/geometry/src/line.rs index 212ef48292..a92077b902 100644 --- a/crates/geometry/src/line.rs +++ b/crates/geometry/src/line.rs @@ -6,7 +6,11 @@ use serde::{Deserialize, Serialize}; use linear_algebra::{distance_squared, Point, Point2, Transform, Vector}; use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; -use crate::{direction::Direction, line_segment::LineSegment, Distance}; +use crate::{ + direction::{Direction, Rotate90Degrees}, + line_segment::LineSegment, + Distance, +}; #[derive( Copy, Clone, Debug, Deserialize, Serialize, PathSerialize, PathIntrospect, PathDeserialize, @@ -33,8 +37,9 @@ impl Line2 { } pub fn signed_distance_to_point(&self, point: Point2) -> f32 { - let normal_vector = Direction::Counterclockwise - .rotate_vector_90_degrees(self.direction) + let normal_vector = self + .direction + .rotate_90_degrees(Direction::Counterclockwise) .normalize(); normal_vector.dot(&(point - self.point)) } diff --git a/crates/geometry/src/line_segment.rs b/crates/geometry/src/line_segment.rs index 9d1f4d1cf8..f3a8342c53 100644 --- a/crates/geometry/src/line_segment.rs +++ b/crates/geometry/src/line_segment.rs @@ -8,11 +8,13 @@ use approx::{AbsDiffEq, RelativeEq}; use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; use serde::{Deserialize, Serialize}; -use linear_algebra::{ - center, distance, distance_squared, vector, Point2, Rotation2, Transform, Vector2, -}; +use linear_algebra::{center, distance, distance_squared, Point2, Rotation2, Transform, Vector2}; -use crate::{arc::Arc, direction::Direction, Distance}; +use crate::{ + arc::Arc, + direction::{Direction, Rotate90Degrees}, + Distance, +}; #[derive( Clone, @@ -50,8 +52,8 @@ impl LineSegment { pub fn signed_distance_to_point(&self, point: Point2) -> f32 { let line_vector = self.1 - self.0; - let normal_vector = Direction::Counterclockwise - .rotate_vector_90_degrees(line_vector) + let normal_vector = line_vector + .rotate_90_degrees(Direction::Counterclockwise) .normalize(); normal_vector.dot(&point.coords()) - normal_vector.dot(&self.0.coords()) } @@ -69,8 +71,7 @@ impl LineSegment { 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 orthogonal_other_direction = - Direction::Clockwise.rotate_vector_90_degrees(other_direction); + let orthogonal_other_direction = other_direction.rotate_90_degrees(Direction::Clockwise); signed_acute_angle(self_direction, orthogonal_other_direction) } @@ -128,7 +129,7 @@ impl LineSegment { pub fn get_direction(&self, point: Point2) -> Direction { let direction_vector = self.1 - self.0; - let clockwise_normal_vector = vector![direction_vector.y(), -direction_vector.x()]; + let clockwise_normal_vector = direction_vector.rotate_90_degrees(Direction::Clockwise); let directed_cathetus = clockwise_normal_vector.dot(&(point - self.0)); match directed_cathetus { diff --git a/crates/types/src/field_marks.rs b/crates/types/src/field_marks.rs index fb4fa1650e..69085f7949 100644 --- a/crates/types/src/field_marks.rs +++ b/crates/types/src/field_marks.rs @@ -1,4 +1,7 @@ -use geometry::{direction::Direction as RotationDirection, line_segment::LineSegment}; +use geometry::{ + direction::{Direction as RotationDirection, Rotate90Degrees}, + line_segment::LineSegment, +}; use ordered_float::NotNan; use serde::{Deserialize, Serialize}; @@ -116,8 +119,8 @@ impl FieldMark { let measured_direction = (measured_line.0 - measured_line.1).normalize(); let center_vector = (correspondence_0_reference - center) + (correspondence_1_reference - center); - let reference_direction = RotationDirection::Counterclockwise - .rotate_vector_90_degrees(center_vector) + let reference_direction = center_vector + .rotate_90_degrees(RotationDirection::Counterclockwise) .normalize(); Correspondences { diff --git a/tools/twix/src/twix_painter.rs b/tools/twix/src/twix_painter.rs index ba1d59eb0a..513af72b2e 100644 --- a/tools/twix/src/twix_painter.rs +++ b/tools/twix/src/twix_painter.rs @@ -11,7 +11,12 @@ use eframe::{ use nalgebra::{Rotation2, SMatrix, Similarity2}; use coordinate_systems::{Field, Ground, Screen}; -use geometry::{arc::Arc, circle::Circle, direction::Direction, rectangle::Rectangle}; +use geometry::{ + arc::Arc, + circle::Circle, + direction::{Direction, Rotate90Degrees}, + rectangle::Rectangle, +}; use linear_algebra::{point, vector, IntoTransform, Isometry2, Point2, Pose2, Transform, Vector2}; use types::{field_dimensions::FieldDimensions, planned_path::PathSegment}; @@ -159,8 +164,8 @@ impl TwixPainter { let start_relative = start - center; let end_relative = end - center; let angle_difference = start_relative.angle(&end_relative); - let end_right_of_start = Direction::Counterclockwise - .rotate_vector_90_degrees(start_relative) + let end_right_of_start = start_relative + .rotate_90_degrees(Direction::Counterclockwise) .dot(&end_relative) < 0.0; let counterclockwise_angle_difference = if end_right_of_start {