1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
pub mod aabb2;
pub mod align2;
mod angles2;
mod arc2;
mod boundary;
mod circle2;
mod curve2;
pub mod hull;
mod line2;
pub mod polyline2;
mod segment2;
use crate::AngleDir;
use crate::AngleDir::Cw;
use crate::common::surface_point::SurfacePoint;
use crate::common::svd_basis::SvdBasis;
use crate::common::{PCoords, SurfacePointCollection};
use crate::na::SVector;
use parry2d_f64::na::UnitComplex;
use serde::{Deserialize, Serialize};
use std::ops;
pub type Point2 = parry2d_f64::na::Point2<f64>;
pub type Vector2 = parry2d_f64::na::Vector2<f64>;
pub type UnitVec2 = parry2d_f64::na::Unit<Vector2>;
pub type SurfacePoint2 = SurfacePoint<2>;
pub type Iso2 = parry2d_f64::na::Isometry2<f64>;
pub type SvdBasis2 = SvdBasis<2>;
pub type Ray2 = parry2d_f64::query::Ray;
pub type Align2 = crate::common::align::Alignment<UnitComplex<f64>, 2>;
pub type KdTree2 = crate::common::kd_tree::KdTree<2>;
pub use self::aabb2::Aabb2;
pub use self::angles2::{directed_angle, rot90, rot270, signed_angle};
pub use self::arc2::Arc2;
pub use self::boundary::BoundaryElement;
pub use self::circle2::Circle2;
pub use self::curve2::{Curve2, CurveStation2};
pub use self::line2::{Line2, intersect_lines, intersect_rays, intersection_param};
pub use self::segment2::Segment2;
#[derive(Clone, Debug, Serialize, Deserialize)]
pub struct ManifoldPosition2 {
/// The position of the point along the manifold length
pub l: f64,
/// The position of the point in full 2D cartesian space
pub point: Point2,
/// The direction of the positive manifold length in full 2D cartesian space at the current
/// position. You can think of this vector as pointing in the direction of the point at
/// `self.l + epsilon`
pub direction: UnitVec2,
/// The direction of the manifold surface normal in full 2D cartesian space at the current
/// position. This should follow the counter-clockwise winding order convention, so for most
/// manifolds this will be equivalent to `self.direction` rotated clockwise by 90 degrees.
pub normal: UnitVec2,
}
impl ManifoldPosition2 {
pub fn new(t: f64, point: Point2, direction: UnitVec2, normal: UnitVec2) -> Self {
Self {
l: t,
point,
direction,
normal,
}
}
pub fn normal_scalar_projection(&self, other: &impl PCoords<2>) -> f64 {
self.normal.dot(&(other.coords() - self.point.coords))
}
pub fn direction_scalar_project(&self, other: &impl PCoords<2>) -> f64 {
self.direction.dot(&(other.coords() - self.point.coords))
}
}
impl PCoords<2> for ManifoldPosition2 {
fn coords(&self) -> SVector<f64, 2> {
self.point.coords
}
}
pub trait HasBounds2 {
fn aabb(&self) -> &Aabb2;
}
impl ops::Mul<SurfacePoint2> for &Iso2 {
type Output = SurfacePoint2;
fn mul(self, rhs: SurfacePoint2) -> Self::Output {
rhs.transformed(self)
}
}
impl ops::Mul<&SurfacePoint2> for &Iso2 {
type Output = SurfacePoint2;
fn mul(self, rhs: &SurfacePoint2) -> Self::Output {
rhs.transformed(self)
}
}
impl SurfacePointCollection<2> for &[SurfacePoint2] {
fn clone_points(&self) -> Vec<Point2> {
self.iter().map(|sp| sp.point).collect()
}
fn clone_normals(&self) -> Vec<UnitVec2> {
self.iter().map(|sp| sp.normal).collect()
}
}
impl SurfacePointCollection<2> for &Vec<SurfacePoint2> {
fn clone_points(&self) -> Vec<Point2> {
self.iter().map(|sp| sp.point).collect()
}
fn clone_normals(&self) -> Vec<UnitVec2> {
self.iter().map(|sp| sp.normal).collect()
}
}
impl Line2 for SurfacePoint2 {
fn origin(&self) -> Point2 {
self.point
}
fn dir(&self) -> Vector2 {
self.normal.into_inner()
}
fn at(&self, t: f64) -> Point2 {
self.at_distance(t)
}
fn orthogonal(&self) -> Vector2 {
rot90(Cw) * self.normal.into_inner()
}
}
impl SurfacePoint2 {
/// Shift a surface point in the direction orthogonal to its normal (sideways) by the given
/// distance. The direction of travel is the surface point's normal vector rotated by 90 degrees
/// in the *clockwise* direction. If the normal is pointing up, a positive distance will move
/// the point to the right, and a negative distance will move the point to the left.
///
/// # Arguments
///
/// * `distance`: the distance to shift the point
///
/// returns: SurfacePoint<2>
///
/// # Examples
///
/// ```
/// use approx::assert_relative_eq;
/// use engeom::SurfacePoint2;
///
/// let sp = SurfacePoint2::new_normalize([0.0, 0.0].into(), [0.0, 1.0].into());
/// let shifted = sp.shift_orthogonal(1.0);
/// assert_relative_eq!(shifted.point, [1.0, 0.0].into(), epsilon = 1e-6);
/// ```
pub fn shift_orthogonal(&self, distance: f64) -> Self {
let shift = rot90(Cw) * self.normal.into_inner() * distance;
Self::new(self.point + shift, self.normal)
}
/// Rotate the normal vector of a surface point by the given angle. The angle is in radians and
/// is measured counterclockwise from the positive x-axis. If the normal is pointing up, a
/// positive angle will rotate it to the left, and a negative angle will rotate it to the right.
///
/// # Arguments
///
/// * `angle`: the angle to rotate the normal vector by
///
/// returns: SurfacePoint<2>
///
/// # Examples
///
/// ```
/// use approx::assert_relative_eq;
/// use engeom::{SurfacePoint2, Vector2, UnitVec2};
///
/// let sp = SurfacePoint2::new_normalize([0.0, 0.0].into(), [0.0, 1.0].into());
/// let rotated = sp.rot_normal(std::f64::consts::PI / 2.0);
/// let expected = UnitVec2::new_normalize(Vector2::new(-1.0, 0.0));
/// assert_relative_eq!(rotated.normal, expected, epsilon = 1e-6);
/// ```
pub fn rot_normal(&self, angle: f64) -> Self {
let n = Iso2::rotation(angle) * self.normal.into_inner();
Self::new_normalize(self.point, n)
}
/// Rotate the normal vector of a surface point by 90 degrees in the given direction. If the
/// normal is pointing up, rotating it clockwise will make it point to the right, and rotating
/// it counterclockwise will make it point to the left.
///
/// # Arguments
///
/// * `dir`: the direction to rotate the normal vector
///
/// returns: SurfacePoint<2>
///
/// # Examples
///
/// ```
/// use approx::assert_relative_eq;
/// use engeom::{SurfacePoint2, AngleDir, Vector2, UnitVec2};
///
/// let sp = SurfacePoint2::new_normalize([0.0, 0.0].into(), [0.0, 1.0].into());
/// let rotated = sp.rot_normal_90(AngleDir::Cw);
/// let expected = UnitVec2::new_normalize(Vector2::new(1.0, 0.0));
/// assert_relative_eq!(rotated.normal, expected, epsilon = 1e-6);
/// ```
pub fn rot_normal_90(&self, dir: AngleDir) -> Self {
Self::new_normalize(self.point, rot90(dir) * self.normal.into_inner())
}
}
#[cfg(test)]
pub mod tests {
use super::*;
use approx::assert_relative_eq;
use rand::{Rng, RngExt};
use std::f64::consts::PI;
pub struct Random2 {
rng: rand::rngs::ThreadRng,
}
impl Default for Random2 {
fn default() -> Self {
Self::new()
}
}
impl Random2 {
pub fn new() -> Self {
Self { rng: rand::rng() }
}
pub fn point(&mut self, limit: f64) -> Point2 {
Point2::new(
self.rng.random_range(-limit..limit),
self.rng.random_range(-limit..limit),
)
}
pub fn angle_sym_pi(&mut self) -> f64 {
self.rng.random_range(-PI..PI)
}
pub fn angle_sym_2pi(&mut self) -> f64 {
self.rng.random_range(-2.0 * PI..2.0 * PI)
}
pub fn angle_pos_2pi(&mut self) -> f64 {
self.rng.random_range(0.0..2.0 * PI)
}
pub fn positive(&mut self, limit: f64) -> f64 {
self.rng.random_range(0.0..limit)
}
}
#[test]
fn iso2_only_rotates_vector() {
let iso = Iso2::new(Vector2::new(1.0, 2.0), -PI / 2.0);
let v = Vector2::new(1.0, 1.0);
let vt = iso * v;
assert_relative_eq!(vt, Vector2::new(1.0, -1.0), epsilon = 1e-6);
}
}