Skip to content

Commit 136be0e

Browse files
rmburgrmburg
committed
geometry::Arc: use angles for start and end 
1 parent 955ba91 commit 136be0e

File tree

13 files changed

+297
-151
lines changed

13 files changed

+297
-151
lines changed

‎Cargo.lock

+1
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎crates/bevyhavior_simulator/src/robot.rs

+1-1
Original file line numberDiff line numberDiff line change
@@ -236,7 +236,7 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut<BallResource>
236236
PathSegment::LineSegment(LineSegment(_start, end)) => end.coords(),
237237
PathSegment::Arc(arc) => arc
238238
.direction
239-
.rotate_vector_90_degrees(arc.start - arc.circle.center),
239+
.rotate_vector_90_degrees(arc.start.as_direction() * arc.circle.radius),
240240
};
241241

242242
let orientation = match orientation_mode {

‎crates/control/src/motion/step_planner.rs

+3-2
Original file line numberDiff line numberDiff line change
@@ -105,11 +105,12 @@ impl StepPlanner {
105105
Pose2::from_parts(line_segment.1, rotation)
106106
}
107107
PathSegment::Arc(arc) => {
108+
let start_point = arc.start_point();
108109
let direction = arc
109110
.direction
110-
.rotate_vector_90_degrees(arc.start - arc.circle.center);
111+
.rotate_vector_90_degrees(start_point - arc.circle.center);
111112
Pose2::from_parts(
112-
arc.start + direction * 1.0,
113+
start_point + direction,
113114
Orientation2::from_vector(direction),
114115
)
115116
}

‎crates/control/src/path_planner.rs

+29-22
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
use color_eyre::{eyre::eyre, Result};
2-
use geometry::{arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment};
2+
use geometry::{
3+
angle::Angle, arc::Arc, circle::Circle, direction::Direction, line_segment::LineSegment,
4+
};
35
use linear_algebra::{distance, point, vector, Isometry2, Orientation2, Point2};
46
use log::warn;
57
use ordered_float::NotNan;
@@ -57,8 +59,7 @@ impl PathPlanner {
5759
PathSegment::LineSegment(line_segment) => line_segment.1.coords(),
5860
PathSegment::Arc(arc) => arc
5961
.direction
60-
.rotate_vector_90_degrees(arc.start - arc.circle.center)
61-
.normalize(),
62+
.rotate_vector_90_degrees(arc.start.as_direction()),
6263
};
6364
if direction.norm_squared() < f32::EPSILON {
6465
Orientation2::identity()
@@ -333,10 +334,14 @@ impl PathPlanner {
333334
.shape
334335
.as_circle()
335336
.ok_or_else(|| eyre!("obstacle from path node was not a circle"))?;
337+
338+
let start_direction = current_node.position - circle.center;
339+
let end_direction = next_node.position - circle.center;
340+
336341
Ok(PathSegment::Arc(Arc {
337342
circle,
338-
start: current_node.position,
339-
end: next_node.position,
343+
start: Angle::from_direction(start_direction),
344+
end: Angle::from_direction(end_direction),
340345
direction: LineSegment(previous_node.position, current_node.position)
341346
.get_direction(circle.center),
342347
}))
@@ -510,10 +515,12 @@ impl DynamicMap for PathPlanner {
510515
.shape
511516
.as_circle()
512517
.expect("ObstacleShape must be a circle");
518+
let start_direction = self.nodes[index].position - circle.center;
519+
let end_direction = self.nodes[*other_node].position - circle.center;
513520
let arc = Arc::new(
514521
circle,
515-
self.nodes[index].position,
516-
self.nodes[*other_node].position,
522+
Angle::from_direction(start_direction),
523+
Angle::from_direction(end_direction),
517524
direction,
518525
);
519526
if self
@@ -536,7 +543,7 @@ impl DynamicMap for PathPlanner {
536543

537544
#[cfg(test)]
538545
mod tests {
539-
use std::f32::consts::PI;
546+
use std::f32::consts::{FRAC_PI_3, PI};
540547

541548
use approx::assert_relative_eq;
542549
use linear_algebra::point;
@@ -614,8 +621,8 @@ mod tests {
614621
center: point![0.0, 0.0],
615622
radius: 1.0,
616623
},
617-
start: point![-0.5, 0.866],
618-
end: point![0.5, 0.866],
624+
start: Angle(2.0 * FRAC_PI_3),
625+
end: Angle(FRAC_PI_3),
619626
direction: Direction::Clockwise,
620627
}),
621628
PathSegment::LineSegment(LineSegment(point![0.5, 0.866], point![2.0, 0.0])),
@@ -649,8 +656,8 @@ mod tests {
649656
center: point![-1.0, 0.0],
650657
radius: 0.9770229,
651658
},
652-
start: point![-0.9474172, 0.9756069],
653-
end: point![-0.91782254, 0.9735608],
659+
start: Angle(1.5169508),
660+
end: Angle(1.4865868),
654661
direction: Direction::Clockwise,
655662
}),
656663
PathSegment::LineSegment(LineSegment(
@@ -662,8 +669,8 @@ mod tests {
662669
center: point![0.0, 2.0],
663670
radius: 1.1,
664671
},
665-
start: point![-0.092521094, 0.90389776],
666-
end: point![0.09252105, 0.90389776],
672+
start: Angle(-1.6550058),
673+
end: Angle(-1.4865868),
667674
direction: Direction::Counterclockwise,
668675
}),
669676
PathSegment::LineSegment(LineSegment(
@@ -675,8 +682,8 @@ mod tests {
675682
center: point![1.0, 0.0],
676683
radius: 0.9770229,
677684
},
678-
start: point![0.91782254, 0.9735608],
679-
end: point![0.9474171, 0.97560686],
685+
start: Angle(1.6550058),
686+
end: Angle(1.6246419),
680687
direction: Direction::Clockwise,
681688
}),
682689
PathSegment::LineSegment(LineSegment(
@@ -706,8 +713,8 @@ mod tests {
706713
center: point![-0.76, 0.56],
707714
radius: 0.22579876,
708715
},
709-
start: point![-0.8465765, 0.35145843],
710-
end: point![-0.9856166, 0.55093247],
716+
start: Angle(-1.9642965),
717+
end: Angle(-3.1014242),
711718
direction: Direction::Clockwise,
712719
}),
713720
PathSegment::LineSegment(LineSegment(
@@ -744,8 +751,8 @@ mod tests {
744751
center: point![2.2906392, 0.022267818],
745752
radius: 0.35000002,
746753
},
747-
start: point![2.2338033, 0.3676223],
748-
end: point![2.640637, 0.02350672],
754+
start: Angle(1.7339069),
755+
end: Angle(0.0035397257),
749756
direction: Direction::Clockwise,
750757
}),
751758
PathSegment::LineSegment(LineSegment(
@@ -784,8 +791,8 @@ mod tests {
784791
center: point![3.9259436, 0.8854635],
785792
radius: 0.35,
786793
},
787-
start: point![3.8195379, 1.2188969],
788-
end: point![3.8212261, 1.2194309],
794+
start: Angle(1.8797021),
795+
end: Angle(1.874643),
789796
direction: Direction::Clockwise,
790797
}),
791798
PathSegment::LineSegment(LineSegment(

‎crates/geometry/Cargo.toml

+1
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@ approx = { workspace = true }
1010
approx_derive = { workspace = true }
1111
linear_algebra = { workspace = true }
1212
nalgebra = { workspace = true }
13+
num-traits = { workspace = true }
1314
path_serde = { workspace = true }
1415
serde = { workspace = true }
1516

‎crates/geometry/src/angle.rs

+177
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,177 @@
1+
use std::ops::{Add, Div, Mul, Sub};
2+
3+
use approx::RelativeEq;
4+
use nalgebra::RealField;
5+
use num_traits::Euclid;
6+
use serde::{Deserialize, Serialize};
7+
8+
use linear_algebra::{vector, Rotation2, Vector2};
9+
use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
10+
11+
use crate::direction::Direction;
12+
13+
#[derive(
14+
Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
15+
)]
16+
pub struct Angle<T>(pub T);
17+
18+
impl<T> Angle<T> {
19+
pub fn new(value: T) -> Self {
20+
Self(value)
21+
}
22+
23+
pub fn into_inner(self) -> T {
24+
self.0
25+
}
26+
}
27+
28+
impl<T: Copy + RealField> Angle<T> {
29+
pub fn from_direction<Frame>(value: Vector2<Frame, T>) -> Self {
30+
Self(value.y().atan2(value.x()))
31+
}
32+
}
33+
34+
impl<T: Euclid + RealField> RelativeEq for Angle<T> {
35+
fn default_max_relative() -> Self::Epsilon {
36+
T::default_max_relative()
37+
}
38+
39+
fn relative_eq(
40+
&self,
41+
other: &Self,
42+
epsilon: Self::Epsilon,
43+
max_relative: Self::Epsilon,
44+
) -> bool {
45+
self.normalized()
46+
.0
47+
.relative_eq(&other.normalized().0, epsilon, max_relative)
48+
}
49+
}
50+
51+
impl<T: Euclid + RealField> PartialEq for Angle<T> {
52+
fn eq(&self, other: &Self) -> bool {
53+
self.normalized().0 == other.normalized().0
54+
}
55+
}
56+
57+
impl<T: Euclid + RealField + Clone> approx::AbsDiffEq for Angle<T> {
58+
type Epsilon = T::Epsilon;
59+
60+
fn default_epsilon() -> Self::Epsilon {
61+
T::default_epsilon()
62+
}
63+
64+
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
65+
let difference = (self.clone() - other.clone()).normalized().into_inner();
66+
67+
difference.abs_diff_eq(&T::zero(), epsilon.clone())
68+
|| difference.abs_diff_eq(&T::two_pi(), epsilon)
69+
}
70+
}
71+
72+
impl<T: RealField + Euclid> Angle<T> {
73+
pub fn cos(&self) -> T {
74+
self.0.clone().cos()
75+
}
76+
77+
pub fn sin(&self) -> T {
78+
self.0.clone().sin()
79+
}
80+
81+
#[must_use]
82+
pub fn angle_to(&self, to: Self, direction: Direction) -> Self {
83+
((to - self.clone()) * direction.angle_sign::<T>()).normalized()
84+
}
85+
86+
pub fn as_direction<Frame>(&self) -> Vector2<Frame, T> {
87+
vector![self.cos(), self.sin()]
88+
}
89+
90+
#[must_use]
91+
pub fn normalized(&self) -> Self {
92+
Angle(self.0.rem_euclid(&T::two_pi()))
93+
}
94+
}
95+
96+
impl<T: Add<Output = T>> Add for Angle<T> {
97+
type Output = Self;
98+
99+
fn add(self, rhs: Self) -> Self::Output {
100+
Self(self.0 + rhs.0)
101+
}
102+
}
103+
104+
impl<T: Sub<Output = T>> Sub for Angle<T> {
105+
type Output = Self;
106+
107+
fn sub(self, rhs: Self) -> Self::Output {
108+
Self(self.0 - rhs.0)
109+
}
110+
}
111+
112+
impl<T: Mul<Output = T>> Mul<T> for Angle<T> {
113+
type Output = Self;
114+
115+
fn mul(self, rhs: T) -> Self::Output {
116+
Self(self.0 * rhs)
117+
}
118+
}
119+
120+
impl<T: RealField, Frame> Mul<Vector2<Frame, T>> for Angle<T> {
121+
type Output = Vector2<Frame, T>;
122+
123+
fn mul(self, rhs: Vector2<Frame, T>) -> Self::Output {
124+
Rotation2::new(self.0) * rhs
125+
}
126+
}
127+
128+
impl<T: Div<Output = T>> Div for Angle<T> {
129+
type Output = T;
130+
131+
fn div(self, rhs: Self) -> Self::Output {
132+
self.0 / rhs.0
133+
}
134+
}
135+
136+
impl<T: Div<Output = T>> Div<T> for Angle<T> {
137+
type Output = Self;
138+
139+
fn div(self, rhs: T) -> Self::Output {
140+
Angle(self.0 / rhs)
141+
}
142+
}
143+
144+
#[cfg(test)]
145+
mod tests {
146+
use std::f64::consts::{FRAC_PI_2, FRAC_PI_3};
147+
148+
use approx::assert_abs_diff_eq;
149+
150+
use super::*;
151+
152+
#[test]
153+
fn angle_to() {
154+
let eps = 1e-15;
155+
156+
assert_abs_diff_eq!(
157+
Angle(0.0).angle_to(Angle(FRAC_PI_2), Direction::Clockwise),
158+
Angle(3.0 * FRAC_PI_2),
159+
epsilon = eps
160+
);
161+
assert_abs_diff_eq!(
162+
Angle(0.0).angle_to(Angle(FRAC_PI_2), Direction::Counterclockwise),
163+
Angle(FRAC_PI_2),
164+
epsilon = eps
165+
);
166+
assert_abs_diff_eq!(
167+
Angle(5.0 * FRAC_PI_3).angle_to(Angle(FRAC_PI_3), Direction::Clockwise),
168+
Angle(4.0 * FRAC_PI_3),
169+
epsilon = eps
170+
);
171+
assert_abs_diff_eq!(
172+
Angle(5.0 * FRAC_PI_3).angle_to(Angle(FRAC_PI_3), Direction::Counterclockwise),
173+
Angle(2.0 * FRAC_PI_3),
174+
epsilon = eps
175+
);
176+
}
177+
}

0 commit comments

Comments
 (0)