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, ¢er);
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(¬_contained_coplanar_p));
849 assert!(!tri.contains_point(¬_coplanar_p));
850 assert!(!tri.contains_point(¬_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}