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
use crate::math::{Point, Real, UnitVector, Vector};
use crate::query::SplitResult;
use crate::shape::Segment;

impl Segment {
    /// Splits this segment along the given canonical axis.
    ///
    /// This will split the segment by a plane with a normal with it’s `axis`-th component set to 1.
    /// The splitting plane is shifted wrt. the origin by the `bias` (i.e. it passes through the point
    /// equal to `normal * bias`).
    ///
    /// # Result
    /// Returns the result of the split. The first shape returned is the piece lying on the negative
    /// half-space delimited by the splitting plane. The second shape returned is the piece lying on the
    /// positive half-space delimited by the splitting plane.
    pub fn canonical_split(&self, axis: usize, bias: Real, epsilon: Real) -> SplitResult<Self> {
        // TODO: optimize this.
        self.local_split(&Vector::ith_axis(axis), bias, epsilon)
    }

    /// Splits this segment by a plane identified by its normal `local_axis` and
    /// the `bias` (i.e. the plane passes through the point equal to `normal * bias`).
    pub fn local_split(
        &self,
        local_axis: &UnitVector<Real>,
        bias: Real,
        epsilon: Real,
    ) -> SplitResult<Self> {
        self.local_split_and_get_intersection(local_axis, bias, epsilon)
            .0
    }

    /// Split a segment with a plane.
    ///
    /// This returns the result of the splitting operation, as well as
    /// the intersection point (and barycentric coordinate of this point)
    /// with the plane. The intersection point is `None` if the plane is
    /// parallel or near-parallel to the segment.
    pub fn local_split_and_get_intersection(
        &self,
        local_axis: &UnitVector<Real>,
        bias: Real,
        epsilon: Real,
    ) -> (SplitResult<Self>, Option<(Point<Real>, Real)>) {
        let dir = self.b - self.a;
        let a = bias - local_axis.dot(&self.a.coords);
        let b = local_axis.dot(&dir);
        let bcoord = a / b;
        let dir_norm = dir.norm();

        if relative_eq!(b, 0.0)
            || bcoord * dir_norm <= epsilon
            || bcoord * dir_norm >= dir_norm - epsilon
        {
            if a >= 0.0 {
                (SplitResult::Negative, None)
            } else {
                (SplitResult::Positive, None)
            }
        } else {
            let intersection = self.a + dir * bcoord;
            let s1 = Segment::new(self.a, intersection);
            let s2 = Segment::new(intersection, self.b);
            if a >= 0.0 {
                (SplitResult::Pair(s1, s2), Some((intersection, bcoord)))
            } else {
                (SplitResult::Pair(s2, s1), Some((intersection, bcoord)))
            }
        }
    }
}