geop_core_topology/
distance.rs

1//! [`closest_points`]: the least distance between two points, curves or
2//! faces of models — each where a pose puts it — and two points that
3//! attain it.
4//!
5//! A branch and bound over pieces of the two ([`least_distance`]): spans
6//! of curves, patches of a face's surface and spans of its edges. A patch
7//! the face's trim definitely leaves out is dropped, and a patch only
8//! known to meet the trim attains a point where its middle is on the face.
9//! The least distance of a face is either at a point inside it or on its
10//! boundary, which its edges' spans attain.
11
12use geop_core_geometry::{
13    nearest::{CurveSpan, Piece, SurfacePatch, could_come_within, least_distance},
14    search::{MAX_NODES, min_subdivision_size},
15};
16use geop_core_math::{
17    geop_error::{GeopError, GeopResult, WithContext},
18    primitives::Pose,
19    scalars::Scalar,
20    vector::Vector3,
21};
22
23use crate::{
24    Curve3, EdgeId, FaceId, Model,
25    contains::face::{PointClassification, face_contains},
26};
27
28const SEED: u64 = 0xD157_A1CE_0000_0001;
29
30/// How often the two points are refined, each by Newton towards the other,
31/// at most. Bounds effort only: each refinement keeps the point on its
32/// feature and is kept only if it brings the two nearer.
33const MAX_ALTERNATIONS: usize = 64;
34
35/// A face of a model, where `pose` puts it — or where it is, without one.
36#[derive(Clone)]
37pub struct PlacedFace<'m> {
38    model: &'m Model,
39    face: FaceId,
40    pose: Option<Pose>,
41}
42
43impl<'m> PlacedFace<'m> {
44    pub fn new(model: &'m Model, face: FaceId, pose: Option<Pose>) -> GeopResult<Self> {
45        model.get_face(face).with_context(&|e: GeopError| {
46            e.with_context(format!("PlacedFace::new(face={face})"))
47        })?;
48        Ok(Self { model, face, pose })
49    }
50
51    /// Where `(u, v)` — a point, or a box of them — is against the face's
52    /// trim.
53    fn classify(&self, u: Scalar, v: Scalar) -> GeopResult<PointClassification> {
54        face_contains(
55            self.model,
56            self.face,
57            u,
58            v,
59            MAX_NODES,
60            min_subdivision_size(),
61            SEED,
62        )
63    }
64
65    /// `p` where the pose puts it.
66    fn place(&self, p: Vector3) -> Vector3 {
67        place(&self.pose, p)
68    }
69
70    /// `p` in the face's own frame, where it is without the pose.
71    fn local(&self, p: &Vector3) -> Vector3 {
72        match &self.pose {
73            Some(pose) => pose.inverse().apply(p),
74            None => *p,
75        }
76    }
77}
78
79/// `p` where `pose` puts it.
80fn place(pose: &Option<Pose>, p: Vector3) -> Vector3 {
81    match pose {
82        Some(pose) => pose.apply(&p),
83        None => p,
84    }
85}
86
87/// Something a distance is measured to.
88pub enum Feature<'m> {
89    Point(Vector3),
90    /// A curve, already where it is.
91    Curve(Curve3),
92    Face(PlacedFace<'m>),
93}
94
95/// The least distance between two features, and two points of them
96/// attaining as little as the search could tell apart from it.
97#[derive(Clone, Copy, Debug)]
98pub struct Closest {
99    /// An enclosure of the least distance: from what the search proved no
100    /// two points are nearer than, to the distance between `a` and `b`.
101    pub distance: Scalar,
102    pub a: Vector3,
103    pub b: Vector3,
104}
105
106/// Where on a feature a point of it is.
107#[derive(Clone, Copy, Debug)]
108enum Spot {
109    Point,
110    /// On a [`Feature::Curve`], at this parameter.
111    Curve(Scalar),
112    /// On an edge of a face, at this parameter of its curve.
113    Edge(EdgeId, Scalar),
114    /// On a face, at these parameters of its surface.
115    Face(Scalar, Scalar),
116}
117
118impl Feature<'_> {
119    /// From `spot`, Newton's point of the feature nearest `target` around
120    /// it — or `spot` itself where the step would leave the face — and
121    /// where it is. Which point of the feature is a free choice; it stays
122    /// one of its points.
123    fn refine(&self, spot: Spot, target: &Vector3) -> GeopResult<(Spot, Vector3)> {
124        match (self, spot) {
125            (Feature::Point(p), _) => Ok((Spot::Point, *p)),
126            (Feature::Curve(curve), Spot::Curve(t)) => {
127                let t = curve.closest_parameter_from(target, t)?;
128                Ok((Spot::Curve(t), curve.evaluate(t)?))
129            }
130            (Feature::Face(face), Spot::Edge(edge, t)) => {
131                let curve = &face.model.get_edge(edge)?.curve;
132                let t = curve.closest_parameter_from(&face.local(target), t)?;
133                Ok((Spot::Edge(edge, t), face.place(curve.evaluate(t)?)))
134            }
135            (Feature::Face(face), Spot::Face(u, v)) => {
136                let surface = &face.model.get_face(face.face)?.surface;
137                let (ru, rv) = surface.refine_closest(&face.local(target), u, v)?;
138                let (u, v) = match face.classify(ru, rv)? {
139                    PointClassification::Outside => (u, v),
140                    _ => (ru, rv),
141                };
142                Ok((Spot::Face(u, v), face.place(surface.evaluate(u, v)?)))
143            }
144            (_, spot) => Err(GeopError::new(format!(
145                "Feature::refine: {spot:?} is not on this feature"
146            ))),
147        }
148    }
149}
150
151/// A piece of a [`Feature`], for [`least_distance`].
152#[derive(Clone)]
153enum FeaturePiece<'m> {
154    Point(Vector3),
155    /// A span of a curve — of `edge`, if it is a face's — where `pose`
156    /// puts it.
157    Curve {
158        span: CurveSpan,
159        edge: Option<EdgeId>,
160        pose: Option<Pose>,
161    },
162    /// A patch of a face's surface; `inside` once it is known to lie
163    /// inside the trim.
164    Patch {
165        face: PlacedFace<'m>,
166        patch: SurfacePatch,
167        inside: bool,
168    },
169}
170
171impl<'m> FeaturePiece<'m> {
172    /// The pieces `feature` starts as.
173    fn roots(feature: &Feature<'m>) -> GeopResult<Vec<Self>> {
174        Ok(match feature {
175            Feature::Point(p) => vec![FeaturePiece::Point(*p)],
176            Feature::Curve(curve) => vec![FeaturePiece::Curve {
177                span: CurveSpan(curve.clone()),
178                edge: None,
179                pose: None,
180            }],
181            Feature::Face(face) => {
182                let surface = &face.model.get_face(face.face)?.surface;
183                let mut pieces: Vec<Self> =
184                    Self::patch(face, SurfacePatch(surface.clone()), false)?
185                        .into_iter()
186                        .collect();
187                for edge in face.model.face_edges(face.face) {
188                    pieces.push(FeaturePiece::Curve {
189                        span: CurveSpan(face.model.get_edge(edge)?.curve.clone()),
190                        edge: Some(edge),
191                        pose: face.pose,
192                    });
193                }
194                pieces
195            }
196        })
197    }
198
199    /// `patch` of `face`, unless the trim definitely leaves all of it out.
200    /// A patch inside its parent is inside too.
201    fn patch(face: &PlacedFace<'m>, patch: SurfacePatch, inside: bool) -> GeopResult<Option<Self>> {
202        let inside = inside || {
203            let ((u0, u1), (v0, v1)) = (patch.0.domain_u(), patch.0.domain_v());
204            match face.classify(u0.union(u1), v0.union(v1))? {
205                PointClassification::Outside => return Ok(None),
206                PointClassification::Inside => true,
207                PointClassification::OnVertex | PointClassification::OnCoedge => false,
208            }
209        };
210        Ok(Some(FeaturePiece::Patch {
211            face: face.clone(),
212            patch,
213            inside,
214        }))
215    }
216}
217
218impl Piece for FeaturePiece<'_> {
219    type At = Spot;
220
221    fn hull(&self) -> GeopResult<Vec<Vector3>> {
222        Ok(match self {
223            FeaturePiece::Point(p) => vec![*p],
224            FeaturePiece::Curve { span, pose, .. } => {
225                span.hull()?.into_iter().map(|p| place(pose, p)).collect()
226            }
227            FeaturePiece::Patch { face, patch, .. } => {
228                patch.hull()?.into_iter().map(|p| face.place(p)).collect()
229            }
230        })
231    }
232
233    fn point(&self) -> GeopResult<Option<(Spot, Vector3)>> {
234        Ok(match self {
235            FeaturePiece::Point(p) => Some((Spot::Point, *p)),
236            FeaturePiece::Curve { span, edge, pose } => span.point()?.map(|(t, p)| {
237                let spot = match edge {
238                    Some(edge) => Spot::Edge(*edge, t),
239                    None => Spot::Curve(t),
240                };
241                (spot, place(pose, p))
242            }),
243            FeaturePiece::Patch {
244                face,
245                patch,
246                inside,
247            } => {
248                let (u, v) = patch.middle()?;
249                let on = *inside || face.classify(u, v)? != PointClassification::Outside;
250                on.then(|| {
251                    Ok::<_, GeopError>((Spot::Face(u, v), face.place(patch.0.evaluate(u, v)?)))
252                })
253                .transpose()?
254            }
255        })
256    }
257
258    fn split(&self, handoff: Scalar) -> GeopResult<Option<Vec<Self>>> {
259        Ok(match self {
260            FeaturePiece::Point(_) => None,
261            FeaturePiece::Curve { span, edge, pose } => span.split(handoff)?.map(|halves| {
262                halves
263                    .into_iter()
264                    .map(|span| FeaturePiece::Curve {
265                        span,
266                        edge: *edge,
267                        pose: *pose,
268                    })
269                    .collect()
270            }),
271            FeaturePiece::Patch {
272                face,
273                patch,
274                inside,
275            } => match patch.split(handoff)? {
276                None => None,
277                Some(halves) => {
278                    let mut kept = Vec::new();
279                    for half in halves {
280                        kept.extend(Self::patch(face, half, *inside)?);
281                    }
282                    Some(kept)
283                }
284            },
285        })
286    }
287}
288
289/// The least distance between `a` and `b`, and two points attaining it
290/// (see the module docs).
291///
292/// The search encloses the least distance, but the points attaining it
293/// only as sharply as the square root of that: the distance is flat where
294/// it is least. So the two points are then refined in turns, each by
295/// Newton towards the other, as long as that brings them nearer.
296pub fn closest_points(a: &Feature, b: &Feature) -> GeopResult<Closest> {
297    let nearest = least_distance(
298        FeaturePiece::roots(a)?,
299        FeaturePiece::roots(b)?,
300        min_subdivision_size(),
301        MAX_NODES,
302    )?;
303    let ((mut sa, mut pa), (mut sb, mut pb)) = (nearest.a, nearest.b);
304    // Which of two attained pairs to keep is a free choice.
305    let apart = |p: &Vector3, q: &Vector3| p.sub(q).norm_sq().to_f64();
306    for _ in 0..MAX_ALTERNATIONS {
307        let before = apart(&pa, &pb);
308        let (nb, qb) = b.refine(sb, &pa)?;
309        if apart(&pa, &qb) < apart(&pa, &pb) {
310            (sb, pb) = (nb, qb);
311        }
312        let (na, qa) = a.refine(sa, &pb)?;
313        if apart(&qa, &pb) < apart(&pa, &pb) {
314            (sa, pa) = (na, qa);
315        }
316        if apart(&pa, &pb) >= before {
317            break;
318        }
319    }
320    Ok(Closest {
321        distance: nearest.distance.lower().union(pa.sub(&pb).norm()),
322        a: pa,
323        b: pb,
324    })
325}
326
327/// Whether `a` and `b` could be at distance zero: what [`closest_points`]
328/// answers too, without finding the least distance between them (see
329/// [`could_come_within`]).
330pub fn could_touch(a: &Feature, b: &Feature) -> GeopResult<bool> {
331    could_come_within(
332        FeaturePiece::roots(a)?,
333        FeaturePiece::roots(b)?,
334        Scalar::ZERO,
335        min_subdivision_size(),
336        MAX_NODES,
337    )
338}
339
340#[cfg(test)]
341mod tests {
342    use geop_core_math::{primitives::Pose, scalars::Scalar, vector::Vector3};
343
344    use super::{Feature, PlacedFace, closest_points, could_touch};
345    use crate::{Model, test_fixtures::test_cube_solid};
346
347    fn v(x: f64, y: f64, z: f64) -> Vector3 {
348        Vector3::from_array([x, y, z].map(Scalar::from_f64))
349    }
350
351    /// Whether two faces could touch is what their least distance says,
352    /// for every pair of faces of two cubes lying on each other, a hair
353    /// apart, far apart and overlapping.
354    #[test]
355    fn could_touch_answers_as_the_least_distance_does() {
356        let mut model = Model::new();
357        test_cube_solid(&mut model);
358        let faces: Vec<_> = model.faces.keys().copied().collect();
359        let place = |x: f64| Some(Pose::identity().with_position(v(x, 0.0, 0.0)));
360        for x in [1.0, 1.0 + 1e-6, 1.5, 0.5, 50.0] {
361            let mut touching = 0;
362            for &fa in &faces {
363                for &fb in &faces {
364                    let a = Feature::Face(PlacedFace::new(&model, fa, None).unwrap());
365                    let b = Feature::Face(PlacedFace::new(&model, fb, place(x)).unwrap());
366                    let least = closest_points(&a, &b).unwrap();
367                    let answer = could_touch(&a, &b).unwrap();
368                    assert_eq!(
369                        answer,
370                        least.distance.could_be_equal(Scalar::ZERO),
371                        "x = {x}, {least:?}"
372                    );
373                    touching += usize::from(answer);
374                }
375            }
376            assert_eq!(touching > 0, x <= 1.0, "x = {x}: {touching} pairs touch");
377        }
378    }
379
380    /// A point off the unit cube is nearest its face in front of it, a
381    /// point off a corner nearest the corner — over every face, the least
382    /// distance is the true one, attained by points on both.
383    #[test]
384    fn a_point_and_the_faces_of_a_cube() {
385        let mut model = Model::new();
386        test_cube_solid(&mut model);
387        let least = |target: Vector3, pose: Option<Pose>| {
388            model
389                .faces
390                .keys()
391                .map(|&face| {
392                    let face = Feature::Face(PlacedFace::new(&model, face, pose).unwrap());
393                    closest_points(&Feature::Point(target), &face).unwrap()
394                })
395                .min_by(|a, b| a.distance.to_f64().total_cmp(&b.distance.to_f64()))
396                .unwrap()
397        };
398        let front = least(v(2.0, 0.3, 0.6), None);
399        assert!(front.distance.could_be_equal(Scalar::ONE), "{front:?}");
400        assert!(front.b.could_be_equal(&v(1.0, 0.3, 0.6)), "{front:?}");
401        let corner = least(v(2.0, 2.0, 2.0), None);
402        assert!(
403            corner
404                .distance
405                .could_be_equal(Scalar::from_f64(3f64.sqrt())),
406            "{corner:?}"
407        );
408        // The cube moved up by 5: the same point is now 4.4 below its bottom.
409        let up = Pose::identity().with_position(v(0.0, 0.0, 5.0));
410        let below = least(v(0.5, 0.5, 0.6), Some(up));
411        assert!(
412            below.distance.could_be_equal(Scalar::from_f64(4.4)),
413            "{below:?}"
414        );
415    }
416}
geop_core_topology/distance.rs

geop_core_topology/
distance.rs
