parry3d/shape/
segment_pseudo_normals.rs

1use crate::math::Vector;
2
3#[cfg(feature = "alloc")]
4use crate::query::details::NormalConstraints;
5
6/// The pseudo-normals of a polyline segment, approximating the outward normal cones of its
7/// features.
8///
9/// This is the 2D segment analog of [`TrianglePseudoNormals`](crate::shape::TrianglePseudoNormals):
10/// `face` is the segment's outward normal and `edges` are the outward pseudo-normals at its two
11/// endpoints. An oriented polyline uses them to clamp contact normals to one side, so it acts as a
12/// one-sided surface.
13#[derive(Clone, Debug)]
14pub struct SegmentPseudoNormals {
15    /// The segment's outward normal.
16    pub face: Vector,
17    /// The outward pseudo-normals at the segment's two endpoints.
18    pub edges: [Vector; 2],
19}
20
21#[cfg(feature = "alloc")]
22impl NormalConstraints for SegmentPseudoNormals {
23    /// Projects `dir` so it lies within the outward cone defined by `self`.
24    fn project_local_normal_mut(&self, dir: &mut Vector) -> bool {
25        // Find the closest pseudo-normal.
26        let closest_edge = if dir.dot(self.edges[0]) >= dir.dot(self.edges[1]) {
27            self.edges[0]
28        } else {
29            self.edges[1]
30        };
31        crate::shape::pseudo_normals::project_into_cone(self.face, closest_edge, dir)
32    }
33}
34
35#[cfg(test)]
36#[cfg(all(feature = "dim2", feature = "alloc"))]
37mod test {
38    use super::{NormalConstraints, SegmentPseudoNormals};
39    use crate::math::Vector;
40
41    fn bisector(v1: Vector, v2: Vector) -> Vector {
42        (v1 + v2).normalize()
43    }
44
45    #[test]
46    fn degenerate_cone_collapses_to_face() {
47        let pn = SegmentPseudoNormals {
48            face: Vector::Y,
49            edges: [Vector::Y, Vector::Y],
50        };
51
52        assert_eq!(
53            pn.project_local_normal(Vector::new(1.0, 1.0)),
54            Some(Vector::Y)
55        );
56        assert!(pn.project_local_normal(-Vector::Y).is_none());
57    }
58
59    #[test]
60    fn clamps_into_the_outward_cone() {
61        // A cone centered on +Y, bounded at +-45 degrees (endpoint pseudo-normals at +-22.5 degrees).
62        let ends = [
63            bisector(Vector::Y, Vector::X),
64            bisector(Vector::Y, -Vector::X),
65        ];
66        let edges = [bisector(Vector::Y, ends[0]), bisector(Vector::Y, ends[1])];
67        let pn = SegmentPseudoNormals {
68            face: Vector::Y,
69            edges,
70        };
71
72        // Inside the cone: returned unchanged.
73        assert_eq!(pn.project_local_normal(Vector::Y), Some(Vector::Y));
74        for edge in edges {
75            assert_eq!(pn.project_local_normal(edge), Some(edge));
76        }
77        let inside = Vector::new(0.2, 1.0).normalize();
78        assert!(pn
79            .project_local_normal(inside)
80            .unwrap()
81            .abs_diff_eq(inside, 1.0e-5));
82
83        // Outside the cone but still outward: pulled onto the boundary, not left crossing the surface.
84        let sideways = Vector::new(1.0, 0.2).normalize();
85        let clamped = pn.project_local_normal(sideways).unwrap();
86        assert!(clamped.dot(Vector::Y) > sideways.dot(Vector::Y));
87        assert!(clamped.abs_diff_eq(ends[0], 1.0e-5));
88
89        // Into the solid (-face half-space): rejected.
90        assert!(pn.project_local_normal(-Vector::Y).is_none());
91        assert!(pn
92            .project_local_normal(Vector::new(0.3, -1.0).normalize())
93            .is_none());
94    }
95}
parry3d/shape/segment_pseudo_normals.rs

parry3d/shape/
segment_pseudo_normals.rs
