fenris_geometry/primitives/
triangle.rs

1use crate::{
2    AxisAlignedBoundingBox, BoundedGeometry, ConvexPolygon3d, Distance, LineSegment, LineSegment2d, Orientation,
3    OrientationTestResult, SignedDistance, SignedDistanceResult,
4};
5use fenris_traits::Real;
6use nalgebra::allocator::Allocator;
7use nalgebra::Matrix3;
8use nalgebra::{DefaultAllocator, DimName, OPoint, OVector, Point2, Point3, Scalar, Vector3, U2, U3};
9use numeric_literals::replace_float_literals;
10use serde::{Deserialize, Serialize};
11use std::fmt::Debug;
12
13#[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)]
14#[serde(bound(
15    serialize = "OPoint<T, D>: Serialize",
16    deserialize = "OPoint<T, D>: Deserialize<'de>"
17))]
18pub struct Triangle<T, D>(pub [OPoint<T, D>; 3])
19where
20    T: Scalar,
21    D: DimName,
22    DefaultAllocator: Allocator<T, D>;
23
24/// A triangle in two dimensions, consisting of three vertices.
25///
26/// For most purposes, the triangle is assumed to be specified with a counter-clockwise
27/// winding order, but it also provides facilities for determining the winding order
28/// of an arbitrarily constructed triangle.
29pub type Triangle2d<T> = Triangle<T, U2>;
30pub type Triangle3d<T> = Triangle<T, U3>;
31
32impl<T, D> Copy for Triangle<T, D>
33where
34    T: Scalar,
35    D: DimName,
36    DefaultAllocator: Allocator<T, D>,
37    OPoint<T, D>: Copy,
38{
39}
40
41impl<T, D> Triangle<T, D>
42where
43    T: Scalar,
44    D: DimName,
45    DefaultAllocator: Allocator<T, D>,
46{
47    pub fn swap_vertices(&mut self, i: usize, j: usize) {
48        self.0.swap(i, j);
49    }
50
51    pub fn edge(&self, index: usize) -> LineSegment<T, D> {
52        assert!(index < 3);
53        let a = self.0[(index + 0) % 3].clone();
54        let b = self.0[(index + 1) % 3].clone();
55        LineSegment::from_end_points(a, b)
56    }
57}
58
59impl<T, D> Triangle<T, D>
60where
61    T: Real,
62    D: DimName,
63    DefaultAllocator: Allocator<T, D>,
64{
65    pub fn centroid(&self) -> OPoint<T, D> {
66        let mut centroid = OVector::zeros();
67        for p in &self.0 {
68            centroid += &p.coords * T::from_f64(1.0 / 3.0).unwrap();
69        }
70        OPoint::from(centroid)
71    }
72
73    /// Returns an array of vectors corresponding to the three sides of the triangle.
74    pub fn sides(&self) -> [OVector<T, D>; 3] {
75        let a = &self.0[0];
76        let b = &self.0[1];
77        let c = &self.0[2];
78        [b - a, c - b, a - c]
79    }
80}
81
82impl<T> Triangle2d<T>
83where
84    T: Real,
85{
86    pub fn orientation(&self) -> Orientation {
87        if self.signed_area() >= T::zero() {
88            Orientation::Counterclockwise
89        } else {
90            Orientation::Clockwise
91        }
92    }
93
94    #[replace_float_literals(T::from_f64(literal).expect("Literal must fit in T."))]
95    pub fn signed_area(&self) -> T {
96        let a = &self.0[0];
97        let b = &self.0[1];
98        let c = &self.0[2];
99        let ab = b - a;
100        let ac = c - a;
101        0.5 * ab.perp(&ac)
102    }
103
104    pub fn area(&self) -> T {
105        self.signed_area().abs()
106    }
107}
108
109impl<T> Triangle3d<T>
110where
111    T: Real,
112{
113    /// Returns a vector normal to the triangle. The vector is *not* normalized.
114    pub fn normal_dir(&self) -> Vector3<T> {
115        let a = &self.0[0];
116        let b = &self.0[1];
117        let c = &self.0[2];
118        let ab = b - a;
119        let ac = c - a;
120        let n = ab.cross(&ac);
121        n
122    }
123
124    pub fn normal(&self) -> Vector3<T> {
125        self.normal_dir().normalize()
126    }
127
128    /// TODO: Remove this. It makes no sense for 3D.
129    pub fn orientation(&self) -> Orientation {
130        if self.signed_area() >= T::zero() {
131            Orientation::Clockwise
132        } else {
133            Orientation::Counterclockwise
134        }
135    }
136
137    /// TODO: Remove this. It makes no sense for 3D (moreover, the current implementation
138    /// gives the non-negative ara in any case).
139    #[replace_float_literals(T::from_f64(literal).expect("Literal must fit in T."))]
140    pub fn signed_area(&self) -> T {
141        let a = &self.0[0];
142        let b = &self.0[1];
143        let c = &self.0[2];
144        let ab = b - a;
145        let ac = c - a;
146        0.5 * ab.cross(&ac).norm()
147    }
148
149    pub fn area(&self) -> T {
150        self.signed_area().abs()
151    }
152
153    /// Determines the orientation of a point with respect to the plane containing the triangle.
154    ///
155    /// This is the common "orientation test" used in computational geometry. The test returns
156    /// a value whose sign is the same as `dot(n, x - x0)`, where `x0` is the projection of
157    /// the point onto the triangle plane.
158    ///
159    /// Note that at the moment, this implementation is **NOT** robust (in the sense of exact/robust
160    /// geometric predicates).
161    pub fn point_orientation(&self, point: &Point3<T>) -> OrientationTestResult {
162        // Note: This is by no means robust in the sense of floating point accuracy.
163        let point_in_plane = &self.0[0];
164
165        let x = point;
166        let x0 = point_in_plane;
167        let n = self.normal_dir().normalize();
168        let projected_dist = n.dot(&(x - x0));
169
170        if projected_dist > T::zero() {
171            OrientationTestResult::Positive
172        } else if projected_dist < T::zero() {
173            OrientationTestResult::Negative
174        } else {
175            OrientationTestResult::Zero
176        }
177    }
178}
179
180impl<T, D> BoundedGeometry<T> for Triangle<T, D>
181where
182    T: Real,
183    D: DimName,
184    DefaultAllocator: Allocator<T, D>,
185{
186    type Dimension = D;
187
188    fn bounding_box(&self) -> AxisAlignedBoundingBox<T, D> {
189        AxisAlignedBoundingBox::from_points(&self.0).unwrap()
190    }
191}
192
193impl<T> Distance<T, Point2<T>> for Triangle2d<T>
194where
195    T: Real,
196{
197    fn distance(&self, point: &Point2<T>) -> T {
198        let sdf = self.query_signed_distance(point).unwrap();
199        T::max(T::zero(), sdf.signed_distance)
200    }
201}
202
203impl<T> SignedDistance<T, U2> for Triangle2d<T>
204where
205    T: Real,
206{
207    fn query_signed_distance(&self, point: &Point2<T>) -> Option<SignedDistanceResult<T, U2>> {
208        // TODO: This is not the most efficient way to compute this
209        let mut inside = true;
210
211        let mut closest_segment = 0;
212        // TODO: Fix unwrap
213        let mut closest_dist2 = T::max_value().unwrap();
214        let mut closest_point = Point2::origin();
215
216        // We assume counterclockwise orientation.
217        for i in 0..3 {
218            let a = &self.0[i];
219            let b = &self.0[(i + 1) % 3];
220            let segment = LineSegment2d::from_end_points(*a, *b);
221            // Normal point outwards, i.e. towards the "right"
222            let normal_dir = segment.normal_dir();
223            let projected_point = segment.closest_point(point);
224            let d = &point.coords - &projected_point.coords;
225
226            if d.dot(&normal_dir) > T::zero() {
227                inside = false;
228            }
229
230            let dist2 = d.magnitude_squared();
231            if dist2 < closest_dist2 {
232                closest_segment = i;
233                closest_dist2 = dist2;
234                closest_point = projected_point;
235            }
236        }
237
238        let sign = if inside { T::from_f64(-1.0).unwrap() } else { T::one() };
239
240        Some(SignedDistanceResult {
241            feature_id: closest_segment,
242            point: closest_point,
243            signed_distance: sign * closest_dist2.sqrt(),
244        })
245    }
246}
247
248impl<T> Distance<T, Point3<T>> for Triangle3d<T>
249where
250    T: Real,
251{
252    #[replace_float_literals(T::from_f64(literal).unwrap())]
253    fn distance(&self, point: &OPoint<T, U3>) -> T {
254        self.closest_point(point).distance
255    }
256}
257
258impl<'a, T: Real> ConvexPolygon3d<'a, T> for Triangle3d<T> {
259    fn num_vertices(&self) -> usize {
260        3
261    }
262
263    fn get_vertex(&self, index: usize) -> Option<OPoint<T, U3>> {
264        self.0.get(index).copied()
265    }
266}
267
268impl<T: Real> Triangle3d<T> {
269    /// Compute the solid angle to the given point.
270    #[replace_float_literals(T::from_f64(literal).unwrap())]
271    pub fn compute_solid_angle(&self, p: &Point3<T>) -> T {
272        // Based on equation (6) in Jacobson et al.,
273        // "Robust Inside-Outside Segmentation using Generalized Winding Numbers"
274        let [a, b, c] = self.0.clone().map(|v_i| v_i - p);
275        let abc_matrix = Matrix3::from_columns(&[a.clone(), b.clone(), c.clone()]);
276
277        let anorm = a.norm();
278        let bnorm = b.norm();
279        let cnorm = c.norm();
280
281        let denominator = anorm * bnorm * cnorm + a.dot(&b) * cnorm + b.dot(&c) * anorm + c.dot(&a) * bnorm;
282        let tan_omega_half = abc_matrix.determinant() / denominator;
283        2.0 * tan_omega_half.atan()
284    }
285}
286
287#[replace_float_literals(T::from_f64(literal).unwrap())]
288pub fn compute_winding_number_for_triangles_3d<T, I>(triangles: I, point: &Point3<T>) -> T
289where
290    T: Real,
291    I: IntoIterator<Item = Triangle3d<T>>,
292{
293    let angle_sum = triangles
294        .into_iter()
295        .map(|triangle| triangle.compute_solid_angle(point))
296        .reduce(|acc, angle| acc + angle)
297        .unwrap_or(T::zero());
298    angle_sum / (4.0 * T::pi())
299}
fenris_geometry/primitives/triangle.rs

fenris_geometry/primitives/
triangle.rs
