parry3d/shape/
triangle.rs

1//! Definition of the triangle shape.
2
3use crate::math::{Isometry, Point, Real, Vector};
4use crate::shape::SupportMap;
5use crate::shape::{PolygonalFeature, Segment};
6use crate::utils;
7
8use core::mem;
9use na::{self, ComplexField, Unit};
10use num::Zero;
11
12#[cfg(feature = "dim3")]
13use {crate::shape::FeatureId, core::f64};
14
15#[cfg(feature = "dim2")]
16use crate::shape::PackedFeatureId;
17
18#[cfg(feature = "rkyv")]
19use rkyv::{bytecheck, CheckBytes};
20
21/// A triangle shape.
22#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
23#[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
24#[cfg_attr(
25    feature = "rkyv",
26    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes),
27    archive(as = "Self")
28)]
29#[derive(PartialEq, Debug, Copy, Clone, Default)]
30#[repr(C)]
31pub struct Triangle {
32    /// The triangle first point.
33    pub a: Point<Real>,
34    /// The triangle second point.
35    pub b: Point<Real>,
36    /// The triangle third point.
37    pub c: Point<Real>,
38}
39
40/// Description of the location of a point on a triangle.
41#[derive(Copy, Clone, Debug)]
42pub enum TrianglePointLocation {
43    /// The point lies on a vertex.
44    OnVertex(u32),
45    /// The point lies on an edge.
46    ///
47    /// The 0-st edge is the segment AB.
48    /// The 1-st edge is the segment BC.
49    /// The 2-nd edge is the segment AC.
50    // XXX: it appears the conversion of edge indexing here does not match the
51    // convention of edge indexing for the `fn edge` method (from the ConvexPolyhedron impl).
52    OnEdge(u32, [Real; 2]),
53    /// The point lies on the triangle interior.
54    ///
55    /// The integer indicates on which side of the face the point is. 0 indicates the point
56    /// is on the half-space toward the CW normal of the triangle. 1 indicates the point is on the other
57    /// half-space. This is always set to 0 in 2D.
58    OnFace(u32, [Real; 3]),
59    /// The point lies on the triangle interior (for "solid" point queries).
60    OnSolid,
61}
62
63impl TrianglePointLocation {
64    /// The barycentric coordinates corresponding to this point location.
65    ///
66    /// Returns `None` if the location is `TrianglePointLocation::OnSolid`.
67    pub fn barycentric_coordinates(&self) -> Option<[Real; 3]> {
68        let mut bcoords = [0.0; 3];
69
70        match self {
71            TrianglePointLocation::OnVertex(i) => bcoords[*i as usize] = 1.0,
72            TrianglePointLocation::OnEdge(i, uv) => {
73                let idx = match i {
74                    0 => (0, 1),
75                    1 => (1, 2),
76                    2 => (0, 2),
77                    _ => unreachable!(),
78                };
79
80                bcoords[idx.0] = uv[0];
81                bcoords[idx.1] = uv[1];
82            }
83            TrianglePointLocation::OnFace(_, uvw) => {
84                bcoords[0] = uvw[0];
85                bcoords[1] = uvw[1];
86                bcoords[2] = uvw[2];
87            }
88            TrianglePointLocation::OnSolid => {
89                return None;
90            }
91        }
92
93        Some(bcoords)
94    }
95
96    /// Returns `true` if the point is located on the relative interior of the triangle.
97    pub fn is_on_face(&self) -> bool {
98        matches!(*self, TrianglePointLocation::OnFace(..))
99    }
100}
101
102/// Orientation of a triangle.
103#[derive(Copy, Clone, Debug, PartialEq, Eq)]
104pub enum TriangleOrientation {
105    /// Orientation with a clockwise orientation, i.e., with a positive signed area.
106    Clockwise,
107    /// Orientation with a clockwise orientation, i.e., with a negative signed area.
108    CounterClockwise,
109    /// Degenerate triangle.
110    Degenerate,
111}
112
113impl From<[Point<Real>; 3]> for Triangle {
114    fn from(arr: [Point<Real>; 3]) -> Self {
115        *Self::from_array(&arr)
116    }
117}
118
119impl Triangle {
120    /// Creates a triangle from three points.
121    #[inline]
122    pub fn new(a: Point<Real>, b: Point<Real>, c: Point<Real>) -> Triangle {
123        Triangle { a, b, c }
124    }
125
126    /// Creates the reference to a triangle from the reference to an array of three points.
127    pub fn from_array(arr: &[Point<Real>; 3]) -> &Triangle {
128        unsafe { mem::transmute(arr) }
129    }
130
131    /// Reference to an array containing the three vertices of this triangle.
132    #[inline]
133    pub fn vertices(&self) -> &[Point<Real>; 3] {
134        unsafe { mem::transmute(self) }
135    }
136
137    /// The normal of this triangle assuming it is oriented ccw.
138    ///
139    /// The normal points such that it is collinear to `AB × AC` (where `×` denotes the cross
140    /// product).
141    #[inline]
142    #[cfg(feature = "dim3")]
143    pub fn normal(&self) -> Option<Unit<Vector<Real>>> {
144        Unit::try_new(self.scaled_normal(), crate::math::DEFAULT_EPSILON)
145    }
146
147    /// The three edges of this triangle: [AB, BC, CA].
148    #[inline]
149    pub fn edges(&self) -> [Segment; 3] {
150        [
151            Segment::new(self.a, self.b),
152            Segment::new(self.b, self.c),
153            Segment::new(self.c, self.a),
154        ]
155    }
156
157    /// Computes a scaled version of this triangle.
158    pub fn scaled(self, scale: &Vector<Real>) -> Self {
159        Self::new(
160            na::Scale::from(*scale) * self.a,
161            na::Scale::from(*scale) * self.b,
162            na::Scale::from(*scale) * self.c,
163        )
164    }
165
166    /// Returns a new triangle with vertices transformed by `m`.
167    #[inline]
168    pub fn transformed(&self, m: &Isometry<Real>) -> Self {
169        Triangle::new(m * self.a, m * self.b, m * self.c)
170    }
171
172    /// The three edges scaled directions of this triangle: [B - A, C - B, A - C].
173    #[inline]
174    pub fn edges_scaled_directions(&self) -> [Vector<Real>; 3] {
175        [self.b - self.a, self.c - self.b, self.a - self.c]
176    }
177
178    /// Return the edge segment of this cuboid with a normal cone containing
179    /// a direction that that maximizes the dot product with `local_dir`.
180    pub fn local_support_edge_segment(&self, dir: Vector<Real>) -> Segment {
181        let dots = na::Vector3::new(
182            dir.dot(&self.a.coords),
183            dir.dot(&self.b.coords),
184            dir.dot(&self.c.coords),
185        );
186
187        match dots.imin() {
188            0 => Segment::new(self.b, self.c),
189            1 => Segment::new(self.c, self.a),
190            _ => Segment::new(self.a, self.b),
191        }
192    }
193
194    /// Return the face of this triangle with a normal that maximizes
195    /// the dot product with `dir`.
196    #[cfg(feature = "dim3")]
197    pub fn support_face(&self, _dir: Vector<Real>) -> PolygonalFeature {
198        PolygonalFeature::from(*self)
199    }
200
201    /// Return the face of this triangle with a normal that maximizes
202    /// the dot product with `dir`.
203    #[cfg(feature = "dim2")]
204    pub fn support_face(&self, dir: Vector<Real>) -> PolygonalFeature {
205        let mut best = 0;
206        let mut best_dot = -Real::MAX;
207
208        for (i, tangent) in self.edges_scaled_directions().iter().enumerate() {
209            let normal = Vector::new(tangent.y, -tangent.x);
210            if let Some(normal) = Unit::try_new(normal, 0.0) {
211                let dot = normal.dot(&dir);
212                if normal.dot(&dir) > best_dot {
213                    best = i;
214                    best_dot = dot;
215                }
216            }
217        }
218
219        let pts = self.vertices();
220        let i1 = best;
221        let i2 = (best + 1) % 3;
222
223        PolygonalFeature {
224            vertices: [pts[i1], pts[i2]],
225            vids: PackedFeatureId::vertices([i1 as u32, i2 as u32]),
226            fid: PackedFeatureId::face(i1 as u32),
227            num_vertices: 2,
228        }
229    }
230
231    /// A vector normal of this triangle.
232    ///
233    /// The vector points such that it is collinear to `AB × AC` (where `×` denotes the cross
234    /// product).
235    ///
236    /// Note that on thin triangles the calculated normals can suffer from numerical issues.
237    /// For a more robust (but more computationally expensive) normal calculation, see
238    /// [`Triangle::robust_scaled_normal`].
239    #[inline]
240    #[cfg(feature = "dim3")]
241    pub fn scaled_normal(&self) -> Vector<Real> {
242        let ab = self.b - self.a;
243        let ac = self.c - self.a;
244        ab.cross(&ac)
245    }
246
247    /// Find a triangle normal more robustly than with [`Triangle::scaled_normal`].
248    ///
249    /// Thin triangles can cause numerical issues when computing its normal. This method accounts
250    /// for these numerical issues more robustly than [`Triangle::scaled_normal`], but is more
251    /// computationally expensive.
252    #[inline]
253    #[cfg(feature = "dim3")]
254    pub fn robust_scaled_normal(&self) -> na::Vector3<Real> {
255        let pts = self.vertices();
256        let best_vertex = self.angle_closest_to_90();
257        let d1 = pts[(best_vertex + 2) % 3] - pts[(best_vertex + 1) % 3];
258        let d2 = pts[best_vertex] - pts[(best_vertex + 1) % 3];
259
260        d1.cross(&d2)
261    }
262
263    /// Similar to [`Triangle::robust_scaled_normal`], but returns the unit length normal.
264    #[inline]
265    #[cfg(feature = "dim3")]
266    pub fn robust_normal(&self) -> na::Vector3<Real> {
267        self.robust_scaled_normal().normalize()
268    }
269
270    /// Computes the extents of this triangle on the given direction.
271    ///
272    /// This computes the min and max values of the dot products between each
273    /// vertex of this triangle and `dir`.
274    #[inline]
275    pub fn extents_on_dir(&self, dir: &Unit<Vector<Real>>) -> (Real, Real) {
276        let a = self.a.coords.dot(dir);
277        let b = self.b.coords.dot(dir);
278        let c = self.c.coords.dot(dir);
279
280        if a > b {
281            if b > c {
282                (c, a)
283            } else if a > c {
284                (b, a)
285            } else {
286                (b, c)
287            }
288        } else {
289            // b >= a
290            if a > c {
291                (c, b)
292            } else if b > c {
293                (a, b)
294            } else {
295                (a, c)
296            }
297        }
298    }
299    //
300    // #[cfg(feature = "dim3")]
301    // fn support_feature_id_toward(&self, local_dir: &Unit<Vector<Real>>, eps: Real) -> FeatureId {
302    //     if let Some(normal) = self.normal() {
303    //         let (seps, ceps) = ComplexField::sin_cos(eps);
304    //
305    //         let normal_dot = local_dir.dot(&*normal);
306    //         if normal_dot >= ceps {
307    //             FeatureId::Face(0)
308    //         } else if normal_dot <= -ceps {
309    //             FeatureId::Face(1)
310    //         } else {
311    //             let edges = self.edges();
312    //             let mut dots = [0.0; 3];
313    //
314    //             let dir1 = edges[0].direction();
315    //             if let Some(dir1) = dir1 {
316    //                 dots[0] = dir1.dot(local_dir);
317    //
318    //                 if dots[0].abs() < seps {
319    //                     return FeatureId::Edge(0);
320    //                 }
321    //             }
322    //
323    //             let dir2 = edges[1].direction();
324    //             if let Some(dir2) = dir2 {
325    //                 dots[1] = dir2.dot(local_dir);
326    //
327    //                 if dots[1].abs() < seps {
328    //                     return FeatureId::Edge(1);
329    //                 }
330    //             }
331    //
332    //             let dir3 = edges[2].direction();
333    //             if let Some(dir3) = dir3 {
334    //                 dots[2] = dir3.dot(local_dir);
335    //
336    //                 if dots[2].abs() < seps {
337    //                     return FeatureId::Edge(2);
338    //                 }
339    //             }
340    //
341    //             if dots[0] > 0.0 && dots[1] < 0.0 {
342    //                 FeatureId::Vertex(1)
343    //             } else if dots[1] > 0.0 && dots[2] < 0.0 {
344    //                 FeatureId::Vertex(2)
345    //             } else {
346    //                 FeatureId::Vertex(0)
347    //             }
348    //         }
349    //     } else {
350    //         FeatureId::Vertex(0)
351    //     }
352    // }
353
354    /// The area of this triangle.
355    #[inline]
356    pub fn area(&self) -> Real {
357        // Kahan's formula.
358        let a = na::distance(&self.a, &self.b);
359        let b = na::distance(&self.b, &self.c);
360        let c = na::distance(&self.c, &self.a);
361
362        let (c, b, a) = utils::sort3(&a, &b, &c);
363        let a = *a;
364        let b = *b;
365        let c = *c;
366
367        let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));
368
369        // We take the max(0.0) because it can be slightly negative
370        // because of numerical errors due to almost-degenerate triangles.
371        ComplexField::sqrt(sqr.max(0.0)) * 0.25
372    }
373
374    /// Computes the unit angular inertia of this triangle.
375    #[cfg(feature = "dim2")]
376    pub fn unit_angular_inertia(&self) -> Real {
377        let factor = 1.0 / 6.0;
378
379        // Algorithm adapted from Box2D
380        let e1 = self.b - self.a;
381        let e2 = self.c - self.a;
382
383        let intx2 = e1.x * e1.x + e2.x * e1.x + e2.x * e2.x;
384        let inty2 = e1.y * e1.y + e2.y * e1.y + e2.y * e2.y;
385        factor * (intx2 + inty2)
386    }
387
388    /// The geometric center of this triangle.
389    #[inline]
390    pub fn center(&self) -> Point<Real> {
391        utils::center(&[self.a, self.b, self.c])
392    }
393
394    /// The perimeter of this triangle.
395    #[inline]
396    pub fn perimeter(&self) -> Real {
397        na::distance(&self.a, &self.b)
398            + na::distance(&self.b, &self.c)
399            + na::distance(&self.c, &self.a)
400    }
401
402    /// The circumcircle of this triangle.
403    pub fn circumcircle(&self) -> (Point<Real>, Real) {
404        let a = self.a - self.c;
405        let b = self.b - self.c;
406
407        let na = a.norm_squared();
408        let nb = b.norm_squared();
409
410        let dab = a.dot(&b);
411
412        let denom = 2.0 * (na * nb - dab * dab);
413
414        if denom.is_zero() {
415            // The triangle is degenerate (the three points are collinear).
416            // So we find the longest segment and take its center.
417            let c = self.a - self.b;
418            let nc = c.norm_squared();
419
420            if nc >= na && nc >= nb {
421                // Longest segment: [&self.a, &self.b]
422                (
423                    na::center(&self.a, &self.b),
424                    ComplexField::sqrt(nc) / na::convert::<f64, Real>(2.0f64),
425                )
426            } else if na >= nb && na >= nc {
427                // Longest segment: [&self.a, pc]
428                (
429                    na::center(&self.a, &self.c),
430                    ComplexField::sqrt(na) / na::convert::<f64, Real>(2.0f64),
431                )
432            } else {
433                // Longest segment: [&self.b, &self.c]
434                (
435                    na::center(&self.b, &self.c),
436                    ComplexField::sqrt(nb) / na::convert::<f64, Real>(2.0f64),
437                )
438            }
439        } else {
440            let k = b * na - a * nb;
441
442            let center = self.c + (a * k.dot(&b) - b * k.dot(&a)) / denom;
443            let radius = na::distance(&self.a, &center);
444
445            (center, radius)
446        }
447    }
448
449    /// Tests if this triangle is affinely dependent, i.e., its points are almost aligned.
450    #[cfg(feature = "dim3")]
451    pub fn is_affinely_dependent(&self) -> bool {
452        const EPS: Real = crate::math::DEFAULT_EPSILON * 100.0;
453
454        let p1p2 = self.b - self.a;
455        let p1p3 = self.c - self.a;
456        relative_eq!(p1p2.cross(&p1p3).norm_squared(), 0.0, epsilon = EPS * EPS)
457
458        // relative_eq!(
459        //     self.area(),
460        //     0.0,
461        //     epsilon = EPS * self.perimeter()
462        // )
463    }
464
465    /// Is this triangle degenerate or almost degenerate?
466    #[cfg(feature = "dim3")]
467    pub fn is_affinely_dependent_eps(&self, eps: Real) -> bool {
468        let p1p2 = self.b - self.a;
469        let p1p3 = self.c - self.a;
470        relative_eq!(
471            p1p2.cross(&p1p3).norm(),
472            0.0,
473            epsilon = eps * p1p2.norm().max(p1p3.norm())
474        )
475
476        // relative_eq!(
477        //     self.area(),
478        //     0.0,
479        //     epsilon = EPS * self.perimeter()
480        // )
481    }
482
483    /// Tests if a point is inside of this triangle.
484    #[cfg(feature = "dim2")]
485    pub fn contains_point(&self, p: &Point<Real>) -> bool {
486        let ab = self.b - self.a;
487        let bc = self.c - self.b;
488        let ca = self.a - self.c;
489        let sgn1 = ab.perp(&(p - self.a));
490        let sgn2 = bc.perp(&(p - self.b));
491        let sgn3 = ca.perp(&(p - self.c));
492        sgn1.signum() * sgn2.signum() >= 0.0
493            && sgn1.signum() * sgn3.signum() >= 0.0
494            && sgn2.signum() * sgn3.signum() >= 0.0
495    }
496
497    /// Tests if a point is inside of this triangle.
498    #[cfg(feature = "dim3")]
499    pub fn contains_point(&self, p: &Point<Real>) -> bool {
500        const EPS: Real = crate::math::DEFAULT_EPSILON;
501
502        let vb = self.b - self.a;
503        let vc = self.c - self.a;
504        let vp = p - self.a;
505
506        let n = vc.cross(&vb);
507        let n_norm = n.norm_squared();
508        if n_norm < EPS || vp.dot(&n).abs() > EPS * n_norm {
509            // the triangle is degenerate or the
510            // point does not lie on the same plane as the triangle.
511            return false;
512        }
513
514        // We are seeking B, C such that vp = vb * B + vc * C .
515        // If B and C are both in [0, 1] and B + C <= 1 then p is in the triangle.
516        //
517        // We can project this equation along a vector nb coplanar to the triangle
518        // and perpendicular to vb:
519        // vp.dot(nb) = vb.dot(nb) * B + vc.dot(nb) * C
520        //     => C = vp.dot(nb) / vc.dot(nb)
521        // and similarly for B.
522        //
523        // In order to avoid divisions and sqrts we scale both B and C - so
524        // b = vb.dot(nc) * B and c = vc.dot(nb) * C - this results in harder-to-follow math but
525        // hopefully fast code.
526
527        let nb = vb.cross(&n);
528        let nc = vc.cross(&n);
529
530        let signed_blim = vb.dot(&nc);
531        let b = vp.dot(&nc) * signed_blim.signum();
532        let blim = signed_blim.abs();
533
534        let signed_clim = vc.dot(&nb);
535        let c = vp.dot(&nb) * signed_clim.signum();
536        let clim = signed_clim.abs();
537
538        c >= 0.0 && c <= clim && b >= 0.0 && b <= blim && c * blim + b * clim <= blim * clim
539    }
540
541    /// The normal of the given feature of this shape.
542    #[cfg(feature = "dim3")]
543    pub fn feature_normal(&self, _: FeatureId) -> Option<Unit<Vector<Real>>> {
544        self.normal()
545    }
546
547    /// The orientation of the triangle, based on its signed area.
548    ///
549    /// Returns `TriangleOrientation::Degenerate` if the triangle’s area is
550    /// smaller than `epsilon`.
551    #[cfg(feature = "dim2")]
552    pub fn orientation(&self, epsilon: Real) -> TriangleOrientation {
553        let area2 = (self.b - self.a).perp(&(self.c - self.a));
554        // println!("area2: {}", area2);
555        if area2 > epsilon {
556            TriangleOrientation::CounterClockwise
557        } else if area2 < -epsilon {
558            TriangleOrientation::Clockwise
559        } else {
560            TriangleOrientation::Degenerate
561        }
562    }
563
564    /// The orientation of the 2D triangle, based on its signed area.
565    ///
566    /// Returns `TriangleOrientation::Degenerate` if the triangle’s area is
567    /// smaller than `epsilon`.
568    pub fn orientation2d(
569        a: &na::Point2<Real>,
570        b: &na::Point2<Real>,
571        c: &na::Point2<Real>,
572        epsilon: Real,
573    ) -> TriangleOrientation {
574        let area2 = (b - a).perp(&(c - a));
575        // println!("area2: {}", area2);
576        if area2 > epsilon {
577            TriangleOrientation::CounterClockwise
578        } else if area2 < -epsilon {
579            TriangleOrientation::Clockwise
580        } else {
581            TriangleOrientation::Degenerate
582        }
583    }
584
585    /// Find the index of a vertex in this triangle, such that the two
586    /// edges incident in that vertex form the angle closest to 90
587    /// degrees in the triangle.
588    pub fn angle_closest_to_90(&self) -> usize {
589        let points = self.vertices();
590        let mut best_cos = 2.0;
591        let mut selected_i = 0;
592
593        for i in 0..3 {
594            let d1 = (points[i] - points[(i + 1) % 3]).normalize();
595            let d2 = (points[(i + 2) % 3] - points[(i + 1) % 3]).normalize();
596
597            let cos_abs = d1.dot(&d2).abs();
598
599            if cos_abs < best_cos {
600                best_cos = cos_abs;
601                selected_i = i;
602            }
603        }
604
605        selected_i
606    }
607
608    /// Reverse the orientation of this triangle by swapping b and c.
609    pub fn reverse(&mut self) {
610        mem::swap(&mut self.b, &mut self.c);
611    }
612}
613
614impl SupportMap for Triangle {
615    #[inline]
616    fn local_support_point(&self, dir: &Vector<Real>) -> Point<Real> {
617        let d1 = self.a.coords.dot(dir);
618        let d2 = self.b.coords.dot(dir);
619        let d3 = self.c.coords.dot(dir);
620
621        if d1 > d2 {
622            if d1 > d3 {
623                self.a
624            } else {
625                self.c
626            }
627        } else if d2 > d3 {
628            self.b
629        } else {
630            self.c
631        }
632    }
633}
634
635/*
636#[cfg(feature = "dim3")]
637impl ConvexPolyhedron for Triangle {
638    fn vertex(&self, id: FeatureId) -> Point<Real> {
639        match id.unwrap_vertex() {
640            0 => self.a,
641            1 => self.b,
642            2 => self.c,
643            _ => panic!("Triangle vertex index out of bounds."),
644        }
645    }
646    fn edge(&self, id: FeatureId) -> (Point<Real>, Point<Real>, FeatureId, FeatureId) {
647        match id.unwrap_edge() {
648            0 => (self.a, self.b, FeatureId::Vertex(0), FeatureId::Vertex(1)),
649            1 => (self.b, self.c, FeatureId::Vertex(1), FeatureId::Vertex(2)),
650            2 => (self.c, self.a, FeatureId::Vertex(2), FeatureId::Vertex(0)),
651            _ => panic!("Triangle edge index out of bounds."),
652        }
653    }
654
655    fn face(&self, id: FeatureId, face: &mut ConvexPolygonalFeature) {
656        face.clear();
657
658        if let Some(normal) = self.normal() {
659            face.set_feature_id(id);
660
661            match id.unwrap_face() {
662                0 => {
663                    face.push(self.a, FeatureId::Vertex(0));
664                    face.push(self.b, FeatureId::Vertex(1));
665                    face.push(self.c, FeatureId::Vertex(2));
666                    face.push_edge_feature_id(FeatureId::Edge(0));
667                    face.push_edge_feature_id(FeatureId::Edge(1));
668                    face.push_edge_feature_id(FeatureId::Edge(2));
669                    face.set_normal(normal);
670                }
671                1 => {
672                    face.push(self.a, FeatureId::Vertex(0));
673                    face.push(self.c, FeatureId::Vertex(2));
674                    face.push(self.b, FeatureId::Vertex(1));
675                    face.push_edge_feature_id(FeatureId::Edge(2));
676                    face.push_edge_feature_id(FeatureId::Edge(1));
677                    face.push_edge_feature_id(FeatureId::Edge(0));
678                    face.set_normal(-normal);
679                }
680                _ => unreachable!(),
681            }
682
683            face.recompute_edge_normals();
684        } else {
685            face.push(self.a, FeatureId::Vertex(0));
686            face.set_feature_id(FeatureId::Vertex(0));
687        }
688    }
689
690    fn support_face_toward(
691        &self,
692        m: &Isometry<Real>,
693        dir: &Unit<Vector<Real>>,
694        face: &mut ConvexPolygonalFeature,
695    ) {
696        let normal = self.scaled_normal();
697
698        if normal.dot(&*dir) >= 0.0 {
699            ConvexPolyhedron::face(self, FeatureId::Face(0), face);
700        } else {
701            ConvexPolyhedron::face(self, FeatureId::Face(1), face);
702        }
703        face.transform_by(m)
704    }
705
706    fn support_feature_toward(
707        &self,
708        transform: &Isometry<Real>,
709        dir: &Unit<Vector<Real>>,
710        eps: Real,
711        out: &mut ConvexPolygonalFeature,
712    ) {
713        out.clear();
714        let tri = self.transformed(transform);
715        let feature = tri.support_feature_id_toward(dir, eps);
716
717        match feature {
718            FeatureId::Vertex(_) => {
719                let v = tri.vertex(feature);
720                out.push(v, feature);
721                out.set_feature_id(feature);
722            }
723            FeatureId::Edge(_) => {
724                let (a, b, fa, fb) = tri.edge(feature);
725                out.push(a, fa);
726                out.push(b, fb);
727                out.push_edge_feature_id(feature);
728                out.set_feature_id(feature);
729            }
730            FeatureId::Face(_) => tri.face(feature, out),
731            _ => unreachable!(),
732        }
733    }
734
735    fn support_feature_id_toward(&self, local_dir: &Unit<Vector<Real>>) -> FeatureId {
736        self.support_feature_id_toward(local_dir, na::convert::<f64, Real>(f64::consts::PI / 180.0))
737    }
738}
739*/
740
741#[cfg(feature = "dim2")]
742#[cfg(test)]
743mod test {
744    use crate::shape::Triangle;
745    use na::Point2;
746
747    #[test]
748    fn test_triangle_area() {
749        let pa = Point2::new(5.0, 0.0);
750        let pb = Point2::new(0.0, 0.0);
751        let pc = Point2::new(0.0, 4.0);
752
753        assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0));
754    }
755
756    #[test]
757    fn test_triangle_contains_point() {
758        let tri = Triangle::new(
759            Point2::new(5.0, 0.0),
760            Point2::new(0.0, 0.0),
761            Point2::new(0.0, 4.0),
762        );
763
764        assert!(tri.contains_point(&Point2::new(1.0, 1.0)));
765        assert!(!tri.contains_point(&Point2::new(-1.0, 1.0)));
766    }
767
768    #[test]
769    fn test_obtuse_triangle_contains_point() {
770        let tri = Triangle::new(
771            Point2::new(-10.0, 10.0),
772            Point2::new(0.0, 0.0),
773            Point2::new(20.0, 0.0),
774        );
775
776        assert!(tri.contains_point(&Point2::new(-3.0, 5.0)));
777        assert!(tri.contains_point(&Point2::new(5.0, 1.0)));
778        assert!(!tri.contains_point(&Point2::new(0.0, -1.0)));
779    }
780}
781
782#[cfg(feature = "dim3")]
783#[cfg(test)]
784mod test {
785    use crate::math::Real;
786    use crate::shape::Triangle;
787    use na::Point3;
788
789    #[test]
790    fn test_triangle_area() {
791        let pa = Point3::new(0.0, 5.0, 0.0);
792        let pb = Point3::new(0.0, 0.0, 0.0);
793        let pc = Point3::new(0.0, 0.0, 4.0);
794
795        assert!(relative_eq!(Triangle::new(pa, pb, pc).area(), 10.0));
796    }
797
798    #[test]
799    fn test_triangle_contains_point() {
800        let tri = Triangle::new(
801            Point3::new(0.0, 5.0, 0.0),
802            Point3::new(0.0, 0.0, 0.0),
803            Point3::new(0.0, 0.0, 4.0),
804        );
805
806        assert!(tri.contains_point(&Point3::new(0.0, 1.0, 1.0)));
807        assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 1.0)));
808    }
809
810    #[test]
811    fn test_obtuse_triangle_contains_point() {
812        let tri = Triangle::new(
813            Point3::new(-10.0, 10.0, 0.0),
814            Point3::new(0.0, 0.0, 0.0),
815            Point3::new(20.0, 0.0, 0.0),
816        );
817
818        assert!(tri.contains_point(&Point3::new(-3.0, 5.0, 0.0)));
819        assert!(tri.contains_point(&Point3::new(5.0, 1.0, 0.0)));
820        assert!(!tri.contains_point(&Point3::new(0.0, -1.0, 0.0)));
821    }
822
823    #[test]
824    fn test_3dtriangle_contains_point() {
825        let o = Point3::new(0.0, 0.0, 0.0);
826        let pa = Point3::new(1.2, 1.4, 5.6);
827        let pb = Point3::new(1.5, 6.7, 1.9);
828        let pc = Point3::new(5.0, 2.1, 1.3);
829
830        let tri = Triangle::new(pa, pb, pc);
831
832        let va = pa - o;
833        let vb = pb - o;
834        let vc = pc - o;
835
836        let n = (pa - pb).cross(&(pb - pc));
837
838        // This is a simple algorithm for generating points that are inside the
839        // triangle: o + (va * alpha + vb * beta + vc * gamma) is always inside the
840        // triangle if:
841        // * each of alpha, beta, gamma is in (0, 1)
842        // * alpha + beta + gamma = 1
843        let contained_p = o + (va * 0.2 + vb * 0.3 + vc * 0.5);
844        let not_contained_coplanar_p = o + (va * -0.5 + vb * 0.8 + vc * 0.7);
845        let not_coplanar_p = o + (va * 0.2 + vb * 0.3 + vc * 0.5) + n * 0.1;
846        let not_coplanar_p2 = o + (va * -0.5 + vb * 0.8 + vc * 0.7) + n * 0.1;
847        assert!(tri.contains_point(&contained_p));
848        assert!(!tri.contains_point(&not_contained_coplanar_p));
849        assert!(!tri.contains_point(&not_coplanar_p));
850        assert!(!tri.contains_point(&not_coplanar_p2));
851
852        // Test that points that are clearly within the triangle as seen as such, by testing
853        // a number of points along a line intersecting the triangle.
854        for i in -50i16..150 {
855            let a = 0.15;
856            let b = 0.01 * Real::from(i); // b ranges from -0.5 to 1.5
857            let c = 1.0 - a - b;
858            let p = o + (va * a + vb * b + vc * c);
859
860            match i {
861                ii if ii < 0 || ii > 85 => assert!(
862                    !tri.contains_point(&p),
863                    "Should not contain: i = {}, b = {}",
864                    i,
865                    b
866                ),
867                ii if ii > 0 && ii < 85 => assert!(
868                    tri.contains_point(&p),
869                    "Should contain: i = {}, b = {}",
870                    i,
871                    b
872                ),
873                _ => (), // Points at the edge may be seen as inside or outside
874            }
875        }
876    }
877}