fj_math/
triangle.rs

1use parry3d_f64::query::{Ray, RayCast as _};
2
3use crate::Vector;
4
5use super::{Point, Scalar};
6
7/// A triangle
8///
9/// The dimensionality of the triangle is defined by the const generic `D`
10/// parameter.
11#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Hash, Ord, PartialOrd)]
12#[repr(C)]
13pub struct Triangle<const D: usize> {
14    points: [Point<D>; 3],
15}
16
17impl<const D: usize> Triangle<D> {
18    /// Construct a triangle from three points
19    ///
20    /// Returns an error, if the points don't form a triangle.
21    pub fn from_points(
22        points: [impl Into<Point<D>>; 3],
23    ) -> Result<Self, NotATriangle<D>> {
24        let points = points.map(Into::into);
25
26        let area = {
27            let [a, b, c] = points.map(Point::to_xyz);
28            (b - a).cross(&(c - a)).magnitude()
29        };
30
31        // A triangle is not valid if it doesn't span any area
32        if area != Scalar::from(0.0) {
33            Ok(Self { points })
34        } else {
35            Err(NotATriangle { points })
36        }
37    }
38
39    /// Access the triangle's points
40    pub fn points(&self) -> [Point<D>; 3] {
41        self.points
42    }
43
44    /// Normalize the triangle
45    ///
46    /// Returns a new `Triangle` instance with the same points, but the points
47    /// ordered such that they are ordered according to their `Ord`/`PartialOrd`
48    /// implementation.
49    ///
50    /// This is useful for comparing triangles, where the order of points is not
51    /// important.
52    pub fn normalize(mut self) -> Self {
53        self.points.sort();
54        self
55    }
56}
57
58impl Triangle<2> {
59    /// Returns the direction of the line through the points of the triangle.
60    pub fn winding(&self) -> Winding {
61        let [pa, pb, pc] = self.points.map(|point| robust::Coord {
62            x: point.u,
63            y: point.v,
64        });
65        let orient2d = robust::orient2d(pa, pb, pc);
66
67        if orient2d < 0. {
68            return Winding::Cw;
69        }
70        if orient2d > 0. {
71            return Winding::Ccw;
72        }
73
74        unreachable!(
75            "Points don't form a triangle, but this was verified in the \
76            constructor."
77        )
78    }
79}
80
81impl Triangle<3> {
82    /// Convert the triangle to a Parry triangle
83    pub fn to_parry(self) -> parry3d_f64::shape::Triangle {
84        self.points().map(|vertex| vertex.to_na()).into()
85    }
86
87    /// Cast a ray against the Triangle
88    pub fn cast_local_ray(
89        &self,
90        origin: Point<3>,
91        dir: Vector<3>,
92        max_toi: f64,
93        solid: bool,
94    ) -> Option<Scalar> {
95        let ray = Ray {
96            origin: origin.to_na(),
97            dir: dir.to_na(),
98        };
99
100        self.to_parry()
101            .cast_local_ray(&ray, max_toi, solid)
102            .map(Into::into)
103    }
104
105    /// Compute the triangle's normal
106    pub fn normal(&self) -> Vector<3> {
107        self.to_parry()
108            .normal()
109            .expect("triangle is valid (validated on construction)")
110            .into_inner()
111            .into()
112    }
113}
114
115impl<P, const D: usize> From<[P; 3]> for Triangle<D>
116where
117    P: Into<Point<D>>,
118{
119    fn from(points: [P; 3]) -> Self {
120        Self::from_points(points).expect("invalid triangle")
121    }
122}
123
124/// Returned by [`Triangle::from_points`], if the points don't form a triangle
125#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash, Ord, PartialOrd)]
126pub struct NotATriangle<const D: usize> {
127    pub points: [Point<D>; 3],
128}
129
130/// Winding direction of a triangle.
131#[derive(Clone, Copy, Debug, Eq, PartialEq, Hash, Ord, PartialOrd)]
132pub enum Winding {
133    /// Counter-clockwise
134    Ccw,
135
136    /// Clockwise
137    Cw,
138}
139
140impl Winding {
141    /// Indicate whether the winding is counter-clockwise
142    pub fn is_ccw(&self) -> bool {
143        matches!(self, Self::Ccw)
144    }
145
146    /// Indicate whether the winding is clockwise
147    pub fn is_cw(&self) -> bool {
148        matches!(self, Self::Cw)
149    }
150}
151
152#[cfg(test)]
153mod tests {
154    use crate::{Point, Vector};
155
156    use super::Triangle;
157
158    #[test]
159    fn valid_triangle_2d() {
160        let a = Point::from([0.0, 0.0]);
161        let b = Point::from([1.0, 1.0]);
162        let c = Point::from([1.0, 2.0]);
163        let _triangle = Triangle::from([a, b, c]);
164    }
165
166    #[test]
167    fn valid_triangle_3d() {
168        let a = Point::from([0.0, 0.0, 0.0]);
169        let b = Point::from([1.0, 1.0, 0.0]);
170        let c = Point::from([1.0, 2.0, 0.0]);
171        let _triangle = Triangle::from([a, b, c]);
172    }
173
174    #[test]
175    #[should_panic]
176    fn invalid_triangle_2d() {
177        let a = Point::from([0.0, 0.0]);
178        let b = Point::from([1.0, 1.0]);
179        let c = Point::from([2.0, 2.0]);
180        let _triangle = Triangle::from([a, b, c]);
181    }
182
183    #[test]
184    #[should_panic]
185    fn invalid_triangle_3d() {
186        let a = Point::from([0.0, 0.0, 0.0]);
187        let b = Point::from([1.0, 1.0, 1.0]);
188        let c = Point::from([2.0, 2.0, 2.0]);
189        let _triangle = Triangle::from([a, b, c]);
190    }
191
192    #[test]
193    fn normal() {
194        let triangle =
195            Triangle::from([[0.0, 0.0, 0.0], [2.0, 1.0, 0.0], [2.0, 0.0, 0.0]]);
196        assert_eq!(triangle.normal(), Vector::from([0.0, 0.0, -1.0]));
197    }
198}
fj_math/triangle.rs

fj_math/
triangle.rs
