engeom 0.5.0

Library for working with geometry with a focus on engineering metrology applications.
Documentation
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
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
use crate::Result;
use crate::common::{Line, PCoords};
use parry3d_f64::na::{AbstractRotation, Isometry, Point, SVector, Unit};
use serde::{Deserialize, Serialize};

#[cfg(test)]
use approx::{AbsDiffEq, RelativeEq};

/// A `SurfacePoint` is a struct that is used to represent a point on a surface (n-1 dimensional
/// manifold) in n-dimensional space. It is defined by a point and a normal vector. Mathematically,
/// a `SurfacePoint` is identical to a parameterized line or a ray with a unit direction. It also
/// uniquely defines half-spaces (so a plane in 3D and a half-space line in 2D).
///
/// `SurfacePoint<D>` is the base for `SurfacePoint2` and `SurfacePoint3`, which are two of
/// `engeom`'s geometric primitives.
#[derive(Debug, Clone, Copy, Serialize, Deserialize, PartialEq)]
pub struct SurfacePoint<const D: usize> {
    pub point: Point<f64, D>,
    pub normal: Unit<SVector<f64, D>>,
}

impl<const D: usize> SurfacePoint<D> {
    /// Creates a new `SurfacePoint` from a point and an already-normalized unit normal.
    pub fn new(point: Point<f64, D>, normal: Unit<SVector<f64, D>>) -> Self {
        Self { point, normal }
    }

    /// Creates a new `SurfacePoint` from a point and an arbitrary normal vector, normalizing the
    /// vector in the process.
    pub fn new_normalize(point: Point<f64, D>, normal: SVector<f64, D>) -> Self {
        Self::new(point, Unit::new_normalize(normal))
    }

    /// Returns the point offset from the surface point by the given distance along the normal
    pub fn at_distance(&self, distance: f64) -> Point<f64, D> {
        self.point + self.normal.as_ref() * distance
    }

    /// Returns the scalar projection value of another point onto the line defined by the point and
    /// normal. This can be interpreted as the physical distance along the normal line that the
    /// other point is from the surface point.
    ///
    /// # Arguments
    ///
    /// * `other`: the point to project onto the line defined by the surface point
    ///
    /// returns: f64
    ///
    /// # Examples
    ///
    /// ```
    /// use engeom::{Point2, SurfacePoint2, Vector2};
    /// use approx::assert_relative_eq;
    ///
    /// let sp = SurfacePoint2::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0));
    ///
    /// let other = Point2::new(-1.0, -1.0);
    /// let scalar_projection = sp.scalar_projection(&other);
    ///
    /// assert_relative_eq!(scalar_projection, -1.0, epsilon = 1e-6);
    /// ```
    pub fn scalar_projection(&self, other: &impl PCoords<D>) -> f64 {
        self.normal.dot(&(other.coords() - self.point.coords))
    }

    /// Returns the point on the line defined by the point and normal that is closest to the other
    /// point, aka the projection of the other point onto the line defined by this surface point.
    pub fn projection(&self, other: &impl PCoords<D>) -> Point<f64, D> {
        self.at_distance(self.scalar_projection(other))
    }

    /// Returns a new surface point with the same point but with the normal reversed
    pub fn reversed(&self) -> Self {
        Self::new(self.point, -self.normal)
    }

    /// Returns a new surface point transformed by the given isometry
    pub fn transformed_by<R>(&self, t: &Isometry<f64, R, D>) -> Self
    where
        R: AbstractRotation<f64, D>,
    {
        Self::new(t * self.point, t * self.normal)
    }

    /// Returns the distance between a test point and its projection onto the line defined by the
    /// surface point. This is a complement to the `scalar_projection` method, except that it can
    /// only compute the magnitude of the distance, since the number of other dimensions may be
    /// greater than one.
    pub fn planar_distance(&self, other: &impl PCoords<D>) -> f64 {
        let projection = self.projection(other);
        (projection.coords - other.coords()).norm()
    }

    /// Returns a new surface point shifted from the original surface point by the given distance
    /// along the normal. This is useful for creating a new surface point that is a certain distance
    /// away from the original surface point, in the direction of the normal.
    ///
    /// # Arguments
    ///
    /// * `shift`: the distance to offset the surface point along the normal
    ///
    /// returns: SurfacePoint<{ D }>
    ///
    /// # Examples
    ///
    /// ```
    /// use engeom::{Point2, SurfacePoint2, Vector2};
    /// use approx::assert_relative_eq;
    ///
    /// let sp = SurfacePoint2::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0));
    ///
    /// let shifted = sp.shifted(2.0);
    /// assert_relative_eq!(shifted.point, Point2::new(0.0, 2.0), epsilon = 1e-6);
    /// assert_relative_eq!(shifted.normal.into_inner(), Vector2::new(0.0, 1.0), epsilon = 1e-6);
    /// ```
    pub fn shifted(&self, offset: f64) -> Self {
        let new_point = self.point + self.normal.as_ref() * offset;
        Self::new(new_point, self.normal)
    }

    /// Returns a `Line<D>` with the same origin as this surface point and a unit direction equal
    /// to the surface point's normal.
    ///
    /// # Examples
    ///
    /// ```
    /// use engeom::{Point2, SurfacePoint2, Vector2};
    /// use approx::assert_relative_eq;
    ///
    /// let sp = SurfacePoint2::new_normalize(Point2::new(1.0, 2.0), Vector2::new(0.0, 3.0));
    /// let line = sp.as_line();
    ///
    /// assert_relative_eq!(line.origin, sp.point, epsilon = 1e-12);
    /// assert_relative_eq!(line.direction, sp.normal.into_inner(), epsilon = 1e-12);
    /// ```
    pub fn as_line(&self) -> Line<D> {
        Line::new(self.point, self.normal.into_inner())
    }

    /// Performs spherical linear interpolation from this surface point to another one.
    ///
    /// # Arguments
    ///
    /// * `other`: the surface point to interpolate towards
    /// * `t`: the fraction of the way from this surface point to the other surface point, where
    ///   0.0 returns this point and 1.0 returns the other
    ///
    /// returns: SurfacePoint<{ D }>
    ///
    /// # Examples
    pub fn slerp(&self, other: &Self, t: f64) -> Self {
        let n = self.normal.slerp(&other.normal, t);
        let v = other.point - self.point;
        Self::new(self.point + v * t, n)
    }
}

#[cfg(test)]
impl<const D: usize> AbsDiffEq<Self> for SurfacePoint<D> {
    type Epsilon = f64;

    fn default_epsilon() -> Self::Epsilon {
        1e-8
    }

    fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
        self.point.abs_diff_eq(&other.point, epsilon)
            && self.normal.abs_diff_eq(&other.normal, epsilon)
    }
}

#[cfg(test)]
impl<const D: usize> RelativeEq for SurfacePoint<D> {
    fn default_max_relative() -> Self::Epsilon {
        1e-8
    }

    fn relative_eq(
        &self,
        other: &Self,
        epsilon: Self::Epsilon,
        max_relative: Self::Epsilon,
    ) -> bool {
        self.point.relative_eq(&other.point, epsilon, max_relative)
            && self
                .normal
                .relative_eq(&other.normal, epsilon, max_relative)
    }
}

/// Created a vector of `SurfacePoint` instances from a vector of points and a vector of normals.
/// If the number of points and normals are not the same, an error is returned.
///
/// # Arguments
///
/// * `points`: the vector of points, ordered to match the normals
/// * `normals`: the vector of normals, ordered to match the points
///
/// returns: Result<Vec<SurfacePoint<{ D }>, Global>, Box<dyn Error, Global>>
///
/// # Examples
///
/// ```
/// use engeom::{Point2, Vector2};
/// use engeom::common::surface_point::surface_point_vector;
/// use engeom::geom2::UnitVec2;
///
/// let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 1.0)];
/// let normals = vec![Vector2::new(0.0, 1.0), Vector2::new(1.0, 0.0)];
///
/// let surface_points = surface_point_vector(&points, &normals).unwrap();
///
/// assert_eq!(surface_points[0].point, Point2::new(0.0, 0.0));
/// assert_eq!(surface_points[0].normal, UnitVec2::new_normalize(Vector2::new(0.0, 1.0)));
/// assert_eq!(surface_points[1].point, Point2::new(1.0, 1.0));
/// assert_eq!(surface_points[1].normal, UnitVec2::new_normalize(Vector2::new(1.0, 0.0)));
/// ```
pub fn surface_point_vector<const D: usize>(
    points: &[Point<f64, D>],
    normals: &[SVector<f64, D>],
) -> Result<Vec<SurfacePoint<D>>> {
    // Check that the number of points and normals are the same
    if points.len() != normals.len() {
        return Err("The number of points and normals must be the same".into());
    }

    Ok(points
        .iter()
        .zip(normals.iter())
        .map(|(p, n)| SurfacePoint::new_normalize(*p, *n))
        .collect())
}

/// A trait for collections of `SurfacePoint` instances that can produce owned copies of their
/// points and normals as flat `Vec`s. Implemented for slices and `Vec` references of typed
/// surface point aliases such as `SurfacePoint2` and `SurfacePoint3`.
pub trait SurfacePointCollection<const D: usize> {
    /// Returns a `Vec` of cloned position points from this collection.
    fn clone_points(&self) -> Vec<Point<f64, D>>;

    /// Returns a `Vec` of cloned unit normals from this collection.
    fn clone_normals(&self) -> Vec<Unit<SVector<f64, D>>>;
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::SurfacePoint2;
    use approx::assert_relative_eq;
    use parry3d_f64::na::{Point2, Point3, Vector2, Vector3};

    fn sp2(px: f64, py: f64, nx: f64, ny: f64) -> SurfacePoint<2> {
        SurfacePoint::new_normalize(Point2::new(px, py), Vector2::new(nx, ny))
    }

    fn sp3(px: f64, py: f64, pz: f64, nx: f64, ny: f64, nz: f64) -> SurfacePoint<3> {
        SurfacePoint::new_normalize(Point3::new(px, py, pz), Vector3::new(nx, ny, nz))
    }

    #[test]
    fn new_stores_point_and_normal() {
        let sp = sp2(1.0, 2.0, 0.0, 1.0);
        assert_relative_eq!(sp.point, Point2::new(1.0, 2.0));
        assert_relative_eq!(sp.normal.into_inner(), Vector2::new(0.0, 1.0));
    }

    #[test]
    fn new_normalize_normalizes_the_vector() {
        let sp = SurfacePoint::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 3.0));
        assert_relative_eq!(sp.normal.into_inner().norm(), 1.0, epsilon = 1e-12);
        assert_relative_eq!(
            sp.normal.into_inner(),
            Vector2::new(0.0, 1.0),
            epsilon = 1e-12
        );
    }

    #[test]
    fn at_distance_positive_moves_along_normal() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0);
        let p = sp.at_distance(3.0);
        assert_relative_eq!(p, Point2::new(0.0, 3.0), epsilon = 1e-12);
    }

    #[test]
    fn at_distance_negative_moves_opposite_normal() {
        let sp = sp2(1.0, 1.0, 1.0, 0.0);
        let p = sp.at_distance(-2.0);
        assert_relative_eq!(p, Point2::new(-1.0, 1.0), epsilon = 1e-12);
    }

    #[test]
    fn at_distance_zero_returns_the_origin_point() {
        let sp = sp3(3.0, 4.0, 5.0, 0.0, 0.0, 1.0);
        assert_relative_eq!(sp.at_distance(0.0), sp.point, epsilon = 1e-12);
    }

    #[test]
    fn scalar_projection_point_ahead_of_plane() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0); // normal = +y
        let other = Point2::new(0.0, 5.0);
        assert_relative_eq!(sp.scalar_projection(&other), 5.0, epsilon = 1e-12);
    }

    #[test]
    fn scalar_projection_point_behind_plane() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0); // normal = +y
        let other = Point2::new(1.0, -3.0);
        assert_relative_eq!(sp.scalar_projection(&other), -3.0, epsilon = 1e-12);
    }

    #[test]
    fn scalar_projection_point_on_plane_is_zero() {
        let sp = sp2(0.0, 0.0, 1.0, 0.0); // normal = +x, point at origin
        let other = Point2::new(0.0, 7.0); // lies on the plane x = 0
        assert_relative_eq!(sp.scalar_projection(&other), 0.0, epsilon = 1e-12);
    }

    #[test]
    fn scalar_projection_offset_origin() {
        // Surface point not at origin: the projection should be relative to sp.point
        let sp = sp2(0.0, 2.0, 0.0, 1.0); // normal = +y, point at (0, 2)
        let other = Point2::new(0.0, 5.0);
        assert_relative_eq!(sp.scalar_projection(&other), 3.0, epsilon = 1e-12);
    }

    #[test]
    fn projection_lands_on_the_line() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0); // vertical line through origin
        let other = Point2::new(4.0, 3.0);
        let proj = sp.projection(&other);
        assert_relative_eq!(proj, Point2::new(0.0, 3.0), epsilon = 1e-12);
    }

    #[test]
    fn projection_of_point_already_on_line_is_itself() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0);
        let on_line = Point2::new(0.0, 7.0);
        assert_relative_eq!(sp.projection(&on_line), on_line, epsilon = 1e-12);
    }

    #[test]
    fn reversed_flips_normal_keeps_point() {
        let sp = sp2(1.0, 2.0, 0.0, 1.0);
        let rev = sp.reversed();
        assert_relative_eq!(rev.point, sp.point);
        assert_relative_eq!(
            rev.normal.into_inner(),
            -sp.normal.into_inner(),
            epsilon = 1e-12
        );
    }

    #[test]
    fn reversed_twice_is_identity() {
        let sp = sp3(1.0, 2.0, 3.0, 1.0, 1.0, 0.0);
        let twice = sp.reversed().reversed();
        assert_relative_eq!(twice.point, sp.point);
        assert_relative_eq!(
            twice.normal.into_inner(),
            sp.normal.into_inner(),
            epsilon = 1e-12
        );
    }

    #[test]
    fn planar_distance_measures_lateral_offset() {
        // Normal is +y; lateral distance is along x
        let sp = sp2(0.0, 0.0, 0.0, 1.0);
        let other = Point2::new(3.0, 7.0);
        assert_relative_eq!(sp.planar_distance(&other), 3.0, epsilon = 1e-12);
    }

    #[test]
    fn planar_distance_zero_for_point_on_line() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0);
        let on_line = Point2::new(0.0, 5.0);
        assert_relative_eq!(sp.planar_distance(&on_line), 0.0, epsilon = 1e-12);
    }

    #[test]
    fn planar_distance_3d() {
        // Normal is +z; lateral distance is in xy-plane
        let sp = sp3(0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
        let other = Point3::new(3.0, 4.0, 10.0); // distance from z-axis in xy = 5
        assert_relative_eq!(sp.planar_distance(&other), 5.0, epsilon = 1e-12);
    }

    #[test]
    fn shifted_moves_point_keeps_normal() {
        let sp = sp2(0.0, 0.0, 0.0, 1.0); // normal = +y
        let shifted = sp.shifted(4.0);
        assert_relative_eq!(shifted.point, Point2::new(0.0, 4.0), epsilon = 1e-12);
        assert_relative_eq!(
            shifted.normal.into_inner(),
            sp.normal.into_inner(),
            epsilon = 1e-12
        );
    }

    #[test]
    fn shifted_negative_offset() {
        let sp = sp2(0.0, 5.0, 0.0, 1.0); // normal = +y, point at (0, 5)
        let shifted = sp.shifted(-3.0);
        assert_relative_eq!(shifted.point, Point2::new(0.0, 2.0), epsilon = 1e-12);
    }

    #[test]
    fn surface_point_vector_builds_correctly() {
        let points = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
        let normals = vec![Vector2::new(0.0, 1.0), Vector2::new(0.0, 1.0)];
        let spv = surface_point_vector(&points, &normals).unwrap();
        assert_eq!(spv.len(), 2);
        assert_relative_eq!(spv[0].point, points[0]);
        assert_relative_eq!(spv[1].point, points[1]);
        assert_relative_eq!(spv[0].normal.into_inner(), normals[0]);
        assert_relative_eq!(spv[1].normal.into_inner(), normals[1]);
    }

    #[test]
    fn surface_point_vector_length_mismatch_returns_error() {
        let points = vec![Point2::new(0.0, 0.0)];
        let normals: Vec<Vector2<f64>> = vec![];
        assert!(surface_point_vector(&points, &normals).is_err());
    }

    #[test]
    fn slerp() {
        let sp0 = SurfacePoint2::new_normalize(Point2::new(0.0, 0.0), Vector2::new(0.0, 1.0));
        let sp1 = SurfacePoint2::new_normalize(Point2::new(1.0, 0.0), Vector2::new(1.0, 0.0));

        assert_relative_eq!(sp0, sp0.slerp(&sp1, 0.0));
        assert_relative_eq!(sp1, sp0.slerp(&sp1, 1.0));

        let e = SurfacePoint2::new_normalize(Point2::new(0.5, 0.0), Vector2::new(1.0, 1.0));
        assert_relative_eq!(e, sp0.slerp(&sp1, 0.5));
    }
}