Implement extension trait for rotating vectors

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<BallResource>
 
                 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 {

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),

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 {

crates/geometry/src/direction.rs

@@ -21,12 +21,16 @@ pub enum Direction {
     Colinear,
 }
 
-impl Direction {
-    pub fn rotate_vector_90_degrees<Frame>(&self, subject: Vector2<Frame>) -> Vector2<Frame> {
-        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<Frame> Rotate90Degrees for Vector2<Frame> {
+    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,
         }
     }
 }

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<Frame> Line2<Frame> {
     }
 
     pub fn signed_distance_to_point(&self, point: Point2<Frame>) -> 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))
     }

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<Frame> LineSegment<Frame> {
 
     pub fn signed_distance_to_point(&self, point: Point2<Frame>) -> 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<Frame> LineSegment<Frame> {
     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<Frame> LineSegment<Frame> {
 
     pub fn get_direction(&self, point: Point2<Frame>) -> 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 {

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 {

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<World> TwixPainter<World> {
         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 {