ncollide3d/utils/
triangle.rs

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
use crate::math::Point;
use crate::utils;
use na;
use simba::scalar::RealField;

/// Computes the area of a triangle.
#[inline]
pub fn triangle_area<N: RealField + Copy>(pa: &Point<N>, pb: &Point<N>, pc: &Point<N>) -> N {
    // Kahan's formula.
    let mut a = na::distance(pa, pb);
    let mut b = na::distance(pb, pc);
    let mut c = na::distance(pc, pa);

    let (c, b, a) = utils::sort3(&mut a, &mut b, &mut c);
    let a = *a;
    let b = *b;
    let c = *c;

    let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));

    sqr.sqrt() * na::convert(0.25)
}

/// Computes the center of a triangle.
#[inline]
pub fn triangle_center<N: RealField + Copy>(
    pa: &Point<N>,
    pb: &Point<N>,
    pc: &Point<N>,
) -> Point<N> {
    utils::center(&[*pa, *pb, *pc])
}

/// Computes the perimeter of a triangle.
#[inline]
pub fn triangle_perimeter<N: RealField + Copy>(pa: &Point<N>, pb: &Point<N>, pc: &Point<N>) -> N {
    na::distance(pa, pb) + na::distance(pb, pc) + na::distance(pc, pa)
}

/// Computes the circumcircle of a triangle.
pub fn circumcircle<N: RealField + Copy>(
    pa: &Point<N>,
    pb: &Point<N>,
    pc: &Point<N>,
) -> (Point<N>, N) {
    let a = *pa - *pc;
    let b = *pb - *pc;

    let na = a.norm_squared();
    let nb = b.norm_squared();

    let dab = a.dot(&b);

    let _2: N = na::convert(2.0);
    let denom = _2 * (na * nb - dab * dab);

    if denom.is_zero() {
        // The triangle is degenerate (the three points are colinear).
        // So we find the longest segment and take its center.
        let c = *pa - *pb;
        let nc = c.norm_squared();

        if nc >= na && nc >= nb {
            // Longest segment: [pa, pb]
            (na::center(pa, pb), nc.sqrt() / na::convert(2.0f64))
        } else if na >= nb && na >= nc {
            // Longest segment: [pa, pc]
            (na::center(pa, pc), na.sqrt() / na::convert(2.0f64))
        } else {
            // Longest segment: [pb, pc]
            (na::center(pb, pc), nb.sqrt() / na::convert(2.0f64))
        }
    } else {
        let k = b * na - a * nb;

        let center = *pc + (a * k.dot(&b) - b * k.dot(&a)) / denom;
        let radius = na::distance(pa, &center);

        (center, radius)
    }
}

/// Tests if three 3D points are approximately aligned.
#[cfg(feature = "dim3")]
pub fn is_affinely_dependent_triangle<N: RealField + Copy>(
    p1: &Point<N>,
    p2: &Point<N>,
    p3: &Point<N>,
) -> bool {
    let p1p2 = *p2 - *p1;
    let p1p3 = *p3 - *p1;

    // FIXME: use this as nalgebra standard epsilon?
    let _eps = N::default_epsilon(); // FIXME: use Float::epsilon instead?
    let _eps_tol = _eps * na::convert(100.0f64);

    relative_eq!(
        p1p2.cross(&p1p3).norm_squared(),
        na::zero(),
        epsilon = _eps_tol * _eps_tol
    )
}

/// Tests if a point is inside of a triangle.
pub fn is_point_in_triangle<N: RealField + Copy>(
    p: &Point<N>,
    p1: &Point<N>,
    p2: &Point<N>,
    p3: &Point<N>,
) -> bool {
    let p1p2 = *p2 - *p1;
    let p2p3 = *p3 - *p2;
    let p3p1 = *p1 - *p3;

    let p1p = *p - *p1;
    let p2p = *p - *p2;
    let p3p = *p - *p3;

    let d11 = p1p.dot(&p1p2);
    let d12 = p2p.dot(&p2p3);
    let d13 = p3p.dot(&p3p1);

    d11 >= na::zero()
        && d11 <= p1p2.norm_squared()
        && d12 >= na::zero()
        && d12 <= p2p3.norm_squared()
        && d13 >= na::zero()
        && d13 <= p3p1.norm_squared()
}

#[cfg(feature = "dim3")]
#[cfg(test)]
mod test {
    use na::Point3;

    #[test]
    fn test_triangle_area() {
        let pa = Point3::new(0.0f64, 5.0, 0.0);
        let pb = Point3::new(0.0f64, 0.0, 0.0);
        let pc = Point3::new(0.0f64, 0.0, 4.0);

        assert!(relative_eq!(super::triangle_area(&pa, &pb, &pc), 10.0));
    }
}