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

math_utils/geometry/primitive/
simplex.rs
