parry3d/query/clip/
clip_segment_segment.rs

1use crate::math::{Point, Real};
2#[cfg(feature = "dim2")]
3use crate::{math::Vector, utils};
4
5// Features in clipping points are:
6// 0 = First vertex.
7// 1 = On the face.
8// 2 = Second vertex.
9pub type ClippingPoints = (Point<Real>, Point<Real>, usize, usize);
10
11/// Projects two segments on one another towards the direction `normal`,
12/// and compute their intersection.
13#[cfg(feature = "dim2")]
14pub fn clip_segment_segment_with_normal(
15    mut seg1: (Point<Real>, Point<Real>),
16    mut seg2: (Point<Real>, Point<Real>),
17    normal: Vector<Real>,
18) -> Option<(ClippingPoints, ClippingPoints)> {
19    use crate::utils::WBasis;
20    let tangent = normal.orthonormal_basis()[0];
21
22    let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)];
23    let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)];
24    let mut features1 = [0, 2];
25    let mut features2 = [0, 2];
26
27    if range1[1] < range1[0] {
28        range1.swap(0, 1);
29        features1.swap(0, 1);
30        core::mem::swap(&mut seg1.0, &mut seg1.1);
31    }
32
33    if range2[1] < range2[0] {
34        range2.swap(0, 1);
35        features2.swap(0, 1);
36        core::mem::swap(&mut seg2.0, &mut seg2.1);
37    }
38
39    if range2[0] > range1[1] || range1[0] > range2[1] {
40        // No clip point.
41        return None;
42    }
43
44    let ca = if range2[0] > range1[0] {
45        let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]);
46        let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
47        let p2 = seg2.0;
48
49        (p1, p2, 1, features2[0])
50    } else {
51        let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]);
52        let p1 = seg1.0;
53        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
54
55        (p1, p2, features1[0], 1)
56    };
57
58    let cb = if range2[1] < range1[1] {
59        let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]);
60        let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
61        let p2 = seg2.1;
62
63        (p1, p2, 1, features2[1])
64    } else {
65        let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]);
66        let p1 = seg1.1;
67        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
68
69        (p1, p2, features1[1], 1)
70    };
71
72    Some((ca, cb))
73}
74
75/// Projects two segments on one another and compute their intersection.
76pub fn clip_segment_segment(
77    mut seg1: (Point<Real>, Point<Real>),
78    mut seg2: (Point<Real>, Point<Real>),
79) -> Option<(ClippingPoints, ClippingPoints)> {
80    // NOTE: no need to normalize the tangent.
81    let tangent1 = seg1.1 - seg1.0;
82    let sqnorm_tangent1 = tangent1.norm_squared();
83
84    let mut range1 = [0.0, sqnorm_tangent1];
85    let mut range2 = [
86        (seg2.0 - seg1.0).dot(&tangent1),
87        (seg2.1 - seg1.0).dot(&tangent1),
88    ];
89    let mut features1 = [0, 2];
90    let mut features2 = [0, 2];
91
92    if range1[1] < range1[0] {
93        range1.swap(0, 1);
94        features1.swap(0, 1);
95        core::mem::swap(&mut seg1.0, &mut seg1.1);
96    }
97
98    if range2[1] < range2[0] {
99        range2.swap(0, 1);
100        features2.swap(0, 1);
101        core::mem::swap(&mut seg2.0, &mut seg2.1);
102    }
103
104    if range2[0] > range1[1] || range1[0] > range2[1] {
105        // No clip point.
106        return None;
107    }
108
109    let length1 = range1[1] - range1[0];
110    let length2 = range2[1] - range2[0];
111
112    let ca = if range2[0] > range1[0] {
113        let bcoord = (range2[0] - range1[0]) / length1;
114        let p1 = seg1.0 + tangent1 * bcoord;
115        let p2 = seg2.0;
116
117        (p1, p2, 1, features2[0])
118    } else {
119        let bcoord = (range1[0] - range2[0]) / length2;
120        let p1 = seg1.0;
121        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
122
123        (p1, p2, features1[0], 1)
124    };
125
126    let cb = if range2[1] < range1[1] {
127        let bcoord = (range2[1] - range1[0]) / length1;
128        let p1 = seg1.0 + tangent1 * bcoord;
129        let p2 = seg2.1;
130
131        (p1, p2, 1, features2[1])
132    } else {
133        let bcoord = (range1[1] - range2[0]) / length2;
134        let p1 = seg1.1;
135        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
136
137        (p1, p2, features1[1], 1)
138    };
139
140    Some((ca, cb))
141}
parry3d/query/clip/clip_segment_segment.rs

parry3d/query/clip/
clip_segment_segment.rs
