math_utils/geometry/primitive/
simplex.rs

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

math_utils/geometry/primitive/
simplex.rs
