math_utils/geometry/primitive/simplex/
mod.rs

1pub use self::simplex2::Simplex2;
2pub use self::simplex3::Simplex3;
3
4#[macro_use]
5mod macro_def;
6
7/// 2D simplex types
8pub mod simplex2 {
9  #[cfg(feature = "derive_serdes")]
10  use serde::{Deserialize, Serialize};
11
12  use crate::*;
13  use crate::geometry::*;
14
15  /// Parameterized point on a 2D segment
16  pub type SegmentPoint <S>  = (Normalized <S>, Point2 <S>);
17  /// Parameterized point on a 2D triangle
18  pub type TrianglePoint <S> = ([Normalized <S>; 2], Point2 <S>);
19
20  /// A $n<3$-simplex in 2-dimensional space.
21  ///
22  /// Individual simplex variants should fail to construct in debug builds when points
23  /// are degenerate.
24  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
25  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
26  pub enum Simplex2 <S> {
27    Segment  (Segment  <S>),
28    Triangle (Triangle <S>)
29  }
30
31  /// A 1-simplex or line segment in 2D space.
32  ///
33  /// Creation methods should fail with a debug assertion if the points are identical.
34  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
35  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
36  pub struct Segment <S> {
37    a : Point2 <S>,
38    b : Point2 <S>
39  }
40
41  /// A 2-simplex or triangle in 2D space
42  ///
43  /// Creation methods should fail with a debug assertion if the points are colinear.
44  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
45  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
46  pub struct Triangle <S> {
47    a : Point2 <S>,
48    b : Point2 <S>,
49    c : Point2 <S>
50  }
51
52  impl_accessors!(Segment,  Point2, 2, [(point_a, a), (point_b, b)]);
53  impl_accessors!(Triangle, Point2, 3, [(point_a, a), (point_b, b), (point_c, c)]);
54
55  impl <S : OrderedRing> Segment <S> {
56    /// Returns `None` if the points are identical:
57    ///
58    /// ```
59    /// # use math_utils::geometry::simplex2::Segment;
60    /// assert!(Segment::new ([0.0, 0.0].into(), [0.0, 0.0].into()).is_none());
61    /// ```
62    pub fn new (a : Point2 <S>, b : Point2 <S>) -> Option <Self> {
63      if a == b {
64        None
65      } else {
66        Some (Segment { a, b })
67      }
68    }
69    /// Panics if the points are identical:
70    ///
71    /// ```should_panic
72    /// # use math_utils::geometry::simplex2::Segment;
73    /// let s = Segment::noisy ([0.0, 0.0].into(), [0.0, 0.0].into());
74    /// ```
75    pub fn noisy (a : Point2 <S>, b : Point2 <S>) -> Self {
76      assert_ne!(a, b);
77      Segment { a, b }
78    }
79    /// Debug panic if the points are identical:
80    ///
81    /// ```should_panic
82    /// # use math_utils::geometry::simplex2::Segment;
83    /// let s = Segment::unchecked ([0.0, 0.0].into(), [0.0, 0.0].into());
84    /// ```
85    pub fn unchecked (a : Point2 <S>, b : Point2 <S>) -> Self {
86      debug_assert_ne!(a, b);
87      Segment { a, b }
88    }
89    #[inline]
90    pub fn from_array ([a, b] : [Point2 <S>; 2]) -> Option <Self> {
91      Self::new (a, b)
92    }
93    #[inline]
94    pub fn numcast <T> (self) -> Option <Segment <T>> where
95      S : num::NumCast,
96      T : num::NumCast
97    {
98      Some (Segment {
99        a: self.a.numcast()?,
100        b: self.b.numcast()?
101      })
102    }
103    /// Return a parameterized point along the segment
104    pub fn point (self, param : Normalized <S>) -> Point2 <S> {
105      self.a + *self.vector() * param
106    }
107    #[inline]
108    pub fn length (self) -> NonNegative <S> where S : Field + Sqrt {
109      self.vector().norm()
110    }
111    #[inline]
112    pub fn length_squared (self) -> NonNegative <S> {
113      self.vector().self_dot()
114    }
115    /// Returns `point_b() - point_a()`
116    #[inline]
117    pub fn vector (self) -> NonZero2 <S> {
118      NonZero2::unchecked (self.b - self.a)
119    }
120    #[inline]
121    pub fn perpendicular (self) -> NonZero2 <S> {
122      self.vector().map_unchecked (Vector2::yx)
123    }
124    #[inline]
125    pub fn midpoint (self) -> Point2 <S> where S : Field {
126      ((self.a.0 + self.b.0) * S::half()).into()
127    }
128    #[inline]
129    pub fn aabb2 (self) -> Aabb2 <S> {
130      Aabb2::from_points_unchecked (self.a, self.b)
131    }
132    #[inline]
133    pub fn line (self) -> Line2 <S> where S : Real {
134      Line2::new (self.a, self.vector().normalize())
135    }
136    pub fn affine_line (self) -> frame::Line2 <S> {
137      frame::Line2 {
138        origin: self.a,
139        basis:  self.vector()
140      }
141    }
142    #[inline]
143    pub fn nearest_point (self, point : Point2 <S>) -> SegmentPoint <S> where
144      S : Field
145    {
146      distance::nearest_segment2_point2 (self, point)
147    }
148    #[inline]
149    pub fn intersect_aabb (self, aabb : Aabb2 <S>)
150      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
151    where S : Real {
152      intersect::intersect_segment2_aabb2 (self, aabb)
153    }
154    #[inline]
155    pub fn intersect_sphere (self, sphere : Sphere2 <S>)
156      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
157    where S : Real {
158      intersect::intersect_segment2_sphere2 (self, sphere)
159    }
160  }
161  impl <S : Ring> Default for Segment <S> {
162    /// A default simplex is arbitrarily chosen to be the simplex from -1.0 to 1.0 lying
163    /// on the X axis:
164    ///
165    /// ```
166    /// # use math_utils::geometry::simplex2::Segment;
167    /// assert_eq!(
168    ///   Segment::default(),
169    ///   Segment::noisy ([-1.0, 0.0].into(), [1.0, 0.0].into())
170    /// );
171    /// ```
172    fn default() -> Self {
173      Segment {
174        a: [-S::one(), S::zero()].into(),
175        b: [ S::one(), S::zero()].into()
176      }
177    }
178  }
179  impl <S> std::ops::Index <usize> for Segment <S> where S : Copy {
180    type Output = Point2 <S>;
181    fn index (&self, index : usize) -> &Point2 <S> {
182      [&self.a, &self.b][index]
183    }
184  }
185  impl <S> TryFrom <[Point2 <S>; 2]> for Segment <S> where S : OrderedRing {
186    type Error = [Point2 <S>; 2];
187    fn try_from ([a, b] : [Point2 <S>; 2]) -> Result <Self, Self::Error> {
188      Self::new (a, b).ok_or([a, b])
189    }
190  }
191  impl <S> Primitive <S, Point2 <S>> for Segment <S> where
192    S : OrderedField + approx::AbsDiffEq
193  {
194    fn translate (&mut self, displacement : Vector2 <S>) {
195      self.a += displacement;
196      self.b += displacement;
197    }
198    fn scale (&mut self, scale : Positive <S>) {
199      let midpoint = self.midpoint();
200      self.translate (-midpoint.0);
201      self.a = Point2 (self.a.0 * scale);
202      self.b = Point2 (self.b.0 * scale);
203      self.translate (midpoint.0);
204    }
205    fn support (&self, direction : NonZero2 <S>) -> (Point2 <S>, S) {
206      let [dot_a, dot_b] = self.points().map (|p| direction.dot (p.0));
207      if dot_a >= dot_b {
208        (self.a, dot_a)
209      } else {
210        (self.b, dot_b)
211      }
212    }
213  }
214
215  impl <S : OrderedField> Triangle <S> {
216    /// Returns `None` if the points are colinear:
217    ///
218    /// ```
219    /// # use math_utils::geometry::simplex2::Triangle;
220    /// assert!(Triangle::new (
221    ///   [-1.0, -1.0].into(),
222    ///   [ 0.0,  0.0].into(),
223    ///   [ 1.0,  1.0].into()
224    /// ).is_none());
225    /// ```
226    pub fn new (a : Point2 <S>, b : Point2 <S>, c : Point2 <S>) -> Option <Self> where
227      S : approx::AbsDiffEq <Epsilon = S>
228    {
229      if colinear_2 (a, b, c) {
230        None
231      } else {
232        Some (Triangle { a, b, c })
233      }
234    }
235    /// Panics if the points are colinear:
236    ///
237    /// ```should_panic
238    /// # use math_utils::geometry::simplex2::Triangle;
239    /// let s = Triangle::noisy (
240    ///   [-1.0, -1.0].into(),
241    ///   [ 0.0,  0.0].into(),
242    ///   [ 1.0,  1.0].into());
243    /// ```
244    pub fn noisy (a : Point2 <S>, b : Point2 <S>, c : Point2 <S>) -> Self where
245      S : approx::AbsDiffEq <Epsilon = S>
246    {
247      debug_assert_ne!(a, b);
248      debug_assert_ne!(a, c);
249      debug_assert_ne!(b, c);
250      assert!(!colinear_2 (a, b, c));
251      Triangle { a, b, c }
252    }
253    /// Debug panic if the points are colinear:
254    ///
255    /// ```should_panic
256    /// # use math_utils::geometry::simplex2::Triangle;
257    /// let s = Triangle::unchecked (
258    ///   [-1.0, -1.0].into(),
259    ///   [ 0.0,  0.0].into(),
260    ///   [ 1.0,  1.0].into());
261    /// ```
262    pub fn unchecked (a : Point2 <S>, b : Point2 <S>, c : Point2 <S>) -> Self where
263      S : approx::AbsDiffEq <Epsilon = S>
264    {
265      debug_assert_ne!(a, b);
266      debug_assert_ne!(a, c);
267      debug_assert_ne!(b, c);
268      debug_assert!(!colinear_2 (a, b, c));
269      Triangle { a, b, c }
270    }
271    #[inline]
272    pub fn from_array ([a, b, c] : [Point2 <S>; 3]) -> Option <Self> where
273      S : approx::AbsDiffEq <Epsilon = S>
274    {
275      Self::new (a, b, c)
276    }
277    /// Return a parameterized point on the triangle.
278    ///
279    /// Returns `None` if `s + t > 1.0`.
280    pub fn point (self, s : Normalized <S>, t : Normalized <S>) -> Option <Point2 <S>> {
281      if *s + *t > S::one() {
282        None
283      } else {
284        Some (self.a + (self.b - self.a) * s + (self.c - self.a) * t)
285      }
286    }
287    #[inline]
288    pub const fn edge_ab (self) -> Segment2 <S> {
289      Segment { a: self.a, b: self.b }
290    }
291    #[inline]
292    pub const fn edge_bc (self) -> Segment2 <S> {
293      Segment { a: self.b, b: self.c }
294    }
295    #[inline]
296    pub const fn edge_ca (self) -> Segment2 <S> {
297      Segment { a: self.c, b: self.a }
298    }
299    /// Returns edges `[AB, BC, CA]`
300    #[inline]
301    pub const fn edges (self) -> [Segment2 <S>; 3] {
302      [ self.edge_ab(), self.edge_bc(), self.edge_ca() ]
303    }
304    /// Change triangle winding by swapping points B and C.
305    ///
306    /// Equivalent to `triangle.acb()`.
307    #[inline]
308    pub const fn rewind (self) -> Self {
309      self.acb()
310    }
311    /// Change triangle winding by swapping points B and C
312    #[inline]
313    pub const fn acb (self) -> Self {
314      Triangle { a: self.a, b: self.c, c: self.b }
315    }
316    /// Returns perpendicular vector to edge `AB`.
317    ///
318    /// ```
319    /// # use math_utils::geometry::simplex2::Triangle;
320    /// let s = Triangle::unchecked (
321    ///   [0.0, 0.0].into(),
322    ///   [0.0, 2.0].into(),
323    ///   [2.0, 1.0].into());
324    /// assert_eq!(*s.perpendicular_ab(), [-2.0, 0.0].into());
325    /// let s = Triangle::unchecked (
326    ///   [ 0.0, 0.0].into(),
327    ///   [ 0.0, 2.0].into(),
328    ///   [-2.0, 1.0].into());
329    /// assert_eq!(*s.perpendicular_ab(), [2.0, 0.0].into());
330    /// ```
331    pub fn perpendicular_ab (self) -> NonZero2 <S> {
332      let ac = self.c - self.a;
333      let mut perp = self.edge_ab().perpendicular();
334      if perp.dot (ac) > S::zero() {
335        perp = -perp
336      }
337      perp
338    }
339    /// Returns perpendicular vector to edge `BC`.
340    ///
341    /// ```
342    /// # use math_utils::geometry::simplex2::Triangle;
343    /// let s = Triangle::unchecked (
344    ///   [2.0, 1.0].into(),
345    ///   [0.0, 0.0].into(),
346    ///   [0.0, 2.0].into());
347    /// assert_eq!(*s.perpendicular_bc(), [-2.0, 0.0].into());
348    /// let s = Triangle::unchecked (
349    ///   [-2.0, 1.0].into(),
350    ///   [ 0.0, 0.0].into(),
351    ///   [ 0.0, 2.0].into());
352    /// assert_eq!(*s.perpendicular_bc(), [2.0, 0.0].into());
353    /// ```
354    pub fn perpendicular_bc (self) -> NonZero2 <S> {
355      let bc = self.c - self.b;
356      let ba = self.a - self.b;
357      let mut perp = bc.yx();
358      if perp.dot (ba) > S::zero() {
359        perp = -perp
360      }
361      NonZero2::unchecked (perp)
362    }
363    /// Returns perpendicular vector to edge `CA`.
364    ///
365    /// ```
366    /// # use math_utils::geometry::simplex2::Triangle;
367    /// let s = Triangle::unchecked (
368    ///   [0.0, 2.0].into(),
369    ///   [2.0, 1.0].into(),
370    ///   [0.0, 0.0].into());
371    /// assert_eq!(*s.perpendicular_ca(), [-2.0, 0.0].into());
372    /// let s = Triangle::unchecked (
373    ///   [ 0.0, 2.0].into(),
374    ///   [-2.0, 1.0].into(),
375    ///   [ 0.0, 0.0].into());
376    /// assert_eq!(*s.perpendicular_ca(), [2.0, 0.0].into());
377    /// ```
378    pub fn perpendicular_ca (self) -> NonZero2 <S> {
379      let ca = self.a - self.c;
380      let cb = self.b - self.c;
381      let mut perp = ca.yx();
382      if perp.dot (cb) > S::zero() {
383        perp = -perp
384      }
385      NonZero2::unchecked (perp)
386    }
387    #[inline]
388    pub fn area (self) -> Positive <S> where S : num::real::Real {
389      let ab = self.b - self.a;
390      let ac = self.c - self.a;
391      Positive::unchecked (ab.exterior_product (ac).abs())
392    }
393    /// Barycentric coordinate $1/3 : 1/3 : 1/3$ corresponds to the center of mass
394    /// (barycenter) of a uniform triangular lamina
395    #[inline]
396    pub fn centroid (self) -> Point2 <S> {
397      self.barycentric (Vector3::broadcast (S::one() / S::three()))
398    }
399    /// Trilinear coordinate $1 : 1 : 1$ corresponds to center of inscribed circle
400    // TODO: is it more efficient to calculate barycentric coordinates based on angles ?
401    #[inline]
402    pub fn incenter (self) -> Point2 <S> where S : Real + num::real::Real {
403      self.trilinear (Vector3::one())
404    }
405    pub fn barycentric (self, coords : Vector3 <S>) -> Point2 <S> {
406      (self.a.0 * coords.x + self.b.0 * coords.y + self.c.0 * coords.z).into()
407    }
408    pub fn trilinear (self, coords : Vector3 <S>) -> Point2 <S> where
409      S : Real + num::real::Real
410    {
411      let (ab, bc, ca) = (self.b - self.a, self.c - self.b, self.a - self.c);
412      let len_ab = ab.magnitude();
413      let len_bc = bc.magnitude();
414      let len_ca = ca.magnitude();
415      let ax = len_bc * coords.x;
416      let by = len_ca * coords.y;
417      let cz = len_ab * coords.z;
418      let denom = S::one() / (ax + by + cz);
419      (self.a.0 * ax * denom + self.b.0 * by * denom + self.c.0 * cz * denom).into()
420    }
421    #[inline]
422    pub fn affine_plane (self) -> frame::Plane2 <S> {
423      frame::Plane2 {
424        origin: self.a,
425        basis:  frame::PlanarBasis2::new (Matrix2::from_col_arrays ([
426          (self.b - self.a).into_array(),
427          (self.c - self.a).into_array()
428        ])).unwrap()
429      }
430    }
431    #[inline]
432    pub fn nearest_point (self, point : Point2 <S>) -> TrianglePoint <S> {
433      distance::nearest_triangle2_point2 (self, point)
434    }
435  }
436  impl <S : Field + Sqrt> Default for Triangle <S> {
437    /// A default triangle is arbitrarily chosen to be the equilateral triangle with
438    /// point at 1.0 on the Y axis:
439    ///
440    /// ```
441    /// # use math_utils::approx;
442    /// # use math_utils::geometry::simplex2::Triangle;
443    /// use std::f64::consts::FRAC_1_SQRT_2;
444    /// let s = Triangle::default();
445    /// let t = Triangle::noisy (
446    ///   [           0.0,            1.0].into(),
447    ///   [-FRAC_1_SQRT_2, -FRAC_1_SQRT_2].into(),
448    ///   [ FRAC_1_SQRT_2, -FRAC_1_SQRT_2].into()
449    /// );
450    /// approx::assert_relative_eq!(s.point_a(), t.point_a());
451    /// approx::assert_relative_eq!(s.point_b(), t.point_b());
452    /// approx::assert_relative_eq!(s.point_c(), t.point_c());
453    /// ```
454    fn default() -> Self {
455      let frac_1_sqrt_2 = S::one() / (S::one() + S::one()).sqrt();
456      Triangle {
457        a: [     S::zero(),       S::one()].into(),
458        b: [-frac_1_sqrt_2, -frac_1_sqrt_2].into(),
459        c: [ frac_1_sqrt_2, -frac_1_sqrt_2].into()
460      }
461    }
462  }
463  impl <S> std::ops::Index <usize> for Triangle <S> where S : Copy {
464    type Output = Point2 <S>;
465    fn index (&self, index : usize) -> &Point2 <S> {
466      [&self.a, &self.b, &self.c][index]
467    }
468  }
469  impl <S> TryFrom <[Point2 <S>; 3]> for Triangle <S>
470    where S : OrderedField + approx::AbsDiffEq <Epsilon = S>
471  {
472    type Error = [Point2 <S>; 3];
473    fn try_from ([a, b, c] : [Point2 <S>; 3]) -> Result <Self, Self::Error> {
474      Self::new (a, b, c).ok_or([a, b, c])
475    }
476  }
477  impl <S> Primitive <S, Point2 <S>> for Triangle <S> where
478    S : OrderedField + approx::AbsDiffEq
479  {
480    fn translate (&mut self, displacement : Vector2 <S>) {
481      self.a += displacement;
482      self.b += displacement;
483      self.c += displacement;
484    }
485    fn scale (&mut self, scale : Positive <S>) {
486      let centroid = self.centroid();
487      self.translate (-centroid.0);
488      self.a = Point2 (self.a.0 * scale);
489      self.b = Point2 (self.b.0 * scale);
490      self.c = Point2 (self.c.0 * scale);
491      self.translate (centroid.0);
492    }
493    fn support (&self, direction : NonZero2 <S>) -> (Point2 <S>, S) {
494      let [dot_a, dot_b, dot_c] = self.points().map (|p| direction.dot (p.0));
495      if dot_a >= dot_b && dot_a >= dot_c {
496        (self.a, dot_a)
497      } else if dot_b >= dot_c {
498        (self.b, dot_b)
499      } else {
500        (self.c, dot_c)
501      }
502    }
503  }
504}
505
506/// 3D simplex types
507pub mod simplex3 {
508  #[cfg(feature = "derive_serdes")]
509  use serde::{Deserialize, Serialize};
510
511  use crate::*;
512  use crate::geometry::*;
513
514  /// Parameterized point on a 3D segment
515  pub type SegmentPoint <S>     = (Normalized <S>, Point3 <S>);
516  /// Parameterized point on a 3D triangle
517  pub type TrianglePoint <S>    = ([Normalized <S>; 2], Point3 <S>);
518  /// Parameterized point on a 3D tetrahedron
519  pub type TetrahedronPoint <S> = ([Normalized <S>; 3], Point3 <S>);
520
521  /// A $n<4$-simplex in 3-dimensional space.
522  ///
523  /// Individual simplex variants should fail to construct in debug builds when points
524  /// are degenerate.
525  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
526  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
527  pub enum Simplex3 <S> {
528    Segment     (Segment     <S>),
529    Triangle    (Triangle    <S>),
530    Tetrahedron (Tetrahedron <S>)
531  }
532
533  /// A 1-simplex or line segment in 3D space.
534  ///
535  /// Creation methods should fail with a debug assertion if the points are identical.
536  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
537  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
538  pub struct Segment <S> {
539    a : Point3 <S>,
540    b : Point3 <S>
541  }
542
543  /// A 2-simplex or triangle in 3D space
544  ///
545  /// Creation methods should fail with a debug assertion if the points are colinear.
546  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
547  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
548  pub struct Triangle <S> {
549    a : Point3 <S>,
550    b : Point3 <S>,
551    c : Point3 <S>
552  }
553
554  /// A 3-simplex or tetrahedron in 3D space
555  ///
556  /// Creation methods will fail with a debug assertion if the points are coplanar.
557  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
558  #[derive(Clone, Copy, Debug, Eq, PartialEq)]
559  pub struct Tetrahedron <S> {
560    a : Point3 <S>,
561    b : Point3 <S>,
562    c : Point3 <S>,
563    d : Point3 <S>
564  }
565
566  impl_accessors!(Segment,     Point3, 2, [(point_a, a), (point_b, b)]);
567  impl_accessors!(Triangle,    Point3, 3, [(point_a, a), (point_b, b), (point_c, c)]);
568  impl_accessors!(Tetrahedron, Point3, 4,
569    [(point_a, a), (point_b, b), (point_c, c), (point_d, d)]);
570
571  impl <S : OrderedRing> Segment <S> {
572    /// Returns `None` if the points are identical:
573    ///
574    /// ```
575    /// # use math_utils::geometry::simplex3::Segment;
576    /// assert!(Segment::new ([0.0, 0.0, 1.0].into(), [0.0, 0.0, 1.0].into()).is_none());
577    /// ```
578    pub fn new (a : Point3 <S>, b : Point3 <S>) -> Option <Self> {
579      if a == b {
580        None
581      } else {
582        Some (Segment { a, b })
583      }
584    }
585    /// Panics if the points are identical:
586    ///
587    /// ```should_panic
588    /// # use math_utils::geometry::simplex3::Segment;
589    /// let s = Segment::noisy ([0.0, 0.0, 1.0].into(), [0.0, 0.0, 1.0].into());
590    /// ```
591    pub fn noisy (a : Point3 <S>, b : Point3 <S>) -> Self {
592      assert_ne!(a, b);
593      Segment { a, b }
594    }
595    /// Debug panic if the points are identical:
596    ///
597    /// ```should_panic
598    /// # use math_utils::geometry::simplex3::Segment;
599    /// let s = Segment::unchecked ([0.0, 0.0, 1.0].into(), [0.0, 0.0, 1.0].into());
600    /// ```
601    pub fn unchecked (a : Point3 <S>, b : Point3 <S>) -> Self {
602      debug_assert_ne!(a, b);
603      Segment { a, b }
604    }
605    #[inline]
606    pub fn from_array ([a, b] : [Point3 <S>; 2]) -> Option <Self> {
607      Self::new (a, b)
608    }
609    #[inline]
610    pub fn numcast <T> (self) -> Option <Segment <T>> where
611      S : num::NumCast,
612      T : num::NumCast
613    {
614      Some (Segment {
615        a: self.a.numcast()?,
616        b: self.b.numcast()?
617      })
618    }
619    /// Return a parameterized point along the segment
620    pub fn point (self, param : Normalized <S>) -> Point3 <S> {
621      self.a + *self.vector() * param
622    }
623    #[inline]
624    pub fn length (self) -> NonNegative <S> where S : Field + Sqrt {
625      self.vector().norm()
626    }
627    #[inline]
628    pub fn length_squared (self) -> NonNegative <S> {
629      self.vector().self_dot()
630    }
631    /// Returns `point_b() - point_a()`
632    #[inline]
633    pub fn vector (self) -> NonZero3 <S> {
634      NonZero3::unchecked (self.b - self.a)
635    }
636    /// Equivalent to `self.vector().orthogonal()`
637    #[inline]
638    pub fn perpendicular (self) -> NonZero3 <S> where
639      S : Field + approx::AbsDiffEq <Epsilon = S>
640    {
641      NonZero3::unchecked (self.vector().orthogonal())
642    }
643    #[inline]
644    pub fn midpoint (self) -> Point3 <S> where S : Field {
645      ((self.a.0 + self.b.0) * S::half()).into()
646    }
647    #[inline]
648    pub fn aabb3 (self) -> Aabb3 <S> {
649      Aabb3::from_points_unchecked (self.a, self.b)
650    }
651    #[inline]
652    pub fn line (self) -> Line3 <S> where S : Field + Sqrt {
653      Line3::new (self.a, Unit3::normalize (*self.vector()))
654    }
655    pub fn affine_line (self) -> frame::Line3 <S> {
656      frame::Line3 {
657        origin: self.a,
658        basis:  self.vector()
659      }
660    }
661    #[inline]
662    pub fn nearest_point (self, point : Point3 <S>) -> SegmentPoint <S> where
663      S : Field
664    {
665      distance::nearest_segment3_point3 (self, point)
666    }
667    #[inline]
668    pub fn intersect_aabb (self, aabb : Aabb3 <S>)
669      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
670    where S : Real + num::Float + approx::RelativeEq <Epsilon=S> {
671      intersect::intersect_segment3_aabb3 (self, aabb)
672    }
673    #[inline]
674    pub fn intersect_triangle (self, triangle : Triangle3 <S>)
675      -> Option <SegmentPoint <S>>
676    where S : OrderedField + approx::AbsDiffEq <Epsilon=S> {
677      intersect::intersect_segment3_triangle3 (self, triangle)
678    }
679    #[inline]
680    pub fn intersect_sphere (self, sphere : Sphere3 <S>)
681      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
682    where S : Field + Sqrt {
683      intersect::intersect_segment3_sphere3 (self, sphere)
684    }
685    #[inline]
686    pub fn intersect_cylinder (self, sphere : Cylinder3 <S>)
687      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
688    where S : Real {
689      intersect::intersect_segment3_cylinder3 (self, sphere)
690    }
691    #[inline]
692    pub fn intersect_capsule (self, capsule : Capsule3 <S>)
693      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
694    where S : Real {
695      intersect::intersect_segment3_capsule3 (self, capsule)
696    }
697    #[inline]
698    pub fn intersect_hull (self, hull : &Hull3 <S>, mesh : &mesh::VertexEdgeTriangleMesh)
699      -> Option <(SegmentPoint <S>, SegmentPoint <S>)>
700    where S : OrderedField + Sqrt + approx::RelativeEq <Epsilon = S> {
701      intersect::intersect_segment3_hull3 (self, hull, mesh)
702    }
703  }
704  impl <S : Ring> Default for Segment <S> {
705    /// A default simplex is arbitrarily chosen to be the simplex from -1.0 to 1.0 lying
706    /// on the X axis:
707    ///
708    /// ```
709    /// # use math_utils::geometry::simplex3::Segment;
710    /// assert_eq!(
711    ///   Segment::default(),
712    ///   Segment::noisy ([-1.0, 0.0, 0.0].into(), [1.0, 0.0, 0.0].into())
713    /// );
714    /// ```
715    fn default() -> Self {
716      Segment {
717        a: [-S::one(), S::zero(), S::zero()].into(),
718        b: [ S::one(), S::zero(), S::zero()].into()
719      }
720    }
721  }
722  impl <S> std::ops::Index <usize> for Segment <S> where S : Copy {
723    type Output = Point3 <S>;
724    fn index (&self, index : usize) -> &Point3 <S> {
725      [&self.a, &self.b][index]
726    }
727  }
728  impl <S> TryFrom <[Point3 <S>; 2]> for Segment <S> where S : OrderedRing {
729    type Error = [Point3 <S>; 2];
730    fn try_from ([a, b] : [Point3 <S>; 2]) -> Result <Self, Self::Error> {
731      Self::new (a, b).ok_or([a, b])
732    }
733  }
734  impl <S> Primitive <S, Point3 <S>> for Segment <S> where
735    S : OrderedField + approx::AbsDiffEq
736  {
737    fn translate (&mut self, displacement : Vector3 <S>) {
738      self.a += displacement;
739      self.b += displacement;
740    }
741    fn scale (&mut self, scale : Positive <S>) {
742      let midpoint = self.midpoint();
743      self.translate (-midpoint.0);
744      self.a = Point3 (self.a.0 * scale);
745      self.b = Point3 (self.b.0 * scale);
746      self.translate (midpoint.0);
747    }
748    fn support (&self, direction : NonZero3 <S>) -> (Point3 <S>, S) {
749      let [dot_a, dot_b] = self.points().map (|p| direction.dot (p.0));
750      if dot_a >= dot_b {
751        (self.a, dot_a)
752      } else {
753        (self.b, dot_b)
754      }
755    }
756  }
757
758  impl <S : OrderedField> Triangle <S> {
759    /// Returns `None` if points are colinear:
760    ///
761    /// ```
762    /// # use math_utils::geometry::simplex3::Triangle;
763    /// assert!(Triangle::new (
764    ///   [-1.0, -1.0, -1.0].into(),
765    ///   [ 0.0,  0.0,  0.0].into(),
766    ///   [ 1.0,  1.0,  1.0].into()
767    /// ).is_none());
768    /// ```
769    pub fn new (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>) -> Option <Self> where
770      S : Sqrt + approx::RelativeEq <Epsilon = S>
771    {
772      if colinear_3 (a, b, c) {
773        None
774      } else {
775        Some (Triangle { a, b, c })
776      }
777    }
778    /// Panics if the points are colinear:
779    ///
780    /// ```should_panic
781    /// # use math_utils::geometry::simplex3::Triangle;
782    /// let s = Triangle::noisy (
783    ///   [-1.0, -1.0, -1.0].into(),
784    ///   [ 0.0,  0.0,  0.0].into(),
785    ///   [ 1.0,  1.0,  1.0].into());
786    /// ```
787    pub fn noisy (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>) -> Self where
788      S : Sqrt + approx::RelativeEq <Epsilon = S>
789    {
790      debug_assert_ne!(a, b);
791      debug_assert_ne!(a, c);
792      debug_assert_ne!(b, c);
793      assert!(!colinear_3 (a, b, c));
794      Triangle { a, b, c }
795    }
796    /// Debug panic if the points are colinear:
797    ///
798    /// ```should_panic
799    /// # use math_utils::geometry::simplex3::Triangle;
800    /// let s = Triangle::unchecked (
801    ///   [-1.0, -1.0, -1.0].into(),
802    ///   [ 0.0,  0.0,  0.0].into(),
803    ///   [ 1.0,  1.0,  1.0].into());
804    /// ```
805    pub fn unchecked (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>) -> Self where
806      S : Sqrt + approx::RelativeEq <Epsilon = S>
807    {
808      debug_assert_ne!(a, b);
809      debug_assert_ne!(a, c);
810      debug_assert_ne!(b, c);
811      debug_assert!(!colinear_3 (a, b, c));
812      Triangle { a, b, c }
813    }
814    #[inline]
815    pub fn from_array ([a, b, c] : [Point3 <S>; 3]) -> Option <Self> where
816      S : Sqrt + approx::RelativeEq <Epsilon = S>
817    {
818      Self::new (a, b, c)
819    }
820    /// Return a parameterized point on the triangle.
821    ///
822    /// Returns `None` if `s + t > 1.0`.
823    pub fn point (self, s : Normalized <S>, t : Normalized <S>) -> Option <Point3 <S>> {
824      if *s + *t > S::one() {
825        None
826      } else {
827        Some (self.a + (self.b - self.a) * s + (self.c - self.a) * t)
828      }
829    }
830    #[inline]
831    pub const fn edge_ab (self) -> Segment3 <S> {
832      Segment { a: self.a, b: self.b }
833    }
834    #[inline]
835    pub const fn edge_bc (self) -> Segment3 <S> {
836      Segment { a: self.b, b: self.c }
837    }
838    #[inline]
839    pub const fn edge_ca (self) -> Segment3 <S> {
840      Segment { a: self.c, b: self.a }
841    }
842    #[inline]
843    pub const fn edges (self) -> [Segment3 <S>; 3] {
844      [ Segment { a: self.a, b: self.b },
845        Segment { a: self.b, b: self.c },
846        Segment { a: self.c, b: self.a }
847      ]
848    }
849    /// Change triangle winding by swapping points B and C.
850    ///
851    /// Equivalent to `triangle.acb()`.
852    #[inline]
853    pub const fn rewind (self) -> Self {
854      self.acb()
855    }
856    /// Change triangle winding by swapping points B and C
857    #[inline]
858    pub const fn acb (self) -> Self {
859      Triangle { a: self.a, b: self.c, c: self.b }
860    }
861    /// Cross product of edges `AB x AC`
862    #[inline]
863    pub fn normal (self) -> NonZero3 <S> {
864      let ab = self.b - self.a;
865      let ac = self.c - self.b;
866      NonZero3::unchecked (ab.cross (ac))
867    }
868    /// Normalized cross product of edges `AB x AC`
869    #[inline]
870    pub fn unit_normal (self) -> Unit3 <S> where S : Sqrt {
871      self.normal().normalize()
872    }
873    /// Returns perpendicular vector to edge `AB`.
874    ///
875    /// ```
876    /// # use math_utils::*;
877    /// # use math_utils::geometry::simplex3::Triangle;
878    /// let s = Triangle::unchecked (
879    ///   [0.0, 0.0, 0.0].into(),
880    ///   [0.0, 2.0, 0.0].into(),
881    ///   [2.0, 1.0, 0.0].into());
882    /// assert_eq!(*s.perpendicular_ab(), vector![-8.0, 0.0, 0.0]);
883    /// let s = Triangle::unchecked (
884    ///   [ 0.0, 0.0, 0.0].into(),
885    ///   [ 0.0, 2.0, 0.0].into(),
886    ///   [-2.0, 1.0, 0.0].into());
887    /// assert_eq!(*s.perpendicular_ab(), vector![8.0, 0.0, 0.0]);
888    /// ```
889    pub fn perpendicular_ab (self) -> NonZero3 <S> {
890      let ab = self.b - self.a;
891      let ac = self.c - self.a;
892      let perp = ab.cross (ab.cross (ac));
893      debug_assert!(perp.dot (ac) < S::zero());
894      NonZero3::unchecked (perp)
895    }
896    /// Returns perpendicular vector to edge `BC`.
897    ///
898    /// ```
899    /// # use math_utils::*;
900    /// # use math_utils::geometry::simplex3::Triangle;
901    /// let s = Triangle::unchecked (
902    ///   [2.0, 1.0, 0.0].into(),
903    ///   [0.0, 0.0, 0.0].into(),
904    ///   [0.0, 2.0, 0.0].into());
905    /// assert_eq!(*s.perpendicular_bc(), vector![-8.0, 0.0, 0.0]);
906    /// let s = Triangle::unchecked (
907    ///   [-2.0, 1.0, 0.0].into(),
908    ///   [ 0.0, 0.0, 0.0].into(),
909    ///   [ 0.0, 2.0, 0.0].into());
910    /// assert_eq!(*s.perpendicular_bc(), vector![8.0, 0.0, 0.0]);
911    /// ```
912    pub fn perpendicular_bc (self) -> NonZero3 <S> {
913      let bc = self.c - self.b;
914      let ba = self.a - self.b;
915      let perp = bc.cross (bc.cross (ba));
916      debug_assert!(perp.dot (ba) < S::zero());
917      NonZero3::unchecked (perp)
918    }
919    /// Returns perpendicular vector to edge `CA`.
920    ///
921    /// ```
922    /// # use math_utils::*;
923    /// # use math_utils::geometry::simplex3::Triangle;
924    /// let s = Triangle::unchecked (
925    ///   [0.0, 2.0, 0.0].into(),
926    ///   [2.0, 1.0, 0.0].into(),
927    ///   [0.0, 0.0, 0.0].into());
928    /// assert_eq!(*s.perpendicular_ca(), vector![-8.0, 0.0, 0.0]);
929    /// let s = Triangle::unchecked (
930    ///   [ 0.0, 2.0, 0.0].into(),
931    ///   [-2.0, 1.0, 0.0].into(),
932    ///   [ 0.0, 0.0, 0.0].into());
933    /// assert_eq!(*s.perpendicular_ca(), vector![8.0, 0.0, 0.0]);
934    /// ```
935    pub fn perpendicular_ca (self) -> NonZero3 <S> {
936      let ca = self.a - self.c;
937      let cb = self.b - self.c;
938      let perp = ca.cross (ca.cross (cb));
939      debug_assert!(perp.dot (cb) < S::zero());
940      NonZero3::unchecked (perp)
941    }
942    #[inline]
943    pub fn area (self) -> Positive <S> where S : Sqrt {
944      Positive::unchecked (*self.normal().norm() * S::half())
945    }
946    /// Barycentric coordinate $1/3 : 1/3 : 1/3$ corresponds to the center of mass
947    /// (barycenter) of a uniform triangular lamina
948    #[inline]
949    pub fn centroid (self) -> Point3 <S> {
950      self.barycentric (Vector3::broadcast (S::one() / S::three()))
951    }
952    /// Trilinear coordinate $1 : 1 : 1$ corresponds to center of inscribed circle
953    // TODO: is it more efficient to calculate barycentric coordinates based on angles ?
954    #[inline]
955    pub fn incenter (self) -> Point3 <S> where S : Real + num::real::Real {
956      self.trilinear (Vector3::one())
957    }
958    pub fn barycentric (self, coords : Vector3 <S>) -> Point3 <S> {
959      (self.a.0 * coords.x + self.b.0 * coords.y + self.c.0 * coords.z).into()
960    }
961    pub fn trilinear (self, coords : Vector3 <S>) -> Point3 <S> where
962      S : Real + num::real::Real
963    {
964      let (ab, bc, ca) = (self.b - self.a, self.c - self.b, self.a - self.c);
965      let len_ab = ab.magnitude();
966      let len_bc = bc.magnitude();
967      let len_ca = ca.magnitude();
968      let ax = len_bc * coords.x;
969      let by = len_ca * coords.y;
970      let cz = len_ab * coords.z;
971      let denom = S::one() / (ax + by + cz);
972      (self.a.0 * ax * denom + self.b.0 * by * denom + self.c.0 * cz * denom).into()
973    }
974    #[inline]
975    pub fn affine_plane (self) -> frame::Plane3 <S> {
976      frame::Plane3 {
977        origin: self.a,
978        basis:  frame::PlanarBasis3::new ([
979          (self.b - self.a),
980          (self.c - self.a)
981        ].map (NonZero3::unchecked)).unwrap()
982      }
983    }
984    #[inline]
985    pub fn nearest_point (self, point : Point3 <S>) -> TrianglePoint <S> {
986      distance::nearest_triangle3_point3 (self, point)
987    }
988  }
989  impl <S : Field + Sqrt> Default for Triangle <S> {
990    /// A default simplex is arbitrarily chosen to be the simplex on the XY plane formed
991    /// by an equilateral triangle with point at 1.0 on the Y axis:
992    ///
993    /// ```
994    /// # use math_utils::approx;
995    /// # use math_utils::geometry::simplex3::Triangle;
996    /// # use math_utils::*;
997    /// use std::f64::consts::FRAC_1_SQRT_2;
998    /// let s = Triangle::default();
999    /// let t = Triangle::noisy (
1000    ///   [           0.0,            1.0, 0.0].into(),
1001    ///   [-FRAC_1_SQRT_2, -FRAC_1_SQRT_2, 0.0].into(),
1002    ///   [ FRAC_1_SQRT_2, -FRAC_1_SQRT_2, 0.0].into()
1003    /// );
1004    /// approx::assert_relative_eq!(
1005    ///   Matrix3::from_col_arrays ([
1006    ///     s.point_a().0.into_array(),
1007    ///     s.point_b().0.into_array(),
1008    ///     s.point_c().0.into_array()
1009    ///   ]),
1010    ///   Matrix3::from_col_arrays ([
1011    ///     t.point_a().0.into_array(),
1012    ///     t.point_b().0.into_array(),
1013    ///     t.point_c().0.into_array()
1014    ///   ]));
1015    /// ```
1016    fn default() -> Self {
1017      let frac_1_sqrt_2 = S::one() / (S::one() + S::one()).sqrt();
1018      Triangle {
1019        a: [     S::zero(),       S::one(), S::zero()].into(),
1020        b: [-frac_1_sqrt_2, -frac_1_sqrt_2, S::zero()].into(),
1021        c: [ frac_1_sqrt_2, -frac_1_sqrt_2, S::zero()].into()
1022      }
1023    }
1024  }
1025  impl <S> std::ops::Index <usize> for Triangle <S> where S : Copy {
1026    type Output = Point3 <S>;
1027    fn index (&self, index : usize) -> &Point3 <S> {
1028      [&self.a, &self.b, &self.c][index]
1029    }
1030  }
1031  impl <S> TryFrom <[Point3 <S>; 3]> for Triangle <S> where
1032    S : OrderedField + Sqrt + approx::RelativeEq <Epsilon = S>
1033  {
1034    type Error = [Point3 <S>; 3];
1035    fn try_from ([a, b, c] : [Point3 <S>; 3]) -> Result <Self, Self::Error> {
1036      Self::new (a, b, c).ok_or([a, b, c])
1037    }
1038  }
1039  impl <S> Primitive <S, Point3 <S>> for Triangle <S> where
1040    S : OrderedField + approx::AbsDiffEq
1041  {
1042    fn translate (&mut self, displacement : Vector3 <S>) {
1043      self.a += displacement;
1044      self.b += displacement;
1045      self.c += displacement;
1046    }
1047    fn scale (&mut self, scale : Positive <S>) {
1048      let centroid = self.centroid();
1049      self.translate (-centroid.0);
1050      self.a = Point3 (self.a.0 * scale);
1051      self.b = Point3 (self.b.0 * scale);
1052      self.c = Point3 (self.c.0 * scale);
1053      self.translate (centroid.0);
1054    }
1055    fn support (&self, direction : NonZero3 <S>) -> (Point3 <S>, S) {
1056      let [dot_a, dot_b, dot_c] = self.points().map (|p| direction.dot (p.0));
1057      if dot_a >= dot_b && dot_a >= dot_c {
1058        (self.a, dot_a)
1059      } else if dot_b >= dot_c {
1060        (self.b, dot_b)
1061      } else {
1062        (self.c, dot_c)
1063      }
1064    }
1065  }
1066
1067  impl <S : OrderedField> Tetrahedron <S> {
1068    /// Returns `None` if the points are coplanar:
1069    ///
1070    /// ```
1071    /// # use math_utils::geometry::simplex3::Tetrahedron;
1072    /// assert!(Tetrahedron::new (
1073    ///   [-1.0, -1.0, -1.0].into(),
1074    ///   [ 1.0,  1.0,  1.0].into(),
1075    ///   [-1.0,  1.0,  0.0].into(),
1076    ///   [ 1.0, -1.0,  0.0].into()
1077    /// ).is_none());
1078    /// ```
1079    pub fn new (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>, d : Point3 <S>)
1080      -> Option <Self>
1081    where S : approx::AbsDiffEq <Epsilon = S> {
1082      if coplanar_3 (a, b, c, d) {
1083        None
1084      } else {
1085        Some (Tetrahedron { a, b, c, d })
1086      }
1087    }
1088    /// Panics if the points are coplanar:
1089    ///
1090    /// ```should_panic
1091    /// # use math_utils::geometry::simplex3::Tetrahedron;
1092    /// let s = Tetrahedron::noisy (
1093    ///   [-1.0, -1.0, -1.0].into(),
1094    ///   [ 1.0,  1.0,  1.0].into(),
1095    ///   [-1.0,  1.0,  0.0].into(),
1096    ///   [ 1.0, -1.0,  0.0].into());
1097    /// ```
1098    pub fn noisy (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>, d : Point3 <S>)
1099      -> Self
1100    where S : approx::AbsDiffEq <Epsilon = S> {
1101      assert!(!coplanar_3 (a, b, c, d));
1102      Tetrahedron { a, b, c, d }
1103    }
1104    /// Debug panic if the points are coplanar:
1105    ///
1106    /// ```should_panic
1107    /// # use math_utils::geometry::simplex3::Tetrahedron;
1108    /// let s = Tetrahedron::unchecked (
1109    ///   [-1.0, -1.0, -1.0].into(),
1110    ///   [ 1.0,  1.0,  1.0].into(),
1111    ///   [-1.0,  1.0,  0.0].into(),
1112    ///   [ 1.0, -1.0,  0.0].into());
1113    /// ```
1114    pub fn unchecked (a : Point3 <S>, b : Point3 <S>, c : Point3 <S>, d : Point3 <S>)
1115      -> Self
1116    where S : approx::AbsDiffEq <Epsilon = S> {
1117      debug_assert!(!coplanar_3 (a, b, c, d));
1118      Tetrahedron { a, b, c, d }
1119    }
1120    #[inline]
1121    pub fn from_array ([a, b, c, d] : [Point3 <S>; 4]) -> Option <Self> where
1122      S : approx::AbsDiffEq <Epsilon = S>
1123    {
1124      Self::new (a, b, c, d)
1125    }
1126    #[inline]
1127    pub const fn edge_ab (self) -> Segment3 <S> {
1128      Segment { a: self.a, b: self.b }
1129    }
1130    #[inline]
1131    pub const fn edge_ac (self) -> Segment3 <S> {
1132      Segment { a: self.a, b: self.c }
1133    }
1134    #[inline]
1135    pub const fn edge_ad (self) -> Segment3 <S> {
1136      Segment { a: self.a, b: self.d }
1137    }
1138    #[inline]
1139    pub const fn edge_bc (self) -> Segment3 <S> {
1140      Segment { a: self.b, b: self.c }
1141    }
1142    #[inline]
1143    pub const fn edge_bd (self) -> Segment3 <S> {
1144      Segment { a: self.b, b: self.d }
1145    }
1146    #[inline]
1147    pub const fn edge_cd (self) -> Segment3 <S> {
1148      Segment { a: self.c, b: self.d }
1149    }
1150    #[inline]
1151    pub const fn edges (self) -> [Segment3 <S>; 6] {
1152      [ self.edge_ab(),
1153        self.edge_ac(),
1154        self.edge_ad(),
1155        self.edge_bc(),
1156        self.edge_bd(),
1157        self.edge_cd()
1158      ]
1159    }
1160    /// Note that the points in the returned triangle will be ordered so that the
1161    /// `triangle.normal()` will face away from the tetrahedron.
1162    #[inline]
1163    pub fn face_abc (self) -> Triangle <S> {
1164      let triangle = Triangle { a: self.a, b: self.b, c: self.c };
1165      if triangle.normal().dot (self.d - self.a) > S::zero() {
1166        triangle.rewind()
1167      } else {
1168        triangle
1169      }
1170    }
1171    /// Note that the points in the returned triangle will be ordered so that the
1172    /// `triangle.normal()` will face away from the tetrahedron.
1173    #[inline]
1174    pub fn face_abd (self) -> Triangle <S> {
1175      let triangle = Triangle { a: self.a, b: self.b, c: self.d };
1176      if triangle.normal().dot (self.c - self.a) > S::zero() {
1177        triangle.rewind()
1178      } else {
1179        triangle
1180      }
1181    }
1182    /// Note that the points in the returned triangle will be ordered so that the
1183    /// `triangle.normal()` will face away from the tetrahedron.
1184    #[inline]
1185    pub fn face_acd (self) -> Triangle <S> {
1186      let triangle = Triangle { a: self.a, b: self.c, c: self.d };
1187      if triangle.normal().dot (self.b - self.a) > S::zero() {
1188        triangle.rewind()
1189      } else {
1190        triangle
1191      }
1192    }
1193    /// Note that the points in the returned triangle will be ordered so that the
1194    /// `triangle.normal()` will face away from the tetrahedron.
1195    #[inline]
1196    pub fn face_bcd (self) -> Triangle <S> {
1197      let triangle = Triangle { a: self.b, b: self.c, c: self.d };
1198      if triangle.normal().dot (self.a - self.b) > S::zero() {
1199        triangle.rewind()
1200      } else {
1201        triangle
1202      }
1203    }
1204    #[inline]
1205    pub fn faces (self) -> [Triangle <S>; 4] {
1206      [ self.face_abc(),
1207        self.face_abd(),
1208        self.face_acd(),
1209        self.face_bcd()
1210      ]
1211    }
1212    pub fn normal_abc (self) -> NonZero3 <S> {
1213      let mut abc_normal = self.face_abc().normal();
1214      let ad = self.d - self.a;
1215      if abc_normal.dot (ad) > S::zero() {
1216        abc_normal = -abc_normal;
1217      }
1218      abc_normal
1219    }
1220    pub fn normal_abd (self) -> NonZero3 <S> {
1221      let mut abd_normal = self.face_abd().normal();
1222      let ac = self.c - self.a;
1223      if abd_normal.dot (ac) > S::zero() {
1224        abd_normal = -abd_normal;
1225      }
1226      abd_normal
1227    }
1228    pub fn normal_acd (self) -> NonZero3 <S> {
1229      let mut acd_normal = self.face_acd().normal();
1230      let ab = self.b - self.a;
1231      if acd_normal.dot (ab) > S::zero() {
1232        acd_normal = -acd_normal;
1233      }
1234      acd_normal
1235    }
1236    pub fn normal_bcd (self) -> NonZero3 <S> {
1237      let mut bcd_normal = self.face_bcd().normal();
1238      let ba = self.a - self.b;
1239      if bcd_normal.dot (ba) > S::zero() {
1240        bcd_normal = -bcd_normal;
1241      }
1242      bcd_normal
1243    }
1244    pub fn volume (self) -> Positive <S> {
1245      Positive::unchecked (
1246        (self.b - self.a).cross (self.c - self.a).dot (self.d - self.a).abs()
1247          * (S::one() / S::six()))
1248    }
1249    #[inline]
1250    pub fn centroid (self) -> Point3 <S> {
1251      Point3 ((self.a.0 + self.b.0 + self.c.0 + self.d.0) / S::four())
1252    }
1253    /// Checks if a point is contained in the tetrahedron:
1254    ///
1255    /// ```
1256    /// # use math_utils::*;
1257    /// # use math_utils::geometry::simplex3::Tetrahedron;
1258    /// let s = Tetrahedron::noisy (
1259    ///   [ 0.0,  0.0,  1.0].into(),
1260    ///   [ 0.0,  1.0, -1.0].into(),
1261    ///   [-1.0, -1.0, -1.0].into(),
1262    ///   [ 1.0, -1.0, -1.0].into()
1263    /// );
1264    /// assert!(s.contains (Point3::origin()));
1265    /// assert!(!s.contains ([0.0, 0.0, 2.0].into()));
1266    /// ```
1267    #[inline]
1268    pub fn contains (self, point : Point3 <S>) -> bool {
1269      let ap = point - self.a;
1270      let bp = point - self.b;
1271      self.normal_abc().dot (ap) < S::zero() &&
1272      self.normal_abd().dot (ap) < S::zero() &&
1273      self.normal_acd().dot (ap) < S::zero() &&
1274      self.normal_bcd().dot (bp) < S::zero()
1275    }
1276  }
1277  impl <S : Field + Sqrt> Default for Tetrahedron <S> {
1278    /// A default simplex is arbitrarily chosen to be the simplex with vertices at unit
1279    /// distance from the origin with A at `[0.0, 0.0, 1.0]` and B at
1280    /// `[0.0, sqrt(8.0/9.0), -1.0/3.0]`, and points C and D at
1281    /// `[ sqrt(2.0/3.0), -sqrt(2.0/9.0), -1.0/3.0]` and
1282    /// `[-sqrt(2.0/3.0), -sqrt(2.0/9.0), -1.0/3.0]`.
1283    ///
1284    /// ```
1285    /// # use math_utils::approx;
1286    /// # use math_utils::geometry::simplex3::Tetrahedron;
1287    /// # use math_utils::*;
1288    /// let s = Tetrahedron::default();
1289    /// let t = Tetrahedron::noisy (
1290    ///   [                0.0,                 0.0,      1.0].into(),
1291    ///   [                0.0,  f64::sqrt(8.0/9.0), -1.0/3.0].into(),
1292    ///   [ f64::sqrt(2.0/3.0), -f64::sqrt(2.0/9.0), -1.0/3.0].into(),
1293    ///   [-f64::sqrt(2.0/3.0), -f64::sqrt(2.0/9.0), -1.0/3.0].into());
1294    /// approx::assert_relative_eq!(
1295    ///   Matrix4::from_col_arrays ([
1296    ///     s.point_a().0.with_w (0.0).into_array(),
1297    ///     s.point_b().0.with_w (0.0).into_array(),
1298    ///     s.point_c().0.with_w (0.0).into_array(),
1299    ///     s.point_d().0.with_w (0.0).into_array()
1300    ///   ]),
1301    ///   Matrix4::from_col_arrays ([
1302    ///     t.point_a().0.with_w (0.0).into_array(),
1303    ///     t.point_b().0.with_w (0.0).into_array(),
1304    ///     t.point_c().0.with_w (0.0).into_array(),
1305    ///     t.point_d().0.with_w (0.0).into_array()
1306    ///   ]));
1307    /// ```
1308    fn default() -> Self {
1309      let frac_1_3 = S::one()    / S::three();
1310      let sqrt_2_3 = (S::two()   / S::three()).sqrt();
1311      let sqrt_8_9 = (S::eight() / S::nine()).sqrt();
1312      let sqrt_2_9 = (S::two()   / S::nine()).sqrt();
1313      Tetrahedron {
1314        a: [S::zero(), S::zero(),  S::one()].into(),
1315        b: [S::zero(),  sqrt_8_9, -frac_1_3].into(),
1316        c: [ sqrt_2_3, -sqrt_2_9, -frac_1_3].into(),
1317        d: [-sqrt_2_3, -sqrt_2_9, -frac_1_3].into()
1318      }
1319    }
1320  }
1321  impl <S> std::ops::Index <usize> for Tetrahedron <S> where S : Copy {
1322    type Output = Point3 <S>;
1323    fn index (&self, index : usize) -> &Point3 <S> {
1324      [&self.a, &self.b, &self.c, &self.d][index]
1325    }
1326  }
1327  impl <S> TryFrom <[Point3 <S>; 4]> for Tetrahedron <S> where
1328    S : OrderedField + approx::RelativeEq <Epsilon = S>
1329  {
1330    type Error = [Point3 <S>; 4];
1331    fn try_from ([a, b, c, d] : [Point3 <S>; 4]) -> Result <Self, Self::Error> {
1332      Self::new (a, b, c, d).ok_or([a, b, c, d])
1333    }
1334  }
1335  impl <S> Primitive <S, Point3 <S>> for Tetrahedron <S> where
1336    S : OrderedField + approx::AbsDiffEq
1337  {
1338    fn translate (&mut self, displacement : Vector3 <S>) {
1339      self.a += displacement;
1340      self.b += displacement;
1341      self.c += displacement;
1342      self.d += displacement;
1343    }
1344    fn scale (&mut self, scale : Positive <S>) {
1345      let centroid = self.centroid();
1346      self.translate (-centroid.0);
1347      self.a = Point3 (self.a.0 * scale);
1348      self.b = Point3 (self.b.0 * scale);
1349      self.c = Point3 (self.c.0 * scale);
1350      self.d = Point3 (self.d.0 * scale);
1351      self.translate (centroid.0);
1352    }
1353    fn support (&self, direction : NonZero3 <S>) -> (Point3 <S>, S) {
1354      let [dot_a, dot_b, dot_c, dot_d] = self.points().map (|p| direction.dot (p.0));
1355      if dot_a >= dot_b && dot_a >= dot_c && dot_a >= dot_d {
1356        (self.a, dot_a)
1357      } else if dot_b >= dot_c && dot_b >= dot_d {
1358        (self.b, dot_b)
1359      } else if dot_c >= dot_d {
1360        (self.c, dot_c)
1361      } else {
1362        (self.d, dot_d)
1363      }
1364    }
1365  }
1366}
1367
1368#[cfg(test)]
1369mod tests {
1370  use crate::*;
1371  use crate::geometry::*;
1372  use super::*;
1373
1374  #[test]
1375  fn barycentric_2d() {
1376    let triangle = simplex2::Triangle::noisy (
1377      [-2.0, -2.0].into(), [2.0, -2.0].into(), [0.0, 2.0].into());
1378    assert_eq!(triangle.barycentric ([1.0, 0.0, 0.0].into()), [-2.0, -2.0].into());
1379    assert_eq!(triangle.barycentric ([0.0, 1.0, 0.0].into()), [ 2.0, -2.0].into());
1380    assert_eq!(triangle.barycentric ([0.0, 0.0, 1.0].into()), [ 0.0,  2.0].into());
1381    assert_eq!(triangle.barycentric ([0.5, 0.5, 0.0].into()), [ 0.0, -2.0].into());
1382    assert_eq!(triangle.barycentric ([0.5, 0.0, 0.5].into()), [-1.0,  0.0].into());
1383    assert_eq!(triangle.barycentric ([0.0, 0.5, 0.5].into()), [ 1.0,  0.0].into());
1384  }
1385
1386  #[test]
1387  fn barycentric_3d() {
1388    let triangle = simplex3::Triangle::noisy (
1389      [-2.0, -2.0, 0.0].into(), [2.0, -2.0, 0.0].into(), [0.0, 2.0, 0.0].into());
1390    assert_eq!(triangle.barycentric ([1.0, 0.0, 0.0].into()), [-2.0, -2.0, 0.0].into());
1391    assert_eq!(triangle.barycentric ([0.0, 1.0, 0.0].into()), [ 2.0, -2.0, 0.0].into());
1392    assert_eq!(triangle.barycentric ([0.0, 0.0, 1.0].into()), [ 0.0,  2.0, 0.0].into());
1393    assert_eq!(triangle.barycentric ([0.5, 0.5, 0.0].into()), [ 0.0, -2.0, 0.0].into());
1394    assert_eq!(triangle.barycentric ([0.5, 0.0, 0.5].into()), [-1.0,  0.0, 0.0].into());
1395    assert_eq!(triangle.barycentric ([0.0, 0.5, 0.5].into()), [ 1.0,  0.0, 0.0].into());
1396  }
1397
1398  #[test]
1399  fn trilinear_2d() {
1400    let triangle = simplex2::Triangle::noisy (
1401      [-2.0, -2.0].into(), [2.0, -2.0].into(), [0.0, 2.0].into());
1402    assert_eq!(triangle.trilinear ([1.0, 0.0, 0.0].into()), [-2.0, -2.0].into());
1403    assert_eq!(triangle.trilinear ([0.0, 1.0, 0.0].into()), [ 2.0, -2.0].into());
1404    assert_eq!(triangle.trilinear ([0.0, 0.0, 1.0].into()), [ 0.0,  2.0].into());
1405    // centroid
1406    assert_eq!(triangle.trilinear (
1407      [1.0 / 20.0f32.sqrt(), 1.0 / 20.0f32.sqrt(), 0.25].into()),
1408      [0.0,  -2.0/3.0].into());
1409    // incenter
1410    approx::assert_relative_eq!(triangle.trilinear (
1411      [1.0, 1.0, 1.0].into()),
1412      [ 0.0, 5.0f32.sqrt() - 3.0].into());
1413  }
1414
1415  #[test]
1416  fn trilinear_3d() {
1417    let triangle = simplex3::Triangle::noisy (
1418      [-2.0, -2.0, 0.0].into(), [2.0, -2.0, 0.0].into(), [0.0, 2.0, 0.0].into());
1419    assert_eq!(triangle.trilinear ([1.0, 0.0, 0.0].into()), [-2.0, -2.0, 0.0].into());
1420    assert_eq!(triangle.trilinear ([0.0, 1.0, 0.0].into()), [ 2.0, -2.0, 0.0].into());
1421    assert_eq!(triangle.trilinear ([0.0, 0.0, 1.0].into()), [ 0.0,  2.0, 0.0].into());
1422    // centroid
1423    assert_eq!(triangle.trilinear (
1424      [1.0 / 20.0f32.sqrt(), 1.0 / 20.0f32.sqrt(), 0.25].into()),
1425      [0.0,  -2.0/3.0, 0.0].into());
1426    // incenter
1427    approx::assert_relative_eq!(triangle.trilinear (
1428      [1.0, 1.0, 1.0].into()),
1429      [ 0.0, 5.0f32.sqrt() - 3.0, 0.0].into());
1430  }
1431
1432  #[test]
1433  fn scale() {
1434    let mut s = simplex2::Segment::noisy (point!(-2.0, 2.0), point!(2.0, 2.0));
1435    s.scale (Positive::noisy (0.5));
1436    assert_eq!(s.point_a(), point!(-1.0, 2.0));
1437    assert_eq!(s.point_b(), point!( 1.0, 2.0));
1438  }
1439
1440  #[test]
1441  fn tetrahedron_face_normals() {
1442    // face normals should face out from tetrahedron
1443    use rand;
1444    use rand_xorshift::XorShiftRng;
1445    use rand::SeedableRng;
1446    let aabb = Aabb3::<f32>::with_minmax_unchecked (
1447      [-10.0, -10.0, -10.0].into(),
1448      [ 10.0,  10.0,  10.0].into());
1449    let mut rng = XorShiftRng::seed_from_u64 (0);
1450    for _ in 0..10000 {
1451      let t = Tetrahedron3::noisy (
1452        aabb.rand_point (&mut rng),
1453        aabb.rand_point (&mut rng),
1454        aabb.rand_point (&mut rng),
1455        aabb.rand_point (&mut rng));
1456      for face in t.faces() {
1457        let other_p = {
1458          let mut point = None;
1459          for p in t.points() {
1460            if !face.points().contains (&p) {
1461              point = Some (p);
1462              break
1463            }
1464          }
1465          point.unwrap()
1466        };
1467        assert!(face.normal().dot (other_p - face.point_a()) < 0.0);
1468      }
1469    }
1470  }
1471}
math_utils/geometry/primitive/simplex/mod.rs

math_utils/geometry/primitive/simplex/
mod.rs
