Skip to main content

math_utils/types/
mod.rs

1use std::marker::PhantomData;
2use derive_more::{Deref, Display, From, Neg};
3
4use crate::{approx, num};
5
6#[cfg(feature = "derive_serdes")]
7use serde::{Deserialize, Serialize};
8
9use crate::traits::*;
10
11pub mod coordinate;
12pub mod frame;
13
14pub use vek::Vec2 as Vector2;
15pub use vek::Vec3 as Vector3;
16pub use vek::Vec4 as Vector4;
17pub use vek::Mat2 as Matrix2;
18pub use vek::Mat3 as Matrix3;
19pub use vek::Mat4 as Matrix4;
20pub use vek::Quaternion as Quaternion;
21
22pub use self::enums::{Axis2, Axis3, Axis4, Octant, Quadrant, Sign, SignedAxis1,
23  SignedAxis2, SignedAxis3, SignedAxis4};
24
25////////////////////////////////////////////////////////////////////////////////////////
26//  enums                                                                             //
27////////////////////////////////////////////////////////////////////////////////////////
28
29// we define enums in a separate module so that the generated iterator types don't
30// pollute the namespace
31mod enums {
32  use strum::{EnumCount, EnumIter, FromRepr};
33  #[cfg(feature = "derive_serdes")]
34  use serde::{Deserialize, Serialize};
35
36  /// Negative, zero, or positive
37  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
38  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
39  #[repr(u8)]
40  pub enum Sign {
41    Negative,
42    Zero,
43    Positive
44  }
45
46  /// 2D quadrant
47  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
48  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
49  #[repr(u8)]
50  pub enum Quadrant {
51    /// +X, +Y
52    PosPos,
53    /// -X, +Y
54    NegPos,
55    /// +X, -Y
56    PosNeg,
57    /// -X, -Y
58    NegNeg
59  }
60
61  /// 3D octant
62  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
63  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
64  #[repr(u8)]
65  pub enum Octant {
66    /// +X, +Y, +Z
67    PosPosPos,
68    /// -X, +Y, +Z
69    NegPosPos,
70    /// +X, -Y, +Z
71    PosNegPos,
72    /// -X, -Y, +Z
73    NegNegPos,
74    /// +X, +Y, -Z
75    PosPosNeg,
76    /// -X, +Y, -Z
77    NegPosNeg,
78    /// +X, -Y, -Z
79    PosNegNeg,
80    /// -X, -Y, -Z
81    NegNegNeg
82  }
83
84  /// X or Y axis
85  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
86  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
87  #[repr(u8)]
88  pub enum Axis2 {
89    X=0,
90    Y
91  }
92
93  /// X, Y, or Z axis
94  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
95  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
96  #[repr(u8)]
97  pub enum Axis3 {
98    X=0,
99    Y,
100    Z
101  }
102
103  /// X, Y, Z, or W axis
104  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
105  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
106  #[repr(u8)]
107  pub enum Axis4 {
108    X=0,
109    Y,
110    Z,
111    W
112  }
113
114  /// Positive or negative X axis
115  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
116  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
117  #[repr(u8)]
118  pub enum SignedAxis1 {
119    NegX, PosX
120  }
121
122  /// Positive or negative X or Y axis
123  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
124  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
125  #[repr(u8)]
126  pub enum SignedAxis2 {
127    NegX, PosX,
128    NegY, PosY
129  }
130
131  /// Positive or negative X, Y, or Z axis
132  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
133  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
134  #[repr(u8)]
135  pub enum SignedAxis3 {
136    NegX, PosX,
137    NegY, PosY,
138    NegZ, PosZ
139  }
140
141  /// Positive or negative X, Y, Z, or W axis
142  #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
143  #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
144  #[repr(u8)]
145  pub enum SignedAxis4 {
146    NegX, PosX,
147    NegY, PosY,
148    NegZ, PosZ,
149    NegW, PosW
150  }
151}
152
153////////////////////////////////////////////////////////////////////////////////////////
154//  structs                                                                           //
155////////////////////////////////////////////////////////////////////////////////////////
156
157/// Operation witness type for `AdditiveGroup`s and `AdditiveMonoid`s
158#[derive(Debug, PartialEq, Eq)]
159pub struct Addition;
160
161/// Operation witness type for `MultplicativeGroup`s and `MultiplicativeMonoid`s
162#[derive(Debug, PartialEq, Eq)]
163pub struct Multiplication;
164
165/// Strictly positive scalars (non-zero)
166#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
167#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
168pub struct Positive <S> (S);
169
170/// Non-negative scalars (may be zero)
171#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
172#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
173pub struct NonNegative <S> (S);
174
175/// Non-zero scalars
176#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
177#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
178pub struct NonZero <S> (S);
179
180/// Scalars in the closed unit interval [0, 1]
181#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
182#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
183pub struct Normalized <S> (S);
184
185/// Scalars in the closed interval [-1, 1]
186#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
187#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
188pub struct NormalSigned <S> (S);
189
190/// Scalars in the discrete set {-1, 1}
191#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
192#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
193pub struct Unit <S> (S);
194
195/// (Euler angles) representation of a 3D orientation. Internally the angles are wrapped
196/// to $[0, 2\pi)$.
197///
198/// Throughout this library this is usually interpreted as intrinsic ZX'Y'' Euler
199/// angles.
200#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
201#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
202pub struct Angles3 <S> {
203  pub yaw   : AngleWrapped <S>,
204  pub pitch : AngleWrapped <S>,
205  pub roll  : AngleWrapped <S>
206}
207
208/// Representation of a 3D position + orientation
209#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
210#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
211pub struct Pose3 <S> {
212  pub position : Point3  <S>,
213  pub angles   : Angles3 <S>
214}
215
216/// Invertible linear map
217#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
218#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
219#[display("{}", _0)]
220pub struct LinearIso <S, V, W, M> (M, PhantomData <(S, V, W)>);
221
222/// Invertible linear endomorphism
223#[derive(Clone, Copy, Debug, Default, PartialEq, Deref, Display, From)]
224#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
225#[display("{}", _0)]
226pub struct LinearAuto <S, V> (pub LinearIso <S, V, V, V::LinearEndo>) where
227  V : Module <S>,
228  V::LinearEndo : MaybeSerDes,
229  S : Ring;
230
231/// Orthogonal 2x2 matrix with determinant +1, i.e. a member of the circle group $SO(2)$
232/// of special orthogonal matrices
233#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
234#[cfg_attr(feature = "derive_serdes", serde(bound = "S : MaybeSerDes"))]
235#[derive(Clone, Copy, Debug, Default, PartialEq)]
236pub struct Rotation2 <S> (LinearAuto <S, Vector2 <S>>) where S : Ring + MaybeSerDes;
237
238/// Orthogonal 3x3 matrix with determinant +1, i.e. a member of the 3D rotation group
239/// $SO(3)$ of special orthogonal matrices
240#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
241#[cfg_attr(feature = "derive_serdes", serde(bound = "S : MaybeSerDes"))]
242#[derive(Clone, Copy, Debug, Default, PartialEq)]
243pub struct Rotation3 <S> (LinearAuto <S, Vector3 <S>>) where S : Ring + MaybeSerDes;
244
245/// Unit quaternion representing an orientation in $\mathbb{R}^3$
246#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
247#[derive(Clone, Copy, Debug, Eq, PartialEq)]
248pub struct Versor <S> (Quaternion <S>);
249
250/// Homomorphism of affine spaces: a combination of a linear map and a translation
251#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
252#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
253#[display("({}, {})", linear_map, translation)]
254pub struct AffineMap <S, A, B, M> where
255  A : AffineSpace <S>,
256  B : AffineSpace <S>,
257  S : Field,
258  M : LinearMap <S, A::Translation, B::Translation>
259{
260  pub linear_map  : M,
261  pub translation : B::Translation,
262  pub _phantom    : PhantomData <(A, S)>
263}
264
265/// Affine isomorphism
266#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
267#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
268#[display("({}, {})", linear_iso, translation)]
269pub struct Affinity <S, A, B, M> where
270  S : Field,
271  A : AffineSpace <S>,
272  B : AffineSpace <S>,
273  M : LinearMap <S, A::Translation, B::Translation>
274{
275  pub linear_iso  : LinearIso <S, A::Translation, B::Translation, M>,
276  pub translation : B::Translation
277}
278
279/// Isomorphism of projective spaces (a.k.a. homography or projective collineation)
280#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
281#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
282#[display("{}", _0)]
283pub struct Projectivity <S, P, Q, M> (
284  pub LinearIso <S, P::Translation, Q::Translation, M>
285) where
286  S : Field,
287  P : ProjectiveSpace <S>,
288  Q : ProjectiveSpace <S>;
289
290////////////////////////////////////////////////////////////////////////////////////////
291//  impls                                                                             //
292////////////////////////////////////////////////////////////////////////////////////////
293
294//
295//  impl Addition
296//
297impl <G : AdditiveGroup> Group <G> for Addition {
298  fn inverse (elem : G) -> G {
299    -elem
300  }
301}
302impl <M : AdditiveMonoid> Monoid <M> for Addition {
303  fn identity() -> M {
304    M::zero()
305  }
306}
307impl <S : std::ops::Add <Output=S>> Semigroup <S> for Addition {
308  fn operation (a : S, b : S) -> S {
309    a + b
310  }
311}
312
313//
314//  impl Multiplication
315//
316impl <G : MultiplicativeGroup> Group <G> for Multiplication {
317  fn inverse (elem : G) -> G {
318    elem.inv()
319  }
320}
321impl <M : MultiplicativeMonoid> Monoid <M> for Multiplication {
322  fn identity() -> M {
323    M::one()
324  }
325}
326impl <S : std::ops::Mul <Output=S>> Semigroup <S> for Multiplication {
327  fn operation (a : S, b : S) -> S {
328    a * b
329  }
330}
331
332//
333//  impl Deg
334//
335impl <S : OrderedField> Angle <S> for Deg <S> {
336  fn full_turn() -> Self {
337    Deg (S::ten() * S::nine() * S::two() * S::two())
338  }
339  fn half_turn() -> Self {
340    Deg (S::ten() * S::nine() * S::two())
341  }
342}
343impl <S : Real> Trig for Deg <S> {
344  fn sin     (self) -> Self {
345    Rad::from (self).sin().into()
346  }
347  fn sin_cos (self) -> (Self, Self) {
348    let (sin, cos) = Rad::from (self).sin_cos();
349    (sin.into(), cos.into())
350  }
351  fn cos     (self) -> Self {
352    Rad::from (self).cos().into()
353  }
354  fn tan     (self) -> Self {
355    Rad::from (self).tan().into()
356  }
357  fn asin    (self) -> Self {
358    Rad::from (self).asin().into()
359  }
360  fn acos    (self) -> Self {
361    Rad::from (self).acos().into()
362  }
363  fn atan    (self) -> Self {
364    Rad::from (self).atan().into()
365  }
366  fn atan2   (self, other : Self) -> Self {
367    Rad::from (self).atan2 (Rad::from (other)).into()
368  }
369}
370impl <S : Real> From <Rad <S>> for Deg <S> {
371  fn from (radians : Rad <S>) -> Self {
372    let full_turns = radians / Rad::full_turn();
373    Deg::full_turn() * full_turns
374  }
375}
376impl <S : OrderedField> From <Turn <S>> for Deg <S> {
377  fn from (turns : Turn <S>) -> Self {
378    Deg (turns.0 * Deg::full_turn().0)
379  }
380}
381
382//
383//  impl Rad
384//
385impl <S : Real> Angle <S> for Rad <S> {
386  fn full_turn() -> Self {
387    Rad (S::pi() * S::two())
388  }
389}
390impl <S : Trig> Trig for Rad <S> {
391  fn sin     (self) -> Self {
392    Rad (self.0.sin())
393  }
394  fn sin_cos (self) -> (Self, Self) {
395    let (sin, cos) = self.0.sin_cos();
396    (Rad (sin), Rad (cos))
397  }
398  fn cos     (self) -> Self {
399    Rad (self.0.cos())
400  }
401  fn tan     (self) -> Self {
402    Rad (self.0.tan())
403  }
404  fn asin    (self) -> Self {
405    Rad (self.0.asin())
406  }
407  fn acos    (self) -> Self {
408    Rad (self.0.acos())
409  }
410  fn atan    (self) -> Self {
411    Rad (self.0.atan())
412  }
413  fn atan2   (self, other : Self) -> Self {
414    Rad (self.0.atan2 (other.0))
415  }
416}
417impl <S : Real> From <Deg <S>> for Rad <S> {
418  fn from (degrees : Deg <S>) -> Self {
419    let full_turns = degrees / Deg::full_turn();
420    Rad::full_turn() * full_turns
421  }
422}
423impl <S : Real> From <Turn <S>> for Rad <S> {
424  fn from (turns : Turn <S>) -> Self {
425    Rad (turns.0 * Rad::full_turn().0)
426  }
427}
428
429//
430//  impl Turn
431//
432impl <S : OrderedField> Angle <S> for Turn <S> {
433  fn full_turn() -> Self {
434    Turn (S::one())
435  }
436  fn half_turn() -> Self {
437    Turn (S::half())
438  }
439}
440impl <S : Real> Trig for Turn <S> {
441  fn sin     (self) -> Self {
442    Rad::from (self).sin().into()
443  }
444  fn sin_cos (self) -> (Self, Self) {
445    let (sin, cos) = Rad::from (self).sin_cos();
446    (sin.into(), cos.into())
447  }
448  fn cos     (self) -> Self {
449    Rad::from (self).cos().into()
450  }
451  fn tan     (self) -> Self {
452    Rad::from (self).tan().into()
453  }
454  fn asin    (self) -> Self {
455    Rad::from (self).asin().into()
456  }
457  fn acos    (self) -> Self {
458    Rad::from (self).acos().into()
459  }
460  fn atan    (self) -> Self {
461    Rad::from (self).atan().into()
462  }
463  fn atan2   (self, other : Self) -> Self {
464    Rad::from (self).atan2 (Rad::from (other)).into()
465  }
466}
467impl <S : Real> From <Rad <S>> for Turn <S> {
468  fn from (radians : Rad <S>) -> Self {
469    Turn (radians.0 / Rad::full_turn().0)
470  }
471}
472impl <S : OrderedField> From <Deg <S>> for Turn <S> {
473  fn from (degrees : Deg <S>) -> Self {
474    Turn (degrees.0 / Deg::full_turn().0)
475  }
476}
477
478//
479//  impl AngleWrapped
480//
481impl <S : Real> AngleWrapped <S> {
482  pub fn wrap (angle : Rad <S>) -> Self {
483    AngleWrapped (angle.wrap_unsigned())
484  }
485
486  #[must_use]
487  pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
488    AngleWrapped (f (self.0).wrap_unsigned())
489  }
490}
491
492//
493//  impl AngleWrappedSigned
494//
495impl <S : Real> AngleWrappedSigned <S> {
496  pub fn wrap (angle : Rad <S>) -> Self {
497    AngleWrappedSigned (angle.wrap_signed())
498  }
499
500  #[must_use]
501  pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
502    AngleWrappedSigned (f (self.0).wrap_signed())
503  }
504}
505
506//
507//  impl Angles3
508//
509
510impl <S> Angles3 <S> {
511  pub const fn new (
512    yaw   : AngleWrapped <S>,
513    pitch : AngleWrapped <S>,
514    roll  : AngleWrapped <S>
515  ) -> Self {
516    Angles3 { yaw, pitch, roll }
517  }
518
519  pub fn zero() -> Self where S : Real {
520    use num::Zero;
521    Angles3::new (
522      AngleWrapped::zero(),
523      AngleWrapped::zero(),
524      AngleWrapped::zero())
525  }
526
527  pub fn wrap (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>) -> Self where S : Real {
528    let yaw   = AngleWrapped::wrap (yaw);
529    let pitch = AngleWrapped::wrap (pitch);
530    let roll  = AngleWrapped::wrap (roll);
531    Angles3 { yaw, pitch, roll }
532  }
533}
534
535impl <S> From <Rotation3 <S>> for Angles3 <S> where S : Real + MaybeSerDes {
536  fn from (rotation : Rotation3 <S>) -> Self {
537    let (yaw, pitch, roll) = {
538      let (yaw, pitch, roll) = rotation.intrinsic_angles();
539      ( AngleWrapped::wrap (yaw),
540        AngleWrapped::wrap (pitch),
541        AngleWrapped::wrap (roll)
542      )
543    };
544    Angles3 { yaw, pitch, roll }
545  }
546}
547
548//
549//  impl Axis2
550//
551impl Axis2 {
552  #[inline]
553  pub const fn component (self) -> usize {
554    self as usize
555  }
556}
557
558//
559//  impl Axis3
560//
561impl Axis3 {
562  #[inline]
563  pub const fn component (self) -> usize {
564    self as usize
565  }
566}
567
568//
569//  impl SignedAxis3
570//
571impl SignedAxis3 {
572  pub fn to_vec <S : Field> (self) -> Vector3 <S> {
573    match self {
574      SignedAxis3::NegX => [-S::one(),   S::zero(),  S::zero()].into(),
575      SignedAxis3::PosX => [ S::one(),   S::zero(),  S::zero()].into(),
576      SignedAxis3::NegY => [ S::zero(), -S::one(),   S::zero()].into(),
577      SignedAxis3::PosY => [ S::zero(),  S::one(),   S::zero()].into(),
578      SignedAxis3::NegZ => [ S::zero(),  S::zero(), -S::one() ].into(),
579      SignedAxis3::PosZ => [ S::zero(),  S::zero(),  S::one() ].into()
580    }
581  }
582  #[inline]
583  pub fn to_unit <S> (self) -> Unit3 <S> where S : Real + std::fmt::Debug {
584    Unit3::unchecked (self.to_vec())
585  }
586}
587impl <S : Field> TryFrom <Vector3 <S>> for SignedAxis3 {
588  type Error = Vector3 <S>;
589  fn try_from (vector : Vector3 <S>) -> Result <Self, Self::Error> {
590    let axis = if vector == Vector3::unit_x() {
591      SignedAxis3::PosX
592    } else if vector == -Vector3::unit_x() {
593      SignedAxis3::NegX
594    } else if vector == Vector3::unit_y() {
595      SignedAxis3::PosY
596    } else if vector == -Vector3::unit_y() {
597      SignedAxis3::NegY
598    } else if vector == Vector3::unit_z() {
599      SignedAxis3::PosZ
600    } else if vector == -Vector3::unit_z() {
601      SignedAxis3::NegZ
602    } else {
603      return Err (vector)
604    };
605    Ok (axis)
606  }
607}
608
609//
610//  impl Quadrant
611//
612impl Quadrant {
613  /// Returns some 'Some (quadrant)' if the vector is strictly contained within a
614  /// quadrant and returns 'None' otherwise
615  pub fn from_vec_strict <S> (vector : Vector2 <S>) -> Option <Quadrant> where
616    S : Ring + SignedExt
617  {
618    let sign_x = vector.x.sign();
619    let sign_y = vector.y.sign();
620    let quadrant = match (sign_x, sign_y) {
621      (Sign::Zero, _) | (_, Sign::Zero) => return None,
622      (Sign::Positive, Sign::Positive)  => Quadrant::PosPos,
623      (Sign::Negative, Sign::Positive)  => Quadrant::NegPos,
624      (Sign::Positive, Sign::Negative)  => Quadrant::PosNeg,
625      (Sign::Negative, Sign::Negative)  => Quadrant::NegNeg
626    };
627    Some (quadrant)
628  }
629  /// Same as `from_vec_strict` for a point
630  #[inline]
631  pub fn from_point_strict <S> (point : Point2 <S>) -> Option <Quadrant> where
632    S : Ring + SignedExt
633  {
634    Quadrant::from_vec_strict (point.to_vector())
635  }
636}
637//  end impl Quadrant
638
639//
640//  impl Octant
641//
642impl Octant {
643  /// Returns some 'Some (octant)' if the vector is strictly contained within an octant
644  /// and returns 'None' otherwise
645  pub fn from_vec_strict <S> (vector : Vector3 <S>) -> Option <Octant> where
646    S : Ring + SignedExt
647  {
648    let sign_x = vector.x.sign();
649    let sign_y = vector.y.sign();
650    let sign_z = vector.z.sign();
651    let octant = match (sign_x, sign_y, sign_z) {
652      (Sign::Zero,          _,          _) |
653      (         _, Sign::Zero,          _) |
654      (         _,          _, Sign::Zero) => return None,
655      (Sign::Positive, Sign::Positive, Sign::Positive) => Octant::PosPosPos,
656      (Sign::Negative, Sign::Positive, Sign::Positive) => Octant::NegPosPos,
657      (Sign::Positive, Sign::Negative, Sign::Positive) => Octant::PosNegPos,
658      (Sign::Negative, Sign::Negative, Sign::Positive) => Octant::NegNegPos,
659      (Sign::Positive, Sign::Positive, Sign::Negative) => Octant::PosPosNeg,
660      (Sign::Negative, Sign::Positive, Sign::Negative) => Octant::NegPosNeg,
661      (Sign::Positive, Sign::Negative, Sign::Negative) => Octant::PosNegNeg,
662      (Sign::Negative, Sign::Negative, Sign::Negative) => Octant::NegNegNeg
663    };
664    Some (octant)
665  }
666  /// Same as `from_vec_strict` for a point
667  #[inline]
668  pub fn from_point_strict <S> (point : Point3 <S>) -> Option <Octant> where
669    S : Ring + SignedExt
670  {
671    Octant::from_vec_strict (point.to_vector())
672  }
673}
674//  end impl Octant
675
676//
677//  impl Positive
678//
679impl <S> Positive <S> where S : PartialOrd + num::Zero {
680  /// Returns 'None' when called with a negative or zero value.
681  ///
682  /// # Example
683  ///
684  /// ```
685  /// # use math_utils::Positive;
686  /// assert!(Positive::new (1.0).is_some());
687  /// assert!(Positive::new (0.0).is_none());
688  /// assert!(Positive::new (-1.0).is_none());
689  /// ```
690  pub fn new (value : S) -> Option <Self> {
691    if value > S::zero() {
692      Some (Positive (value))
693    } else {
694      None
695    }
696  }
697  /// Panics if negative or zero.
698  ///
699  /// # Panics
700  ///
701  /// ```should_panic
702  /// # use math_utils::Positive;
703  /// let x = Positive::noisy (0.0);  // panic!
704  /// ```
705  pub fn noisy (value : S) -> Self {
706    assert!(value > S::zero());
707    Positive (value)
708  }
709  /// Create a new positive number without checking the value.
710  ///
711  /// In debug builds this will fail with a debug assertion if the value is negative:
712  ///
713  /// ```should_panic
714  /// # use math_utils::Positive;
715  /// let negative = Positive::unchecked (-1.0);  // panic!
716  /// ```
717  pub fn unchecked (value : S) -> Self {
718    debug_assert!(value > S::zero());
719    Positive (value)
720  }
721  /// Map an operation on the underlying scalar, panicking if the result is zero or
722  /// negative.
723  ///
724  /// # Example
725  ///
726  /// ```
727  /// # use math_utils::Positive;
728  /// # use math_utils::num::One;
729  /// assert_eq!(*Positive::<f64>::one().map_noisy (|x| 2.0 * x), 2.0);
730  /// ```
731  ///
732  /// # Panics
733  ///
734  /// Panics of the result is negative or zero:
735  ///
736  /// ```should_panic
737  /// # use math_utils::Positive;
738  /// # use math_utils::num::One;
739  /// let v = Positive::<f64>::one().map_noisy (|_| 0.0);  // panic!
740  /// ```
741  #[must_use]
742  pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
743    Self::noisy (fun (self.0))
744  }
745}
746impl <S> Positive <S> where S : num::float::FloatCore {
747  /// Positive infinity
748  pub fn infinity() -> Self {
749    Positive (S::infinity())
750  }
751}
752impl Positive <f32> {
753  /// Const constructor for `f32`
754  pub const fn new_f32 (value : f32) -> Option <Self> {
755    if 0.0 < value {
756      Some (Positive (value))
757    } else {
758      None
759    }
760  }
761  /// Const constructor for `f32` with assertions
762  pub const fn noisy_f32 (value : f32) -> Self {
763    assert!(0.0 < value);
764    Positive (value)
765  }
766  /// Const constructor for `f32` with debug assertions
767  pub const fn unchecked_f32 (value : f32) -> Self {
768    debug_assert!(0.0 < value);
769    Positive (value)
770  }
771}
772impl Positive <f64> {
773  /// Const constructor for `f64`
774  pub const fn new_f64 (value : f64) -> Option <Self> {
775    if 0.0 < value {
776      Some (Positive (value))
777    } else {
778      None
779    }
780  }
781  /// Const constructor for `f64` with assertions
782  pub const fn noisy_f64 (value : f64) -> Self {
783    assert!(0.0 < value);
784    Positive (value)
785  }
786  /// Const constructor for `f64` with debug assertions
787  pub const fn unchecked_f64 (value : f64) -> Self {
788    debug_assert!(0.0 < value);
789    Positive (value)
790  }
791}
792impl <S : Ring> num::One for Positive <S> {
793  fn one() -> Self {
794    Positive (S::one())
795  }
796}
797impl <S> std::ops::Deref for Positive <S> {
798  type Target = S;
799  fn deref (&self) -> &S {
800    &self.0
801  }
802}
803impl <S : Ring> std::ops::Mul <Positive <S>> for Positive <S> {
804  type Output = Positive <S>;
805  fn mul (self, rhs : Positive <S>) -> Self::Output {
806    Positive (self.0 * *rhs)
807  }
808}
809impl <S : Ring> std::ops::MulAssign <Positive <S>> for Positive <S> {
810  fn mul_assign (&mut self, rhs : Positive <S>) {
811    self.0 *= *rhs
812  }
813}
814impl <S : Ring> std::ops::Mul <NonNegative <S>> for Positive <S> {
815  type Output = NonNegative <S>;
816  fn mul (self, rhs : NonNegative <S>) -> Self::Output {
817    NonNegative (self.0 * *rhs)
818  }
819}
820impl <S : Ring> std::ops::MulAssign <NonNegative <S>> for Positive <S> {
821  fn mul_assign (&mut self, rhs : NonNegative <S>) {
822    self.0 *= *rhs
823  }
824}
825impl <S : Field> std::ops::Div for Positive <S> {
826  type Output = Self;
827  fn div (self, rhs : Self) -> Self::Output {
828    Positive (self.0 / rhs.0)
829  }
830}
831impl <S : Field> std::ops::DivAssign for Positive <S> {
832  fn div_assign (&mut self, rhs : Self) {
833    self.0 /= rhs.0
834  }
835}
836impl <S : Ring> std::ops::Add for Positive <S> {
837  type Output = Self;
838  fn add (self, rhs : Self) -> Self::Output {
839    Positive (self.0 + rhs.0)
840  }
841}
842impl <S : Ring> std::ops::AddAssign for Positive <S> {
843  fn add_assign (&mut self, rhs : Self) {
844    self.0 += rhs.0
845  }
846}
847impl <S : Ring> std::ops::Add <NonNegative <S>> for Positive <S> {
848  type Output = Self;
849  fn add (self, rhs : NonNegative <S>) -> Self::Output {
850    Positive (self.0 + rhs.0)
851  }
852}
853impl <S : Ring> std::ops::AddAssign <NonNegative <S>> for Positive <S> {
854  fn add_assign (&mut self, rhs : NonNegative <S>) {
855    self.0 += rhs.0
856  }
857}
858//  end impl Positive
859
860//
861//  impl NonNegative
862//
863impl <S> NonNegative <S> where S : PartialOrd + num::Zero {
864  /// Returns 'None' when called with a negative value.
865  ///
866  /// # Example
867  ///
868  /// ```
869  /// # use math_utils::NonNegative;
870  /// assert!(NonNegative::new (1.0).is_some());
871  /// assert!(NonNegative::new (0.0).is_some());
872  /// assert!(NonNegative::new (-1.0).is_none());
873  /// ```
874  pub fn new (value : S) -> Option <Self> {
875    if value >= S::zero() {
876      Some (NonNegative (value))
877    } else {
878      None
879    }
880  }
881  /// Panics if negative.
882  ///
883  /// # Panics
884  ///
885  /// ```should_panic
886  /// # use math_utils::NonNegative;
887  /// let x = NonNegative::noisy (-1.0);  // panic!
888  /// ```
889  pub fn noisy (value : S) -> Self {
890    assert!(S::zero() <= value);
891    NonNegative (value)
892  }
893  /// Create a new non-negative number without checking the value.
894  ///
895  /// This method is completely unchecked for release builds. Debug builds will panic if
896  /// the value is negative:
897  ///
898  /// ```should_panic
899  /// # use math_utils::NonNegative;
900  /// let negative = NonNegative::unchecked (-1.0);   // panic!
901  /// ```
902  pub fn unchecked (value : S) -> Self {
903    debug_assert!(value >= S::zero());
904    NonNegative (value)
905  }
906  /// Map an operation on the underlying scalar, panicking if the result is negative.
907  ///
908  /// # Example
909  ///
910  /// ```
911  /// # use math_utils::NonNegative;
912  /// assert_eq!(*NonNegative::abs (1.0).map_noisy (|x| 2.0 * x), 2.0);
913  /// ```
914  ///
915  /// # Panics
916  ///
917  /// Panics of the result is negative:
918  ///
919  /// ```should_panic
920  /// # use math_utils::NonNegative;
921  /// let v = NonNegative::abs (1.0).map_noisy (|x| -1.0 * x);  // panic!
922  /// ```
923  #[must_use]
924  pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
925    Self::noisy (fun (self.0))
926  }
927}
928impl <S> NonNegative <S> where S : num::Signed {
929  /// Converts negative input to absolute value.
930  ///
931  /// # Example
932  ///
933  /// ```
934  /// # use math_utils::NonNegative;
935  /// assert_eq!(*NonNegative::abs (1.0), 1.0);
936  /// assert_eq!(*NonNegative::abs (-1.0), 1.0);
937  /// ```
938  pub fn abs (value : S) -> Self {
939    NonNegative (value.abs())
940  }
941  /// Map an operation on the underlying scalar, converting negative result to an
942  /// absolute value.
943  ///
944  /// # Example
945  ///
946  /// ```
947  /// # use math_utils::NonNegative;
948  /// assert_eq!(*NonNegative::abs (1.0).map_abs (|x| -2.0 * x), 2.0);
949  /// ```
950  #[must_use]
951  pub fn map_abs <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
952    Self::abs (fun (self.0))
953  }
954}
955impl <S : Sqrt> Sqrt for NonNegative <S> {
956  fn sqrt (self) -> Self {
957    NonNegative (self.0.sqrt())
958  }
959}
960impl <S : MinMax> MinMax for NonNegative <S> {
961  fn min (self, other : Self) -> Self {
962    NonNegative (S::min (self.0, other.0))
963  }
964  fn max (self, other : Self) -> Self {
965    NonNegative (S::max (self.0, other.0))
966  }
967  fn clamp (self, min : Self, max : Self) -> Self {
968    NonNegative (self.0.clamp (min.0, max.0))
969  }
970}
971impl NonNegative <f32> {
972  /// Const constructor for `f32`
973  pub const fn new_f32 (value : f32) -> Option <Self> {
974    if 0.0 <= value {
975      Some (NonNegative (value))
976    } else {
977      None
978    }
979  }
980  /// Const constructor for `f32` with assertions
981  pub const fn noisy_f32 (value : f32) -> Self {
982    assert!(0.0 <= value);
983    NonNegative (value)
984  }
985  /// Const constructor for `f32` with debug assertions
986  pub const fn unchecked_f32 (value : f32) -> Self {
987    debug_assert!(0.0 <= value);
988    NonNegative (value)
989  }
990}
991impl NonNegative <f64> {
992  /// Const constructor for `f64`
993  pub const fn new_f64 (value : f64) -> Option <Self> {
994    if 0.0 <= value {
995      Some (NonNegative (value))
996    } else {
997      None
998    }
999  }
1000  /// Const constructor for `f64` with assertions
1001  pub const fn noisy_f64 (value : f64) -> Self {
1002    assert!(0.0 <= value);
1003    NonNegative (value)
1004  }
1005  /// Const constructor for `f64` with debug assertions
1006  pub const fn unchecked_f64 (value : f64) -> Self {
1007    debug_assert!(0.0 <= value);
1008    NonNegative (value)
1009  }
1010}
1011impl <S : Ring> From <Positive <S>> for NonNegative <S> {
1012  fn from (positive : Positive <S>) -> Self {
1013    NonNegative (*positive)
1014  }
1015}
1016impl <S : Ring> num::Zero for NonNegative <S> {
1017  fn zero() -> Self {
1018    NonNegative (S::zero())
1019  }
1020  fn is_zero (&self) -> bool {
1021    self.0.is_zero()
1022  }
1023}
1024impl <S : Field> num::One for NonNegative <S> {
1025  fn one() -> Self {
1026    NonNegative (S::one())
1027  }
1028}
1029impl <S> std::ops::Deref for NonNegative <S> {
1030  type Target = S;
1031  fn deref (&self) -> &S {
1032    &self.0
1033  }
1034}
1035impl <S : Ring> std::ops::Mul for NonNegative <S> {
1036  type Output = Self;
1037  fn mul (self, rhs : Self) -> Self::Output {
1038    NonNegative (self.0 * rhs.0)
1039  }
1040}
1041impl <S : Ring> std::ops::Mul <Positive <S>> for NonNegative <S> {
1042  type Output = Self;
1043  fn mul (self, rhs : Positive <S>) -> Self::Output {
1044    NonNegative (self.0 * rhs.0)
1045  }
1046}
1047impl <S : Field> std::ops::Div for NonNegative <S> {
1048  type Output = Self;
1049  fn div (self, rhs : Self) -> Self::Output {
1050    NonNegative (self.0 / rhs.0)
1051  }
1052}
1053impl <S : Ring> std::ops::Add for NonNegative <S> {
1054  type Output = Self;
1055  fn add (self, rhs : Self) -> Self::Output {
1056    NonNegative (self.0 + rhs.0)
1057  }
1058}
1059//  end impl NonNegative
1060
1061//
1062//  impl NonZero
1063//
1064impl <S> NonZero <S> where S : PartialEq + num::Zero {
1065  /// Returns 'None' when called with zero.
1066  ///
1067  /// # Example
1068  ///
1069  /// ```
1070  /// # use math_utils::NonZero;
1071  /// assert!(NonZero::new (2.0).is_some());
1072  /// assert!(NonZero::new (0.0).is_none());
1073  /// assert!(NonZero::new (-2.0).is_some());
1074  /// ```
1075  #[inline]
1076  pub fn new (value : S) -> Option <Self> {
1077    if S::zero() != value {
1078      Some (NonZero (value))
1079    } else {
1080      None
1081    }
1082  }
1083  /// Panics if called with zero.
1084  ///
1085  /// # Panics
1086  ///
1087  /// ```should_panic
1088  /// # use math_utils::NonZero;
1089  /// let x = NonZero::noisy (0.0);  // panic!
1090  /// ```
1091  #[inline]
1092  pub fn noisy (value : S) -> Self where S : std::fmt::Debug {
1093    assert_ne!(S::zero(), value);
1094    NonZero (value)
1095  }
1096  /// Debug panic if called with zero.
1097  ///
1098  /// # Panics
1099  ///
1100  /// ```should_panic
1101  /// # use math_utils::NonZero;
1102  /// let x = NonZero::unchecked (0.0);  // panic!
1103  /// ```
1104  #[inline]
1105  pub fn unchecked (value : S) -> Self where S : std::fmt::Debug {
1106    debug_assert_ne!(S::zero(), value);
1107    NonZero (value)
1108  }
1109  /// Map an operation on the underlying scalar, panicking if the result is zero.
1110  ///
1111  /// # Example
1112  ///
1113  /// ```
1114  /// # use math_utils::NonZero;
1115  /// # use math_utils::num::One;
1116  /// assert_eq!(*NonZero::<f64>::one().map_noisy (|x| 0.5 * x), 0.5);
1117  /// ```
1118  ///
1119  /// # Panics
1120  ///
1121  /// Panics of the result is zero.
1122  ///
1123  /// ```should_panic
1124  /// # use math_utils::NonZero;
1125  /// # use math_utils::num::One;
1126  /// let v = NonZero::<f64>::one().map_noisy (|x| 0.0 * x);  // panic!
1127  /// ```
1128  #[inline]
1129  #[must_use]
1130  pub fn map_noisy <F> (self, mut fun : F) -> Self where
1131    S : std::fmt::Debug,
1132    F : FnMut (S) -> S
1133  {
1134    Self::noisy (fun (self.0))
1135  }
1136}
1137impl NonZero <f32> {
1138  /// Const constructor for `f32`
1139  pub const fn new_f32 (value : f32) -> Option <Self> {
1140    if value != 0.0 {
1141      Some (NonZero (value))
1142    } else {
1143      None
1144    }
1145  }
1146  /// Const constructor for `f32` with assertions
1147  pub const fn noisy_f32 (value : f32) -> Self {
1148    assert!(value != 0.0);
1149    NonZero (value)
1150  }
1151  /// Const constructor for `f32` with debug assertions
1152  pub const fn unchecked_f32 (value : f32) -> Self {
1153    debug_assert!(value != 0.0);
1154    NonZero (value)
1155  }
1156}
1157impl NonZero <f64> {
1158  /// Const constructor for `f64`
1159  pub const fn new_f64 (value : f64) -> Option <Self> {
1160    if value != 0.0 {
1161      Some (NonZero (value))
1162    } else {
1163      None
1164    }
1165  }
1166  /// Const constructor for `f64` with assertions
1167  pub const fn noisy_f64 (value : f64) -> Self {
1168    assert!(value != 0.0);
1169    NonZero (value)
1170  }
1171  /// Const constructor for `f64` with debug assertions
1172  pub const fn unchecked_f64 (value : f64) -> Self {
1173    debug_assert!(value != 0.0);
1174    NonZero (value)
1175  }
1176}
1177impl <S : MultiplicativeMonoid> num::One for NonZero <S> {
1178  fn one() -> Self {
1179    NonZero (S::one())
1180  }
1181}
1182impl <S> std::ops::Deref for NonZero <S> {
1183  type Target = S;
1184  fn deref (&self) -> &S {
1185    &self.0
1186  }
1187}
1188impl <S : MultiplicativeMonoid> std::ops::Mul for NonZero <S> {
1189  type Output = Self;
1190  fn mul (self, rhs : Self) -> Self::Output {
1191    NonZero (self.0 * rhs.0)
1192  }
1193}
1194impl <S : PartialEq> Eq  for NonZero <S> { }
1195//  end impl NonZero
1196
1197//
1198//  impl Normalized
1199//
1200impl <S> Normalized <S> where S : num::One + num::Zero {
1201  // NOTE: num::Zero requires std::ops::Add which Normalized does not implement
1202  #[inline]
1203  pub fn zero() -> Self {
1204    Normalized (S::zero())
1205  }
1206  #[inline]
1207  pub fn is_zero (&self) -> bool {
1208    self.0.is_zero()
1209  }
1210  /// Returns 'None' when called with a value outside of the closed unit interval
1211  /// \[0,1\].
1212  ///
1213  /// # Example
1214  ///
1215  /// ```
1216  /// # use math_utils::Normalized;
1217  /// assert!(Normalized::new (2.0).is_none());
1218  /// assert!(Normalized::new (1.0).is_some());
1219  /// assert!(Normalized::new (0.5).is_some());
1220  /// assert!(Normalized::new (0.0).is_some());
1221  /// assert!(Normalized::new (-1.0).is_none());
1222  /// ```
1223  #[inline]
1224  pub fn new (value : S) -> Option <Self> where S : PartialOrd {
1225    if S::zero() <= value && value <= S::one() {
1226      Some (Normalized (value))
1227    } else {
1228      None
1229    }
1230  }
1231  /// Clamps to the closed unit interval \[0,1\].
1232  ///
1233  /// # Example
1234  ///
1235  /// ```
1236  /// # use math_utils::Normalized;
1237  /// assert_eq!(*Normalized::clamp (2.0), 1.0);
1238  /// assert_eq!(*Normalized::clamp (-1.0), 0.0);
1239  /// ```
1240  // TODO: can we get rid of this method and impl MinMax::clamp ?
1241  #[inline]
1242  #[expect(clippy::same_name_method)]
1243  pub fn clamp (value : S) -> Self where S : MinMax {
1244    Normalized (value.clamp (S::zero(), S::one()))
1245  }
1246  /// Panics if outside the closed unit interval \[0,1\].
1247  ///
1248  /// # Panics
1249  ///
1250  /// ```should_panic
1251  /// # use math_utils::Normalized;
1252  /// let x = Normalized::noisy (-1.0);  // panic!
1253  /// ```
1254  ///
1255  /// ```should_panic
1256  /// # use math_utils::Normalized;
1257  /// let x = Normalized::noisy (2.0);  // panic!
1258  /// ```
1259  #[inline]
1260  pub fn noisy (value : S) -> Self where S : PartialOrd {
1261    assert!(value <= S::one());
1262    assert!(S::zero() <= value);
1263    Normalized (value)
1264  }
1265  /// Create a new normalized number without checking the value.
1266  ///
1267  /// This method is completely unchecked for release builds. Debug builds will panic if
1268  /// the value is negative:
1269  ///
1270  /// ```should_panic
1271  /// # use math_utils::Normalized;
1272  /// let normalized = Normalized::unchecked (1.5);   // panic!
1273  /// ```
1274  pub fn unchecked (value : S) -> Self where S : PartialOrd {
1275    debug_assert!(value <= S::one());
1276    debug_assert!(S::zero() <= value);
1277    Normalized (value)
1278  }
1279  /// Map an operation on the underlying scalar, clamping results to the closed unit
1280  /// interval \[0,1\].
1281  ///
1282  /// # Example
1283  ///
1284  /// ```
1285  /// # use math_utils::Normalized;
1286  /// # use math_utils::num::One;
1287  /// assert_eq!(*Normalized::<f64>::one().map_clamp (|x|  2.0 * x), 1.0);
1288  /// assert_eq!(*Normalized::<f64>::one().map_clamp (|x| -2.0 * x), 0.0);
1289  /// ```
1290  #[inline]
1291  #[must_use]
1292  pub fn map_clamp <F> (self, mut fun : F) -> Self where
1293    S : MinMax,
1294    F : FnMut (S) -> S
1295  {
1296    Self::clamp (fun (self.0))
1297  }
1298  /// Map an operation on the underlying scalar, panicking if the result is outside the
1299  /// unit interval \[0,1\].
1300  ///
1301  /// # Example
1302  ///
1303  /// ```
1304  /// # use math_utils::Normalized;
1305  /// # use math_utils::num::One;
1306  /// assert_eq!(*Normalized::<f64>::one().map_noisy (|x| 0.5 * x), 0.5);
1307  /// ```
1308  ///
1309  /// # Panics
1310  ///
1311  /// Panics of the result is outside \[0,1\]:
1312  ///
1313  /// ```should_panic
1314  /// # use math_utils::Normalized;
1315  /// # use math_utils::num::One;
1316  /// let v = Normalized::<f64>::one().map_noisy (|x| -1.0 * x);  // panic!
1317  /// ```
1318  ///
1319  /// ```should_panic
1320  /// # use math_utils::Normalized;
1321  /// # use math_utils::num::One;
1322  /// let v = Normalized::<f64>::one().map_noisy (|x|  2.0 * x);  // panic!
1323  /// ```
1324  #[inline]
1325  #[must_use]
1326  pub fn map_noisy <F> (self, mut fun : F) -> Self where
1327    S : PartialOrd,
1328    F : FnMut (S) -> S
1329  {
1330    Self::noisy (fun (self.0))
1331  }
1332}
1333impl Normalized <f32> {
1334  /// Const constructor for `f32`
1335  pub const fn new_f32 (value : f32) -> Option <Self> {
1336    if 0.0 <= value && value <= 1.0 {
1337      Some (Normalized (value))
1338    } else {
1339      None
1340    }
1341  }
1342  /// Const constructor for `f32` with assertions
1343  pub const fn noisy_f32 (value : f32) -> Self {
1344    assert!(value <= 1.0);
1345    assert!(0.0 <= value);
1346    Normalized (value)
1347  }
1348  /// Const constructor for `f32` with debug assertions
1349  pub const fn unchecked_f32 (value : f32) -> Self {
1350    debug_assert!(value <= 1.0);
1351    debug_assert!(0.0 <= value);
1352    Normalized (value)
1353  }
1354}
1355impl Normalized <f64> {
1356  /// Const constructor for `f64`
1357  pub const fn new_f64 (value : f64) -> Option <Self> {
1358    if 0.0 <= value && value <= 1.0 {
1359      Some (Normalized (value))
1360    } else {
1361      None
1362    }
1363  }
1364  /// Const constructor for `f64` with assertions
1365  pub const fn noisy_f64 (value : f64) -> Self {
1366    assert!(value <= 1.0);
1367    assert!(0.0 <= value);
1368    Normalized (value)
1369  }
1370  /// Const constructor for `f64` with debug assertions
1371  pub const fn unchecked_f64 (value : f64) -> Self {
1372    debug_assert!(value <= 1.0);
1373    debug_assert!(0.0 <= value);
1374    Normalized (value)
1375  }
1376}
1377impl <S : Ring> num::One for Normalized <S> {
1378  fn one() -> Self {
1379    Normalized (S::one())
1380  }
1381}
1382impl <S> std::ops::Deref for Normalized <S> {
1383  type Target = S;
1384  fn deref (&self) -> &S {
1385    &self.0
1386  }
1387}
1388impl <S : Ring> std::ops::Mul for Normalized <S> {
1389  type Output = Self;
1390  fn mul (self, rhs : Self) -> Self::Output {
1391    Normalized (self.0 * rhs.0)
1392  }
1393}
1394impl <S : PartialEq> Eq  for Normalized <S> { }
1395#[expect(clippy::derive_ord_xor_partial_ord)]
1396impl <S : PartialOrd> Ord for Normalized <S> {
1397  fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1398    // safe to unwrap: normalized values are never NaN
1399    self.partial_cmp (other).unwrap()
1400  }
1401}
1402//  end impl Normalized
1403
1404//
1405//  impl NormalSigned
1406//
1407impl <S> NormalSigned <S> {
1408  // NOTE: num::Zero requires std::ops::Add which Normalized does not implement
1409  #[inline]
1410  pub fn zero() -> Self where S : num::Zero {
1411    NormalSigned (S::zero())
1412  }
1413  #[inline]
1414  pub fn is_zero (&self) -> bool where S : num::Zero {
1415    self.0.is_zero()
1416  }
1417  /// Returns 'None' when called with a value outside of the closed interval \[-1,1\].
1418  ///
1419  /// # Example
1420  ///
1421  /// ```
1422  /// # use math_utils::NormalSigned;
1423  /// assert!(NormalSigned::new (2.0).is_none());
1424  /// assert!(NormalSigned::new (1.0).is_some());
1425  /// assert!(NormalSigned::new (0.5).is_some());
1426  /// assert!(NormalSigned::new (0.0).is_some());
1427  /// assert!(NormalSigned::new (-1.0).is_some());
1428  /// assert!(NormalSigned::new (-2.0).is_none());
1429  /// ```
1430  #[inline]
1431  pub fn new (value : S) -> Option <Self> where S : OrderedRing {
1432    if -S::one() <= value && value <= S::one() {
1433      Some (NormalSigned (value))
1434    } else {
1435      None
1436    }
1437  }
1438  /// Clamps to the closed interval [-1,1].
1439  ///
1440  /// # Example
1441  ///
1442  /// ```
1443  /// # use math_utils::NormalSigned;
1444  /// assert_eq!(*NormalSigned::clamp (2.0), 1.0);
1445  /// assert_eq!(*NormalSigned::clamp (-2.0), -1.0);
1446  /// ```
1447  // TODO: can we get rid of this method and impl MinMax::clamp ?
1448  #[inline]
1449  #[expect(clippy::same_name_method)]
1450  pub fn clamp (value : S) -> Self where S : Ring + MinMax {
1451    NormalSigned (value.clamp (-S::one(), S::one()))
1452  }
1453  /// Panics if outside the closed interval [-1,1].
1454  ///
1455  /// # Panics
1456  ///
1457  /// ```should_panic
1458  /// # use math_utils::NormalSigned;
1459  /// let x = NormalSigned::noisy (-2.0);   // panic!
1460  /// ```
1461  ///
1462  /// ```should_panic
1463  /// # use math_utils::NormalSigned;
1464  /// let x = NormalSigned::noisy (2.0);    // panic!
1465  /// ```
1466  #[inline]
1467  pub fn noisy (value : S) -> Self where S : OrderedRing {
1468    assert!(value <= S::one());
1469    assert!(-S::one() <= value);
1470    NormalSigned (value)
1471  }
1472  /// Map an operation on the underlying scalar, clamping results to the closed interval
1473  /// [-1,1].
1474  ///
1475  /// # Example
1476  ///
1477  /// ```
1478  /// # use math_utils::NormalSigned;
1479  /// # use math_utils::num::One;
1480  /// assert_eq!(*NormalSigned::<f64>::one().map_clamp (|x|  2.0 * x),  1.0);
1481  /// assert_eq!(*NormalSigned::<f64>::one().map_clamp (|x| -2.0 * x), -1.0);
1482  /// ```
1483  #[inline]
1484  #[must_use]
1485  pub fn map_clamp <F> (self, mut fun : F) -> Self where
1486    S : Ring + MinMax,
1487    F : FnMut (S) -> S
1488  {
1489    Self::clamp (fun (self.0))
1490  }
1491  /// Map an operation on the underlying scalar, panicking if the result is outside the
1492  /// interval [-1, 1].
1493  ///
1494  /// # Example
1495  ///
1496  /// ```
1497  /// # use math_utils::NormalSigned;
1498  /// # use math_utils::num::One;
1499  /// assert_eq!(*NormalSigned::<f64>::one().map_noisy (|x| 0.5 * x), 0.5);
1500  /// ```
1501  ///
1502  /// # Panics
1503  ///
1504  /// Panics of the result is outside [-1, 1]:
1505  ///
1506  /// ```should_panic
1507  /// # use math_utils::NormalSigned;
1508  /// # use math_utils::num::One;
1509  /// let v = NormalSigned::<f64>::one().map_noisy (|x| -2.0 * x);  // panic!
1510  /// ```
1511  ///
1512  /// ```should_panic
1513  /// # use math_utils::NormalSigned;
1514  /// # use math_utils::num::One;
1515  /// let v = NormalSigned::<f64>::one().map_noisy (|x|  2.0 * x);  // panic!
1516  /// ```
1517  #[inline]
1518  #[must_use]
1519  pub fn map_noisy <F> (self, mut fun : F) -> Self where
1520    S : OrderedRing,
1521    F : FnMut (S) -> S
1522  {
1523    Self::noisy (fun (self.0))
1524  }
1525}
1526impl NormalSigned <f32> {
1527  /// Const constructor for `f32`
1528  pub const fn new_f32 (value : f32) -> Option <Self> {
1529    if -1.0 <= value && value <= 1.0 {
1530      Some (NormalSigned (value))
1531    } else {
1532      None
1533    }
1534  }
1535  /// Const constructor for `f32` with assertions
1536  pub const fn noisy_f32 (value : f32) -> Self {
1537    assert!(value <= 1.0);
1538    assert!(-1.0 <= value);
1539    NormalSigned (value)
1540  }
1541  /// Const constructor for `f32` with debug assertions
1542  pub const fn unchecked_f32 (value : f32) -> Self {
1543    debug_assert!(value <= 1.0);
1544    debug_assert!(-1.0 <= value);
1545    NormalSigned (value)
1546  }
1547}
1548impl NormalSigned <f64> {
1549  /// Const constructor for `f64`
1550  pub const fn new_f64 (value : f64) -> Option <Self> {
1551    if -1.0 <= value && value <= 1.0 {
1552      Some (NormalSigned (value))
1553    } else {
1554      None
1555    }
1556  }
1557  /// Const constructor for `f64` with assertions
1558  pub const fn noisy_f64 (value : f64) -> Self {
1559    assert!(value <= 1.0);
1560    assert!(-1.0 <= value);
1561    NormalSigned (value)
1562  }
1563  /// Const constructor for `f64` with debug assertions
1564  pub const fn unchecked_f64 (value : f64) -> Self {
1565    debug_assert!(value <= 1.0);
1566    debug_assert!(-1.0 <= value);
1567    NormalSigned (value)
1568  }
1569}
1570impl <S : OrderedField> From <Normalized <S>> for NormalSigned <S> {
1571  fn from (Normalized (value) : Normalized <S>) -> Self {
1572    debug_assert!(S::zero() <= value);
1573    debug_assert!(value <= S::one());
1574    NormalSigned (value)
1575  }
1576}
1577impl <S : Field> num::One for NormalSigned <S> {
1578  fn one() -> Self {
1579    NormalSigned (S::one())
1580  }
1581}
1582impl <S> std::ops::Deref for NormalSigned <S> {
1583  type Target = S;
1584  fn deref (&self) -> &S {
1585    &self.0
1586  }
1587}
1588impl <S : Field> std::ops::Mul for NormalSigned <S> {
1589  type Output = Self;
1590  fn mul (self, rhs : Self) -> Self::Output {
1591    NormalSigned (self.0 * rhs.0)
1592  }
1593}
1594impl <S : Field> Eq  for NormalSigned <S> { }
1595#[expect(clippy::derive_ord_xor_partial_ord)]
1596impl <S : OrderedField> Ord for NormalSigned <S> {
1597  fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1598    // safe to unwrap: normalized values are never NaN
1599    self.partial_cmp (other).unwrap()
1600  }
1601}
1602impl <S : Field> std::ops::Neg for NormalSigned <S> {
1603  type Output = Self;
1604  fn neg (self) -> Self {
1605    NormalSigned (-self.0)
1606  }
1607}
1608//  end impl NormalSigned
1609
1610//
1611//  impl Vector2
1612//
1613impl <S : Ring> Vector2Ext <S> for Vector2 <S> {
1614  /// Exterior or wedge product: $(a,b) \wedge (c,d) = ad - bc$
1615  fn exterior_product (self, rhs : Vector2 <S>) -> S {
1616    self.x * rhs.y - self.y * rhs.x
1617  }
1618}
1619fn ortho_vec2 <S> (vector : Vector2 <S>) -> Vector2 <S> {
1620  vector.yx()
1621}
1622
1623//
1624//  impl Vector3
1625//
1626fn ortho_vec3 <S> (vector : Vector3 <S>) -> Vector3 <S> where
1627  S : Ring + approx::AbsDiffEq <Epsilon = S>
1628{
1629  // NOTE: for floating-point numbers, an algorithm using copysign may be 5-10% faster,
1630  // but copysign is only available for floating-point numbers
1631  // (https://math.stackexchange.com/a/4112622)
1632  let mut ortho = vector.cross (Vector3::unit_x());
1633  if approx::abs_diff_eq!(ortho, Vector3::zero()) {
1634    ortho = vector.cross (Vector3::unit_y())
1635  }
1636  ortho
1637}
1638
1639//
1640//  impl Vector4
1641//
1642fn ortho_vec4 <S> (_vector : Vector4 <S>) -> Vector4 <S> {
1643  unimplemented!("TODO")
1644}
1645
1646//
1647//  impl Unit
1648//
1649impl <S> std::ops::Deref for Unit <S> {
1650  type Target = S;
1651  fn deref (&self) -> &S {
1652    &self.0
1653  }
1654}
1655
1656//
1657//  impl Rotation2
1658//
1659impl <S> Rotation2 <S> where S : Ring + MaybeSerDes {
1660  /// Identity rotation
1661  pub fn identity() -> Self {
1662    use num::One;
1663    Self::one()
1664  }
1665  /// Returns `None` if called with a matrix that is not orthonormal with determinant
1666  /// +1.
1667  ///
1668  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1669  pub fn new (mat : Matrix2 <S>) -> Option <Self> {
1670    let transform = LinearAuto (LinearIso::new (mat)?);
1671    if !transform.is_rotation() {
1672      None
1673    } else {
1674      Some (Rotation2 (transform))
1675    }
1676  }
1677  /// Returns `None` if called with a matrix that is not orthonormal with determinant
1678  /// +1.
1679  ///
1680  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1681  /// (approximately using relative equality).
1682  pub fn new_approx (mat : Matrix2 <S>) -> Option <Self> where
1683    S : approx::RelativeEq <Epsilon=S>
1684  {
1685    let four         = S::one() + S::one() + S::one() + S::one();
1686    let thirtytwo    = four * four * (S::one() + S::one());
1687    let epsilon      = S::default_epsilon() * thirtytwo;
1688    let max_relative = S::default_max_relative() * four;
1689    if approx::relative_ne!(mat * mat.transposed(), Matrix2::identity(),
1690      max_relative=max_relative, epsilon=epsilon
1691    ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
1692      None
1693    } else {
1694      Some (Rotation2 (LinearAuto (LinearIso (mat, PhantomData))))
1695    }
1696  }
1697  /// Create a rotation for the given angle
1698  pub fn from_angle (angle : Rad <S>) -> Self where S : num::real::Real {
1699    Rotation2 (LinearAuto (LinearIso (Matrix2::rotation_z (angle.0), PhantomData)))
1700  }
1701  /// Panic if the given matrix is not orthogonal with determinant +1.
1702  ///
1703  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1704  pub fn noisy (mat : Matrix2 <S>) -> Self where S : std::fmt::Debug {
1705    assert_eq!(mat * mat.transposed(), Matrix2::identity());
1706    assert_eq!(mat.determinant(), S::one());
1707    Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1708  }
1709  /// Panic if the given matrix is not orthogonal with determinant +1.
1710  ///
1711  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1712  /// (approximately using relative equality).
1713  pub fn noisy_approx (mat : Matrix2 <S>) -> Self where
1714    S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1715  {
1716    let four         = S::one() + S::one() + S::one() + S::one();
1717    let epsilon      = S::default_epsilon() * four;
1718    let max_relative = S::default_max_relative() * four;
1719    approx::assert_relative_eq!(mat * mat.transposed(), Matrix2::identity(),
1720      epsilon=epsilon, max_relative=max_relative);
1721    approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
1722    Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1723  }
1724  /// It is a debug panic if the given matrix is not orthogonal with determinant +1.
1725  ///
1726  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1727  pub fn unchecked (mat : Matrix2 <S>) -> Self where S : std::fmt::Debug {
1728    debug_assert!(mat * mat.transposed() == Matrix2::identity());
1729    debug_assert!(mat.determinant() == S::one());
1730    Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1731  }
1732  /// It is a debug panic if the given matrix is not orthogonal with determinant +1.
1733  ///
1734  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1735  /// (approximately using relative equality).
1736  pub fn unchecked_approx (mat : Matrix2 <S>) -> Self where
1737    S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1738  {
1739    if cfg!(debug_assertions) {
1740      let four         = S::one() + S::one() + S::one() + S::one();
1741      let epsilon      = S::default_epsilon() * four;
1742      let max_relative = S::default_max_relative() * four;
1743      approx::assert_relative_eq!(mat * mat.transposed(), Matrix2::identity(),
1744        max_relative=max_relative, epsilon=epsilon);
1745      approx::assert_relative_eq!(mat.determinant(), S::one(),
1746        max_relative=max_relative);
1747    }
1748    Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1749  }
1750  /// Get the underlying matrix
1751  pub const fn mat (self) -> Matrix2 <S> {
1752    self.0.0.0
1753  }
1754  /// Rotates a point around the origin
1755  pub fn rotate (self, point : Point2 <S>) -> Point2 <S> {
1756    Point2::from (self * point.0)
1757  }
1758  /// Rotate around a center point
1759  pub fn rotate_around (self, point : Point2 <S>, center : Point2 <S>) -> Point2 <S> {
1760    self.rotate (point - center.0) + center.0
1761  }
1762}
1763impl <S> std::ops::Deref for Rotation2 <S> where S : Ring + MaybeSerDes {
1764  type Target = LinearAuto <S, Vector2 <S>>;
1765  fn deref (&self) -> &Self::Target {
1766    &self.0
1767  }
1768}
1769impl <S> num::Inv for Rotation2 <S> where S : Ring + MaybeSerDes {
1770  type Output = Self;
1771  fn inv (self) -> Self::Output {
1772    Rotation2 (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
1773  }
1774}
1775impl <S> std::ops::Mul <Vector2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1776  type Output = Vector2 <S>;
1777  fn mul (self, rhs : Vector2 <S>) -> Self::Output {
1778    self.0 * rhs
1779  }
1780}
1781impl <S> std::ops::Mul <NonZero2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1782  type Output = NonZero2 <S>;
1783  fn mul (self, rhs : NonZero2 <S>) -> Self::Output {
1784    NonZero2 (self.0 * rhs.0)
1785  }
1786}
1787impl <S> std::ops::Mul <Unit2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1788  type Output = Unit2 <S>;
1789  fn mul (self, rhs : Unit2 <S>) -> Self::Output {
1790    Unit2 (self.0 * rhs.0)
1791  }
1792}
1793impl <S> std::ops::Mul <Rotation2 <S>> for Vector2 <S> where S : Ring + MaybeSerDes {
1794  type Output = Vector2 <S>;
1795  fn mul (self, rhs : Rotation2 <S>) -> Self::Output {
1796    self * rhs.0
1797  }
1798}
1799impl <S> std::ops::Mul for Rotation2 <S> where S : Ring + MaybeSerDes {
1800  type Output = Self;
1801  fn mul (self, rhs : Self) -> Self::Output {
1802    Rotation2 (self.0 * rhs.0)
1803  }
1804}
1805impl <S> std::ops::MulAssign <Self> for Rotation2 <S> where S : Ring + MaybeSerDes {
1806  fn mul_assign (&mut self, rhs : Self) {
1807    self.0 *= rhs.0
1808  }
1809}
1810impl <S> std::ops::Div for Rotation2 <S> where S : Ring + MaybeSerDes {
1811  type Output = Self;
1812  #[expect(clippy::suspicious_arithmetic_impl)]
1813  fn div (self, rhs : Self) -> Self::Output {
1814    use num::Inv;
1815    self * rhs.inv()
1816  }
1817}
1818impl <S> std::ops::DivAssign <Self> for Rotation2 <S> where S : Ring + MaybeSerDes {
1819  #[expect(clippy::suspicious_op_assign_impl)]
1820  fn div_assign (&mut self, rhs : Self) {
1821    use num::Inv;
1822    *self *= rhs.inv()
1823  }
1824}
1825impl <S> num::One for Rotation2 <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
1826  fn one() -> Self {
1827    Rotation2 (LinearAuto (LinearIso (Matrix2::identity(), PhantomData)))
1828  }
1829}
1830
1831//
1832//  impl Rotation3
1833//
1834impl <S> Rotation3 <S> where S : Ring + MaybeSerDes {
1835  /// Identity rotation
1836  pub fn identity() -> Self {
1837    use num::One;
1838    Self::one()
1839  }
1840  /// Returns `None` if called with a matrix that is not orthonormal with determinant
1841  /// +1.
1842  ///
1843  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1844  pub fn new (mat : Matrix3 <S>) -> Option <Self> {
1845    if mat * mat.transposed() != Matrix3::identity() || mat.determinant() != S::one() {
1846      None
1847    } else {
1848      Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1849    }
1850  }
1851  /// Returns `None` if called with a matrix that is not orthonormal with determinant
1852  /// +1.
1853  ///
1854  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1855  /// (approximately using relative equality).
1856  pub fn new_approx (mat : Matrix3 <S>) -> Option <Self> where
1857    S : approx::RelativeEq <Epsilon=S>
1858  {
1859    let four         = S::one() + S::one() + S::one() + S::one();
1860    let thirtytwo    = four * four * (S::one() + S::one());
1861    let epsilon      = S::default_epsilon() * thirtytwo;
1862    let max_relative = S::default_max_relative() * four;
1863    if approx::relative_ne!(mat * mat.transposed(), Matrix3::identity(),
1864      max_relative=max_relative, epsilon=epsilon
1865    ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
1866      None
1867    } else {
1868      Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1869    }
1870  }
1871  /// Panic if the given matrix is not orthogonal with determinant +1.
1872  ///
1873  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1874  pub fn noisy (mat : Matrix3 <S>) -> Self {
1875    assert!(mat * mat.transposed() == Matrix3::identity());
1876    assert!(mat.determinant() == S::one());
1877    Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1878  }
1879  /// Panic if the given matrix is not orthogonal with determinant +1.
1880  ///
1881  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1882  /// (approximately using relative equality).
1883  pub fn noisy_approx (mat : Matrix3 <S>) -> Self where
1884    S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1885  {
1886    let four         = S::one() + S::one() + S::one() + S::one();
1887    let eight        = four + S::one() + S::one() + S::one() + S::one();
1888    let epsilon      = S::default_epsilon() * four;
1889    let max_relative = S::default_max_relative() * eight;
1890    approx::assert_relative_eq!(mat * mat.transposed(), Matrix3::identity(),
1891      max_relative=max_relative, epsilon=epsilon);
1892    approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
1893    Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1894  }
1895  /// It is a debug panic if the given matrix is not orthogonal with determinant +1.
1896  ///
1897  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
1898  pub fn unchecked (mat : Matrix3 <S>) -> Self where S : std::fmt::Debug {
1899    debug_assert!(mat * mat.transposed() == Matrix3::identity());
1900    debug_assert!(mat.determinant() == S::one());
1901    Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1902  }
1903  /// It is a debug panic if the given matrix is not orthogonal with determinant +1.
1904  ///
1905  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
1906  /// (approximately using relative equality).
1907  pub fn unchecked_approx (mat : Matrix3 <S>) -> Self where
1908    S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1909  {
1910    if cfg!(debug_assertions) {
1911      let four         = S::one() + S::one() + S::one() + S::one();
1912      let epsilon      = S::default_epsilon() * four;
1913      let max_relative = S::default_max_relative() * four;
1914      approx::assert_relative_eq!(mat * mat.transposed(), Matrix3::identity(),
1915        max_relative=max_relative, epsilon=epsilon);
1916      approx::assert_relative_eq!(mat.determinant(), S::one(),
1917        max_relative=max_relative);
1918    }
1919    Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1920  }
1921  /// Construct a rotation around the X axis.
1922  ///
1923  /// Positive angles are counter-clockwise.
1924  pub fn from_angle_x (angle : Rad <S>) -> Self where S : num::real::Real {
1925    Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_x (angle.0), PhantomData)))
1926  }
1927  /// Construct a rotation around the Y axis
1928  ///
1929  /// Positive angles are counter-clockwise.
1930  pub fn from_angle_y (angle : Rad <S>) -> Self where S : num::real::Real {
1931    Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_y (angle.0), PhantomData)))
1932  }
1933  /// Construct a rotation around the Z axis
1934  ///
1935  /// Positive angles are counter-clockwise.
1936  pub fn from_angle_z (angle : Rad <S>) -> Self where S : num::real::Real {
1937    Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_z (angle.0), PhantomData)))
1938  }
1939  /// Construct a rotation from intrinsic ZX'Y'' Euler angles:
1940  ///
1941  /// - yaw: rotation around Z axis
1942  /// - pitch: rotation around X' axis
1943  /// - roll: rotation around Y'' axis
1944  pub fn from_angles_intrinsic (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>)
1945    -> Self where S : num::real::Real
1946  {
1947    let rotation1 = Rotation3::from_angle_z (yaw);
1948    let rotation2 = Rotation3::from_axis_angle (rotation1.cols.x, pitch) * rotation1;
1949    Rotation3::from_axis_angle (rotation2.cols.y, roll) * rotation2
1950  }
1951  /// Construct a rotation around the given axis vector with the given angle
1952  pub fn from_axis_angle (axis : Vector3 <S>, angle : Rad <S>) -> Self where
1953    S : num::real::Real
1954  {
1955    Rotation3 (LinearAuto (LinearIso (
1956      Matrix3::rotation_3d (angle.0, axis), PhantomData)))
1957  }
1958  /// Construct an orthonormal matrix from a set of linearly-independent vectors using
1959  /// the Gram-Schmidt process
1960  pub fn orthonormalize (v1 : Vector3 <S>, v2 : Vector3 <S>, v3 : Vector3 <S>)
1961    -> Option <Self> where S : OrderedField + Sqrt
1962  {
1963    if Matrix3::from_col_arrays ([v1, v2, v3].map (Vector3::into_array)).determinant()
1964      == S::zero()
1965    {
1966      None
1967    } else {
1968      let project = |u : Vector3 <S>, v : Vector3 <S>| u * (u.dot (v) / *u.self_dot());
1969      let u1 = v1;
1970      let u2 = v2 - project (u1, v2);
1971      let u3 = v3 - project (u1, v3) - project (u2, v3);
1972      Some (Rotation3 (LinearAuto (LinearIso (Matrix3::from_col_arrays ([
1973        u1.normalize().into_array(),
1974        u2.normalize().into_array(),
1975        u3.normalize().into_array()
1976      ]), PhantomData))))
1977    }
1978  }
1979
1980  /// Returns a rotation with zero roll and oriented towards the target point.
1981  ///
1982  /// Returns the identity rotation if `point` is the origin.
1983  pub fn look_at (point : Point3 <S>) -> Self where
1984    S : num::Float + num::FloatConst + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1985  {
1986    if point == Point::origin() {
1987      Self::identity()
1988    } else {
1989      use num::Zero;
1990      let forward = point.0.normalized();
1991      let right = {
1992        let projected = forward.with_z (S::zero());
1993        if !projected.is_zero() {
1994          Self::from_angle_z (-Rad (S::FRAC_PI_2())) * projected.normalized()
1995        } else {
1996          Vector3::unit_x()
1997        }
1998      };
1999      let up = right.cross (forward);
2000      let mat =
2001        Matrix3::from_col_arrays ([right, forward, up].map (Vector3::into_array));
2002      Self::noisy_approx (mat)
2003    }
2004  }
2005
2006  /// Return intrinsic yaw (Z), pitch (X'), roll (Y'') angles.
2007  ///
2008  /// NOTE: this function returns the raw output of the `atan2` operations used to
2009  /// compute the angles. This function returns values in the range `[-\pi, \pi]`.
2010  pub fn intrinsic_angles (&self) -> (Rad <S>, Rad <S>, Rad <S>) where S : Real {
2011    let cols  = &self.cols;
2012    let yaw   = Rad (S::atan2 (-cols.y.x, cols.y.y));
2013    let pitch = Rad (S::atan2 (cols.y.z, S::sqrt (S::one() - cols.y.z * cols.y.z)));
2014    let roll  = Rad (S::atan2 (-cols.x.z, cols.z.z));
2015    (yaw, pitch, roll)
2016  }
2017
2018  /// Convert to versor (unit quaternion) representation.
2019  ///
2020  /// <https://stackoverflow.com/a/63745757>
2021  pub fn versor (self) -> Versor <S> where
2022    S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
2023  {
2024    let diagonal = self.diagonal();
2025    let t = diagonal.sum();
2026    let m =
2027      MinMax::max (MinMax::max (MinMax::max (diagonal.x, diagonal.y), diagonal.z), t);
2028    let qmax  = S::half() * Sqrt::sqrt (S::one() - t + S::two() * m);
2029    let qmax4_recip = (S::four() * qmax).recip();
2030    let [qx, qy, qz, qw] : [S; 4];
2031    let cols = self.cols;
2032    if m == diagonal.x {
2033      qx = qmax;
2034      qy = qmax4_recip * (cols.x.y + cols.y.x);
2035      qz = qmax4_recip * (cols.x.z + cols.z.x);
2036      qw = -qmax4_recip * (cols.z.y - cols.y.z);
2037    } else if m == diagonal.y {
2038      qx = qmax4_recip * (cols.x.y + cols.y.x);
2039      qy = qmax;
2040      qz = qmax4_recip * (cols.y.z + cols.z.y);
2041      qw = -qmax4_recip * (cols.x.z - cols.z.x);
2042    } else if m == diagonal.z {
2043      qx = qmax4_recip * (cols.x.z + cols.z.x);
2044      qy = qmax4_recip * (cols.y.z + cols.z.y);
2045      qz = qmax;
2046      qw = -qmax4_recip * (cols.y.x - cols.x.y);
2047    } else {
2048      qx = -qmax4_recip * (cols.z.y - cols.y.z);
2049      qy = -qmax4_recip * (cols.x.z - cols.z.x);
2050      qz = -qmax4_recip * (cols.y.x - cols.x.y);
2051      qw = qmax;
2052    }
2053    let quat = Quaternion::from_xyzw (qx, qy, qz, qw);
2054    Versor::unchecked_approx (quat)
2055  }
2056  /// Get the underlying matrix
2057  pub const fn mat (self) -> Matrix3 <S> {
2058    self.0.0.0
2059  }
2060  /// Rotates a point around the origin
2061  pub fn rotate (self, point : Point3 <S>) -> Point3 <S> {
2062    Point3::from (self * point.0)
2063  }
2064  /// Rotate around a center point
2065  pub fn rotate_around (self, point : Point3 <S>, center : Point3 <S>) -> Point3 <S> {
2066    self.rotate (point - center.0) + center.0
2067  }
2068}
2069impl <S> std::ops::Deref for Rotation3 <S> where S : Ring + MaybeSerDes {
2070  type Target = LinearAuto <S, Vector3 <S>>;
2071  fn deref (&self) -> &Self::Target {
2072    &self.0
2073  }
2074}
2075impl <S> num::Inv for Rotation3 <S> where S : Ring + MaybeSerDes {
2076  type Output = Self;
2077  fn inv (self) -> Self::Output {
2078    Rotation3 (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
2079  }
2080}
2081impl <S> std::ops::Mul <Vector3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2082  type Output = Vector3 <S>;
2083  fn mul (self, rhs : Vector3 <S>) -> Self::Output {
2084    self.0 * rhs
2085  }
2086}
2087impl <S> std::ops::Mul <NonZero3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2088  type Output = NonZero3 <S>;
2089  fn mul (self, rhs : NonZero3 <S>) -> Self::Output {
2090    NonZero3 (self.0 * rhs.0)
2091  }
2092}
2093impl <S> std::ops::Mul <Unit3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2094  type Output = Unit3 <S>;
2095  fn mul (self, rhs : Unit3 <S>) -> Self::Output {
2096    Unit3 (self.0 * rhs.0)
2097  }
2098}
2099impl <S> std::ops::Mul <Rotation3 <S>> for Vector3 <S> where S : Ring + MaybeSerDes {
2100  type Output = Vector3 <S>;
2101  fn mul (self, rhs : Rotation3 <S>) -> Self::Output {
2102    self * rhs.0
2103  }
2104}
2105impl <S> std::ops::Mul for Rotation3 <S> where S : Ring + MaybeSerDes {
2106  type Output = Self;
2107  fn mul (self, rhs : Self) -> Self::Output {
2108    Rotation3 (self.0 * rhs.0)
2109  }
2110}
2111impl <S> std::ops::MulAssign <Self> for Rotation3 <S> where S : Ring + MaybeSerDes {
2112  fn mul_assign (&mut self, rhs : Self) {
2113    self.0 *= rhs.0
2114  }
2115}
2116impl <S> std::ops::Div for Rotation3 <S> where S : Ring + MaybeSerDes {
2117  type Output = Self;
2118  #[expect(clippy::suspicious_arithmetic_impl)]
2119  fn div (self, rhs : Self) -> Self::Output {
2120    use num::Inv;
2121    self * rhs.inv()
2122  }
2123}
2124impl <S> std::ops::DivAssign <Self> for Rotation3 <S> where S : Ring + MaybeSerDes {
2125  #[expect(clippy::suspicious_op_assign_impl)]
2126  fn div_assign (&mut self, rhs : Self) {
2127    use num::Inv;
2128    *self *= rhs.inv()
2129  }
2130}
2131impl <S> num::One for Rotation3 <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
2132  fn one() -> Self {
2133    Rotation3 (LinearAuto (LinearIso (Matrix3::identity(), PhantomData)))
2134  }
2135}
2136impl <S> From <Angles3 <S>> for Rotation3 <S> where
2137  S : Real + num::real::Real + MaybeSerDes
2138{
2139  fn from (angles : Angles3 <S>) -> Self {
2140    Rotation3::from_angles_intrinsic (
2141      angles.yaw.angle(), angles.pitch.angle(), angles.roll.angle())
2142  }
2143}
2144
2145//
2146//  impl Versor
2147//
2148impl <S : Real> Versor <S> {
2149  /// Normalizes the given quaternion.
2150  ///
2151  /// Panics if the zero quaternion is given.
2152  pub fn normalize (quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
2153    assert_ne!(quaternion, Quaternion::zero());
2154    Versor (normalize_quaternion (quaternion))
2155  }
2156  /// Panic if the given quaternion is not a unit quaternion.
2157  ///
2158  /// This method checks whether `quaternion == quaternion.normalized()`.
2159  pub fn noisy (unit_quaternion : Quaternion <S>) -> Self where
2160    S : num::real::Real + std::fmt::Debug
2161  {
2162    assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
2163    Versor (unit_quaternion)
2164  }
2165  /// Panic if the given quaternion is not a unit quaternion.
2166  ///
2167  /// Checks if the magnitude is approximately one.
2168  pub fn noisy_approx (unit_quaternion : Quaternion <S>) -> Self where
2169    S : num::real::Real + approx::RelativeEq <Epsilon=S>
2170  {
2171    assert!(unit_quaternion.into_vec4().is_normalized());
2172    Versor (unit_quaternion)
2173  }
2174  /// It is a debug panic if the given quaternion is not a unit quaternion.
2175  ///
2176  /// This method checks whether `quaternion == quaternion.normalized()`.
2177  pub fn unchecked (unit_quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
2178    debug_assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
2179    Versor (unit_quaternion)
2180  }
2181  /// It is a debug panic if the given quaternion is not a unit quaternion.
2182  ///
2183  /// Checks if the magnitude is approximately one.
2184  pub fn unchecked_approx (unit_quaternion : Quaternion <S>) -> Self where
2185    S : num::real::Real + approx::RelativeEq <Epsilon=S>
2186  {
2187    debug_assert!(unit_quaternion.into_vec4().is_normalized());
2188    Versor (unit_quaternion)
2189  }
2190  /// Constructs the rotation using the direction and magnitude of the given vector
2191  // TODO: work around rotation_3d Float constraint
2192  pub fn from_scaled_axis (scaled_axis : Vector3 <S>) -> Self where
2193    S : std::fmt::Debug + num::Float
2194  {
2195    if scaled_axis == Vector3::zero() {
2196      let versor = Quaternion::rotation_3d (S::zero(), scaled_axis);
2197      debug_assert_eq!(versor.magnitude(), S::one());
2198      Versor (versor)
2199    } else {
2200      let angle = scaled_axis.magnitude();
2201      debug_assert!(S::zero() < angle);
2202      let axis  = scaled_axis / angle;
2203      Versor (Quaternion::rotation_3d (angle, axis))
2204    }
2205  }
2206}
2207impl <S> Default for Versor <S> where S : num::One + num::Zero {
2208  fn default() -> Self {
2209    Versor (Quaternion::default())
2210  }
2211}
2212impl <S> std::ops::Mul <Vector3 <S>> for Versor <S> where S : Real + num::Float {
2213  type Output = Vector3 <S>;
2214  fn mul (self, rhs : Vector3 <S>) -> Self::Output {
2215    self.0 * rhs
2216  }
2217}
2218impl <S> From <Rotation3 <S>> for Versor <S> where
2219  S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
2220{
2221  fn from (rot : Rotation3 <S>) -> Self {
2222    rot.versor()
2223  }
2224}
2225impl <S> std::ops::Deref for Versor <S> {
2226  type Target = Quaternion <S>;
2227  fn deref (&self) -> &Quaternion <S> {
2228    &self.0
2229  }
2230}
2231//  end impl Versor
2232
2233//
2234//  impl LinearIso
2235//
2236impl <S, V, W, M> LinearIso <S, V, W, M> where
2237  M : LinearMap <S, V, W> + Copy,
2238  V : Module <S>,
2239  W : Module <S>,
2240  S : Ring
2241{
2242  pub fn mat (self) -> M {
2243    *self
2244  }
2245  /// Checks whether the determinant is non-zero
2246  pub fn is_invertible (linear_map : M) -> bool {
2247    linear_map.determinant() != S::zero()
2248  }
2249  /// Checks whether the determinant is non-zero with approximate equality
2250  pub fn is_invertible_approx (linear_map : M, epsilon : Option <S>) -> bool where
2251    S : approx::AbsDiffEq <Epsilon=S>
2252  {
2253    let epsilon = epsilon.unwrap_or_else (||{
2254      let four = S::one() + S::one() + S::one() + S::one();
2255      S::default_epsilon() * four
2256    });
2257    approx::abs_diff_ne!(linear_map.determinant(), S::zero(), epsilon=epsilon)
2258  }
2259  /// Checks if map is invertible
2260  pub fn new (linear_map : M) -> Option <Self> {
2261    if Self::is_invertible (linear_map) {
2262      Some (LinearIso (linear_map, PhantomData))
2263    } else {
2264      None
2265    }
2266  }
2267  /// Checks if map is invertible with approximate equality
2268  pub fn new_approx (linear_map : M, epsilon : Option <S>) -> Option <Self> where
2269    S : approx::RelativeEq <Epsilon=S>
2270  {
2271    if Self::is_invertible_approx (linear_map, epsilon) {
2272      Some (LinearIso (linear_map, PhantomData))
2273    } else {
2274      None
2275    }
2276  }
2277}
2278impl <S, V, W, M> std::ops::Deref for LinearIso <S, V, W, M> where
2279  M : LinearMap <S, V, W>,
2280  V : Module <S>,
2281  W : Module <S>,
2282  S : Ring
2283{
2284  type Target = M;
2285  fn deref (&self) -> &Self::Target {
2286    &self.0
2287  }
2288}
2289impl <S, V> num::One for LinearIso <S, V, V, V::LinearEndo> where
2290  V : Module <S>,
2291  S : Ring
2292{
2293  fn one () -> Self {
2294    LinearIso (V::LinearEndo::one(), PhantomData)
2295  }
2296}
2297impl <S, V, W, M> std::ops::Mul <V> for LinearIso <S, V, W, M> where
2298  M : LinearMap <S, V, W>,
2299  V : Module <S>,
2300  W : Module <S>,
2301  S : Ring
2302{
2303  type Output = W;
2304  fn mul (self, rhs : V) -> Self::Output {
2305    self.0 * rhs
2306  }
2307}
2308impl <S, V> std::ops::Mul for LinearIso <S, V, V, V::LinearEndo> where
2309  V : Module <S>,
2310  S : Ring
2311{
2312  type Output = Self;
2313  fn mul (self, rhs : Self) -> Self::Output {
2314    LinearIso (self.0 * rhs.0, PhantomData)
2315  }
2316}
2317impl <S, V> std::ops::MulAssign for LinearIso <S, V, V, V::LinearEndo> where
2318  V : Module <S>,
2319  S : Ring
2320{
2321  fn mul_assign (&mut self, rhs : Self) {
2322    self.0 *= rhs.0
2323  }
2324}
2325
2326//
2327//  impl LinearAuto
2328//
2329impl <S, V> LinearAuto <S, V> where
2330  V : Module <S>,
2331  V::LinearEndo : MaybeSerDes,
2332  S : Ring
2333{
2334  pub fn mat (self) -> V::LinearEndo {
2335    **self
2336  }
2337  /// Returns false if called with a matrix that is not orthogonal with determinant
2338  /// +/-1.
2339  ///
2340  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == +/-1`.
2341  pub fn is_orthonormal (&self) -> bool {
2342    use num::One;
2343    let determinant = self.0.0.determinant();
2344    self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2345    (determinant == S::one() || determinant == -S::one())
2346  }
2347  /// Returns `None` if called with a matrix that is not orthogonal with determinant
2348  /// +/-1.
2349  ///
2350  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == +/-1`
2351  /// (approximately using relative equality).
2352  pub fn is_orthonormal_approx (&self) -> bool where
2353    S : approx::RelativeEq <Epsilon=S>,
2354    V::LinearEndo : approx::RelativeEq <Epsilon=S>
2355  {
2356    use num::One;
2357    let four         = S::one() + S::one() + S::one() + S::one();
2358    let epsilon      = S::default_epsilon() * four;
2359    let max_relative = S::default_max_relative() * four;
2360    let determinant  = self.0.0.determinant();
2361    approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2362      max_relative=max_relative, epsilon=epsilon) &&
2363    ( approx::relative_eq!(determinant,  S::one(), max_relative=max_relative) ||
2364      approx::relative_eq!(determinant, -S::one(), max_relative=max_relative) )
2365  }
2366  /// Checks whether the transformation is a special orthogonal matrix.
2367  ///
2368  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`.
2369  pub fn is_rotation (&self) -> bool {
2370    use num::One;
2371    let determinant = self.0.0.determinant();
2372    self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2373    determinant == S::one()
2374  }
2375  /// Checks whether the transformation is a special orthogonal matrix.
2376  ///
2377  /// This method checks whether `mat * mat^T == I` and `mat.determinant() == 1`
2378  /// (approximately using relative equality).
2379  pub fn is_rotation_approx (&self) -> bool where
2380    S : approx::RelativeEq <Epsilon=S>,
2381    V::LinearEndo : approx::RelativeEq <Epsilon=S>
2382  {
2383    use num::One;
2384    let four         = S::one() + S::one() + S::one() + S::one();
2385    let epsilon      = S::default_epsilon() * four;
2386    let max_relative = S::default_max_relative() * four;
2387    let determinant  = self.0.0.determinant();
2388    approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2389      max_relative=max_relative, epsilon=epsilon) &&
2390    approx::relative_eq!(determinant,  S::one(), max_relative=max_relative)
2391  }
2392}
2393impl <S, V> std::ops::Mul <V> for LinearAuto <S, V> where
2394  V : Module <S>,
2395  V::LinearEndo : MaybeSerDes,
2396  S : Ring
2397{
2398  type Output = V;
2399  fn mul (self, rhs : V) -> Self::Output {
2400    self.0 * rhs
2401  }
2402}
2403impl <S, V> std::ops::Mul for LinearAuto <S, V> where
2404  V : Module <S>,
2405  V::LinearEndo : MaybeSerDes,
2406  S : Ring
2407{
2408  type Output = Self;
2409  fn mul (self, rhs : Self) -> Self::Output {
2410    LinearAuto (self.0 * rhs.0)
2411  }
2412}
2413impl <S, V> std::ops::MulAssign <Self> for LinearAuto <S, V> where
2414  V : Module <S>,
2415  V::LinearEndo : MaybeSerDes,
2416  S : Ring
2417{
2418  fn mul_assign (&mut self, rhs : Self) {
2419    self.0 *= rhs.0
2420  }
2421}
2422impl <S, V> num::One for LinearAuto <S, V> where
2423  V : Module <S>,
2424  V::LinearEndo : MaybeSerDes,
2425  S : Ring
2426{
2427  fn one() -> Self {
2428    LinearAuto (LinearIso::one())
2429  }
2430}
2431
2432//
2433//  impl AffineMap
2434//
2435impl <S, A, B, M> AffineMap <S, A, B, M> where
2436  A : AffineSpace <S>,
2437  B : AffineSpace <S>,
2438  M : LinearMap <S, A::Translation, B::Translation>,
2439  S : Field
2440{
2441  pub const fn new (linear_map : M, translation : B::Translation) -> Self {
2442    AffineMap { linear_map, translation, _phantom: PhantomData }
2443  }
2444  pub fn map (self, point : A) -> B {
2445    B::from_vector (self.linear_map * point.to_vector() + self.translation)
2446  }
2447  pub fn transform (self, translation : A::Translation) -> B::Translation {
2448    self.linear_map * translation
2449  }
2450}
2451
2452//
2453//  impl Affinity
2454//
2455impl <S, A, B, M> Affinity <S, A, B, M> where
2456  M : LinearMap <S, A::Translation, B::Translation>,
2457  A : AffineSpace <S>,
2458  B : AffineSpace <S>,
2459  S : Field
2460{
2461  pub const fn new (
2462    linear_iso  : LinearIso <S, A::Translation, B::Translation, M>,
2463    translation : B::Translation
2464  ) -> Self {
2465    Affinity { linear_iso, translation }
2466  }
2467  pub fn mat (self) -> M {
2468    *self.linear_iso
2469  }
2470  pub fn map (self, point : A) -> B {
2471    B::from_vector (self.linear_iso * point.to_vector() + self.translation)
2472  }
2473  pub fn transform (self, translation : A::Translation) -> B::Translation {
2474    self.linear_iso * translation
2475  }
2476}
2477
2478//
2479//  impl Projectivity
2480//
2481impl <S, P, Q, M> Projectivity <S, P, Q, M> where
2482  M : LinearMap <S, P::Translation, Q::Translation>,
2483  P : ProjectiveSpace <S>,
2484  Q : ProjectiveSpace <S>,
2485  S : Field
2486{
2487  pub const fn new (linear_iso : LinearIso <S, P::Translation, Q::Translation, M>)
2488    -> Self
2489  {
2490    Projectivity (linear_iso)
2491  }
2492  pub fn mat (self) -> M {
2493    **self
2494  }
2495}
2496impl <S, P, Q, M> std::ops::Deref for Projectivity <S, P, Q, M> where
2497  M : LinearMap <S, P::Translation, Q::Translation>,
2498  P : ProjectiveSpace <S>,
2499  Q : ProjectiveSpace <S>,
2500  S : Field
2501{
2502  type Target = LinearIso <S, P::Translation, Q::Translation, M>;
2503  fn deref (&self) -> &Self::Target {
2504    &self.0
2505  }
2506}
2507impl <S, P, Q, M> std::ops::Mul <P> for Projectivity <S, P, Q, M> where
2508  M : LinearMap <S, P::Translation, Q::Translation>,
2509  P : ProjectiveSpace <S>,
2510  Q : ProjectiveSpace <S>,
2511  S : Field
2512{
2513  type Output = Q;
2514  fn mul (self, rhs : P) -> Q {
2515    Q::from_vector (self.0 * rhs.to_vector())
2516  }
2517}
2518
2519pub macro point {
2520  ($x:expr, $y:expr) => {
2521    $crate::point2 ($x, $y)
2522  },
2523  ($x:expr, $y:expr, $z:expr) => {
2524    $crate::point3 ($x, $y, $z)
2525  },
2526  ($x:expr, $y:expr, $z:expr, $w:expr) => {
2527    $crate::point4 ($x, $y, $z, $w)
2528  }
2529}
2530
2531pub macro vector {
2532  ($x:expr, $y:expr) => {
2533    $crate::vector2 ($x, $y)
2534  },
2535  ($x:expr, $y:expr, $z:expr) => {
2536    $crate::vector3 ($x, $y, $z)
2537  },
2538  ($x:expr, $y:expr, $z:expr, $w:expr) => {
2539    $crate::vector4 ($x, $y, $z, $w)
2540  }
2541}
2542
2543pub macro matrix {
2544  ($v00:expr, $v01:expr; $v10:expr, $v11:expr$(;)?) => {
2545    $crate::Matrix2::new ($v00, $v01, $v10, $v11)
2546  },
2547  ( $v00:expr, $v01:expr, $v02:expr;
2548    $v10:expr, $v11:expr, $v12:expr;
2549    $v20:expr, $v21:expr, $v22:expr$(;)?
2550  ) => {
2551    $crate::Matrix3::new ($v00, $v01, $v02, $v10, $v11, $v12, $v20, $v21, $v22)
2552  },
2553  ( $v00:expr, $v01:expr, $v02:expr, $v03:expr;
2554    $v10:expr, $v11:expr, $v12:expr, $v13:expr;
2555    $v20:expr, $v21:expr, $v22:expr, $v23:expr;
2556    $v30:expr, $v31:expr, $v32:expr, $v33:expr$(;)?
2557  ) => {
2558    $crate::Matrix4::new (
2559      $v00, $v01, $v02, $v03,
2560      $v10, $v11, $v12, $v13,
2561      $v20, $v21, $v22, $v23,
2562      $v30, $v31, $v32, $v33)
2563  }
2564}
2565
2566macro_rules! impl_angle {
2567  ( $angle:ident, $docname:expr ) => {
2568    #[doc = $docname]
2569    #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2570    // TODO: derive From here causes a test failure in the intrinsic angles test
2571    #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2572    pub struct $angle <S> (pub S);
2573    impl_numcast_primitive_map!($angle);
2574    impl <S : Ring> std::iter::Sum for $angle <S> {
2575      fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2576        use num::Zero;
2577        iter.fold ($angle::zero(), |acc, item| acc + item)
2578      }
2579    }
2580    impl <S : Ring> std::ops::Neg for $angle <S> {
2581      type Output = Self;
2582      fn neg (self) -> Self {
2583        $angle (-self.0)
2584      }
2585    }
2586    impl <S : Ring> std::ops::Add for $angle <S> {
2587      type Output = Self;
2588      fn add (self, other : Self) -> Self::Output {
2589        $angle (self.0 + other.0)
2590      }
2591    }
2592    impl <S : Ring> std::ops::AddAssign for $angle <S> {
2593      fn add_assign (&mut self, other : Self) {
2594        self.0 += other.0
2595      }
2596    }
2597    impl <S : Ring> std::ops::Sub for $angle <S> {
2598      type Output = Self;
2599      fn sub (self, other : Self) -> Self::Output {
2600        $angle (self.0 - other.0)
2601      }
2602    }
2603    impl <S : Ring> std::ops::SubAssign for $angle <S> {
2604      fn sub_assign (&mut self, other : Self) {
2605        self.0 -= other.0
2606      }
2607    }
2608    impl <S : Ring> std::ops::Mul <S> for $angle <S> {
2609      type Output = Self;
2610      fn mul (self, scalar : S) -> Self::Output {
2611        $angle (scalar * self.0)
2612      }
2613    }
2614    impl <S : Ring> std::ops::MulAssign <S> for $angle <S> {
2615      fn mul_assign (&mut self, scalar : S) {
2616        self.0 *= scalar
2617      }
2618    }
2619    impl <S : Field> std::ops::Div for $angle <S> {
2620      type Output = S;
2621      fn div (self, other : Self) -> Self::Output {
2622        self.0 / other.0
2623      }
2624    }
2625    impl <S : Field> std::ops::Div <S> for $angle <S> {
2626      type Output = Self;
2627      fn div (self, scalar : S) -> Self::Output {
2628        $angle (self.0 / scalar)
2629      }
2630    }
2631    impl <S : Field> std::ops::DivAssign <S> for $angle <S> {
2632      fn div_assign (&mut self, scalar : S) {
2633        self.0 /= scalar
2634      }
2635    }
2636    impl <S : OrderedField> std::ops::Rem for $angle <S> {
2637      type Output = Self;
2638      fn rem (self, other : Self) -> Self::Output {
2639        $angle (self.0 % other.0)
2640      }
2641    }
2642    impl <S : Ring> num::Zero for $angle <S> {
2643      fn zero() -> Self {
2644        $angle (S::zero())
2645      }
2646      fn is_zero (&self) -> bool {
2647        self.0.is_zero()
2648      }
2649    }
2650    impl <S : Ring> From <S> for $angle <S> {
2651      fn from (s : S) -> Self {
2652        $angle (s)
2653      }
2654    }
2655  }
2656}
2657
2658macro_rules! impl_angle_wrapped {
2659  ( $angle:ident, $comment:expr ) => {
2660    #[doc = $comment]
2661    #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2662    #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2663    pub struct $angle <S> (Rad <S>);
2664
2665    impl_numcast!($angle);
2666    impl <S : Copy> $angle <S> {
2667      pub const fn angle (&self) -> Rad <S> {
2668        self.0
2669      }
2670    }
2671    impl <S : Real> std::iter::Sum for $angle <S> {
2672      fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2673        use num::Zero;
2674        iter.fold ($angle::zero(), |acc, item| acc + item)
2675      }
2676    }
2677    impl <S : Real> std::ops::Neg for $angle <S> {
2678      type Output = Self;
2679      fn neg (self) -> Self {
2680        self.map (Rad::neg)
2681      }
2682    }
2683    impl <S : Real> std::ops::Add for $angle <S> {
2684      type Output = Self;
2685      fn add (self, other : Self) -> Self::Output {
2686        self.map (|angle| angle + other.0)
2687      }
2688    }
2689    impl <S : Real> std::ops::AddAssign for $angle <S> {
2690      fn add_assign (&mut self, other : Self) {
2691        *self = *self + other
2692      }
2693    }
2694    impl <S : Real> std::ops::Add <Rad <S>> for $angle <S> {
2695      type Output = Self;
2696      fn add (self, angle : Rad <S>) -> Self::Output {
2697        self.map (|a| a + angle)
2698      }
2699    }
2700    impl <S : Real> std::ops::AddAssign <Rad <S>> for $angle <S> {
2701      fn add_assign (&mut self, angle : Rad <S>) {
2702        *self = *self + angle
2703      }
2704    }
2705    impl <S : Real> std::ops::Sub for $angle <S> {
2706      type Output = Self;
2707      fn sub (self, other : Self) -> Self::Output {
2708        self.map (|angle| angle - other.0)
2709      }
2710    }
2711    impl <S : Real> std::ops::SubAssign for $angle <S> {
2712      fn sub_assign (&mut self, other : Self) {
2713        *self = *self - other
2714      }
2715    }
2716    impl <S : Real> std::ops::Sub <Rad <S>> for $angle <S> {
2717      type Output = Self;
2718      fn sub (self, angle : Rad <S>) -> Self::Output {
2719        self.map (|a| a - angle)
2720      }
2721    }
2722    impl <S : Real> std::ops::SubAssign <Rad <S>> for $angle <S> {
2723      fn sub_assign (&mut self, angle : Rad <S>) {
2724        *self = *self - angle
2725      }
2726    }
2727    impl <S : Real> std::ops::Mul <S> for $angle <S> {
2728      type Output = Self;
2729      fn mul (self, scalar : S) -> Self::Output {
2730        self.map (|angle| angle * scalar)
2731      }
2732    }
2733    impl <S : Real> std::ops::MulAssign <S> for $angle <S> {
2734      fn mul_assign (&mut self, scalar : S) {
2735        *self = *self * scalar
2736      }
2737    }
2738    impl <S : Real> std::ops::Div for $angle <S> {
2739      type Output = S;
2740      fn div (self, other : Self) -> Self::Output {
2741        self.0 / other.0
2742      }
2743    }
2744    impl <S : Real> std::ops::Div <S> for $angle <S> {
2745      type Output = Self;
2746      fn div (self, scalar : S) -> Self::Output {
2747        self.map (|angle| angle / scalar)
2748      }
2749    }
2750    impl <S : Real> std::ops::DivAssign <S> for $angle <S> {
2751      fn div_assign (&mut self, scalar : S) {
2752        *self = *self / scalar
2753      }
2754    }
2755    impl <S : Real> std::ops::Rem for $angle <S> {
2756      type Output = Self;
2757      fn rem (self, other : Self) -> Self::Output {
2758        self.map (|angle| angle % other.0)
2759      }
2760    }
2761    impl <S : Real> num::Zero for $angle <S> {
2762      fn zero() -> Self {
2763        $angle (Rad::zero())
2764      }
2765      fn is_zero (&self) -> bool {
2766        self.0.is_zero()
2767      }
2768    }
2769  }
2770}
2771
2772macro_rules! impl_dimension {
2773  ( $point:ident, $vector:ident, $nonzero:ident, $unit:ident,
2774    $point_fun:ident, $vector_fun:ident, $matrix_fun:ident, $ortho_fun:ident,
2775    [$($component:ident),+],
2776    [$(($axis_method:ident, $unit_method:ident)),+], [$($patch:ident)?],
2777    [$($projective:ident)?],
2778    $matrix:ident, [$($submatrix:ident)?], $ndims:expr, $dimension:expr
2779  ) => {
2780    #[doc = $dimension]
2781    #[doc = "position"]
2782    #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display, From, Neg)]
2783    #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2784    #[display("{}", _0)]
2785    #[repr(C)]
2786    pub struct $point <S> (pub $vector <S>);
2787
2788    #[doc = $dimension]
2789    #[doc = "non-zero vector"]
2790    #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2791    #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2792    #[display("{}", _0)]
2793    #[repr(C)]
2794    pub struct $nonzero <S> ($vector <S>);
2795
2796    #[doc = $dimension]
2797    #[doc = "unit vector"]
2798    #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2799    #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2800    #[display("{}", _0)]
2801    #[repr(C)]
2802    pub struct $unit <S> ($vector <S>);
2803
2804    //
2805    //  impl PointN
2806    //
2807    #[doc = "Construct a"]
2808    #[doc = $dimension]
2809    #[doc = "point"]
2810    pub const fn $point_fun <S> ($($component : S),+) -> $point <S> {
2811      $point::new ($($component),+)
2812    }
2813    impl_numcast!($point);
2814    impl <S> $point <S> {
2815      pub const fn new ($($component : S),+) -> Self {
2816        $point ($vector::new ($($component),+))
2817      }
2818      $(
2819      pub const fn $component (&self) -> S where S : Copy {
2820        self.0.$component
2821      }
2822      )+
2823      pub fn distance (self, other : $point <S>) -> NonNegative <S> where
2824        S : OrderedField + Sqrt
2825      {
2826        MetricSpace::distance (self.0, other.0)
2827      }
2828      pub fn distance_squared (self, other : $point <S>) -> NonNegative <S> where
2829        S : OrderedField
2830      {
2831        MetricSpace::distance_squared (self.0, other.0)
2832      }
2833    }
2834    // projective completion
2835    $(
2836    impl <S : Field> ProjectiveSpace <S> for $projective <S> {
2837      type Patch = $point <S>;
2838    }
2839    )?
2840    impl <S : Field> AffineSpace <S> for $point <S> {
2841      type Translation = $vector <S>;
2842    }
2843    impl <S : Ring> Point <$vector <S>> for $point <S> {
2844      fn to_vector (self) -> $vector <S> {
2845        self.0
2846      }
2847      fn from_vector (vector : $vector <S>) -> Self {
2848        $point (vector)
2849      }
2850    }
2851    impl <S> From <[S; $ndims]> for $point <S> {
2852      fn from (array : [S; $ndims]) -> Self {
2853        $point (array.into())
2854      }
2855    }
2856    $(
2857    impl <S> From <($patch <S>, S)> for $point <S> {
2858      fn from ((patch, scale) : ($patch <S>, S)) -> Self {
2859        $point ((patch.0, scale).into())
2860      }
2861    }
2862    )?
2863    impl <S> std::cmp::PartialOrd for $point <S> where S : PartialOrd {
2864      fn partial_cmp (&self, other : &Self) -> Option <std::cmp::Ordering> {
2865        self.0.as_slice().partial_cmp (&other.0.as_slice())
2866      }
2867    }
2868    impl <S> std::ops::Add <$vector <S>> for $point <S> where S : AdditiveGroup {
2869      type Output = Self;
2870      fn add (self, displacement : $vector <S>) -> Self::Output {
2871        $point (self.0 + displacement)
2872      }
2873    }
2874    impl <S> std::ops::Add <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2875      type Output = Self;
2876      fn add (self, displacement : &$vector <S>) -> Self::Output {
2877        $point (self.0 + *displacement)
2878      }
2879    }
2880    impl <S> std::ops::Add <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2881      type Output = $point <S>;
2882      fn add (self, displacement : $vector <S>) -> Self::Output {
2883        $point (self.0 + displacement)
2884      }
2885    }
2886    impl <S> std::ops::Add <&$vector <S>> for &$point <S> where
2887      S : AdditiveGroup + Copy
2888    {
2889      type Output = $point <S>;
2890      fn add (self, displacement : &$vector <S>) -> Self::Output {
2891        $point (self.0 + *displacement)
2892      }
2893    }
2894    impl <S> std::ops::AddAssign <$vector <S>> for $point <S> where S : AdditiveGroup {
2895      fn add_assign (&mut self, displacement : $vector <S>) {
2896        self.0 += displacement
2897      }
2898    }
2899    impl <S> std::ops::AddAssign <&$vector <S>> for $point <S> where
2900      S : AdditiveGroup + Copy
2901    {
2902      fn add_assign (&mut self, displacement : &$vector <S>) {
2903        self.0 += *displacement
2904      }
2905    }
2906    impl <S> std::ops::Sub <$vector <S>> for $point <S> where S : AdditiveGroup {
2907      type Output = Self;
2908      fn sub (self, displacement : $vector <S>) -> Self::Output {
2909        $point (self.0 - displacement)
2910      }
2911    }
2912    impl <S> std::ops::Sub <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2913      type Output = Self;
2914      fn sub (self, displacement : &$vector <S>) -> Self::Output {
2915        $point (self.0 - *displacement)
2916      }
2917    }
2918    impl <S> std::ops::Sub <$point <S>> for $point <S> where S : AdditiveGroup {
2919      type Output = $vector <S>;
2920      fn sub (self, other : Self) -> Self::Output {
2921        self.0 - other.0
2922      }
2923    }
2924    impl <S> std::ops::Sub <&$point <S>> for $point <S> where S : AdditiveGroup + Copy {
2925      type Output = $vector <S>;
2926      fn sub (self, other : &Self) -> Self::Output {
2927        self.0 - other.0
2928      }
2929    }
2930    impl <S> std::ops::Sub <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2931      type Output = $point <S>;
2932      fn sub (self, displacement : $vector <S>) -> Self::Output {
2933        $point (self.0 - displacement)
2934      }
2935    }
2936    impl <S> std::ops::Sub <&$vector <S>> for &$point <S> where
2937      S : AdditiveGroup + Copy
2938    {
2939      type Output = $point <S>;
2940      fn sub (self, displacement : &$vector <S>) -> Self::Output {
2941        $point (self.0 - *displacement)
2942      }
2943    }
2944    impl <S> std::ops::Sub <$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2945      type Output = $vector <S>;
2946      fn sub (self, other : $point <S>) -> Self::Output {
2947        self.0 - other.0
2948      }
2949    }
2950    impl <S> std::ops::Sub <&$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2951      type Output = $vector <S>;
2952      fn sub (self, other : &$point <S>) -> Self::Output {
2953        self.0 - other.0
2954      }
2955    }
2956    impl <S> std::ops::SubAssign <$vector <S>> for $point <S> where
2957      S : AdditiveGroup
2958    {
2959      fn sub_assign (&mut self, displacement : $vector <S>) {
2960        self.0 -= displacement
2961      }
2962    }
2963    impl <S> std::ops::SubAssign <&$vector <S>> for $point <S> where
2964      S : AdditiveGroup + Copy
2965    {
2966      fn sub_assign (&mut self, displacement : &$vector <S>) {
2967        self.0 -= *displacement
2968      }
2969    }
2970    impl <S> approx::AbsDiffEq for $point <S> where
2971      S : approx::AbsDiffEq,
2972      S::Epsilon : Copy
2973    {
2974      type Epsilon = S::Epsilon;
2975      fn default_epsilon() -> Self::Epsilon {
2976        S::default_epsilon()
2977      }
2978      #[inline]
2979      fn abs_diff_eq (&self, other : &Self, epsilon : Self::Epsilon) -> bool {
2980        self.0.abs_diff_eq (&other.0, epsilon)
2981      }
2982    }
2983    impl <S> approx::RelativeEq for $point <S> where
2984      S : approx::RelativeEq,
2985      S::Epsilon : Copy
2986    {
2987      fn default_max_relative() -> Self::Epsilon {
2988        S::default_max_relative()
2989      }
2990      #[inline]
2991      fn relative_eq (&self,
2992        other : &Self, epsilon : Self::Epsilon, max_relative : Self::Epsilon
2993      ) -> bool {
2994        self.0.relative_eq (&other.0, epsilon, max_relative)
2995      }
2996    }
2997    impl <S> approx::UlpsEq for $point <S> where
2998      S : approx::UlpsEq,
2999      S::Epsilon : Copy
3000    {
3001      fn default_max_ulps() -> u32 {
3002        S::default_max_ulps()
3003      }
3004      #[inline]
3005      fn ulps_eq (&self, other : &Self, epsilon : Self::Epsilon, max_ulps : u32)
3006        -> bool
3007      {
3008        self.0.ulps_eq (&other.0, epsilon, max_ulps)
3009      }
3010    }
3011    //
3012    //  impl VectorN
3013    //
3014    #[doc = "Construct a"]
3015    #[doc = $dimension]
3016    #[doc = "vector"]
3017    pub const fn $vector_fun <S> ($($component : S),+) -> $vector <S> {
3018      $vector::new ($($component),+)
3019    }
3020    impl <S> NormedVectorSpace <S> for $vector <S> where S : OrderedField {
3021      type Unit = $unit <S>;
3022      fn norm_squared (self) -> NonNegative <S> {
3023        self.self_dot()
3024      }
3025      fn norm_max (self) -> NonNegative <S> where S : SignedExt {
3026        NonNegative::unchecked (self.map (|x| x.abs()).reduce_partial_max())
3027      }
3028      fn normalize (self) -> Self::Unit where S : Sqrt {
3029        $unit (self / *self.norm())
3030      }
3031    }
3032    impl <S> InnerProductSpace <S> for $vector <S> where S : Field {
3033      fn outer_product (self, rhs : Self) -> $matrix <S> {
3034        let mut cols : $vector <$vector <S>> = [$vector::zero(); $ndims].into();
3035        for col in 0..$ndims {
3036          for row in 0..$ndims {
3037            cols[col][row] = self[row] * rhs[col];
3038          }
3039        }
3040        $matrix { cols }
3041      }
3042      fn orthogonal (self) -> Self where S : approx::AbsDiffEq <Epsilon = S> {
3043        $ortho_fun (self)
3044      }
3045    }
3046    impl <S : Ring> Dot <S> for $vector <S> {
3047      fn dot (self, other : Self) -> S {
3048        self.dot (other)
3049      }
3050    }
3051    impl <S : Field> VectorSpace <S> for $vector <S> {
3052      type NonZero = $nonzero <S>;
3053      fn map <F> (self, f : F) -> Self where F : FnMut (S) -> S {
3054        self.map (f)
3055      }
3056    }
3057    impl <S> Module <S> for $vector <S> where S : Ring + num::MulAdd {
3058      type LinearEndo = $matrix <S>;
3059    }
3060    impl <S> GroupAction <Addition, $point <S>> for $vector <S>
3061      where S : AdditiveGroup { }
3062    impl <S> MonoidAction <Addition, $point <S>> for $vector <S>
3063      where S : AdditiveGroup { }
3064    impl <S> SemigroupAction <Addition, $point <S>> for $vector <S>
3065      where S : AdditiveGroup
3066    {
3067      fn action (self, point : $point <S>) -> $point <S> {
3068        point + self
3069      }
3070    }
3071    impl <S> std::ops::Mul <LinearAuto <S, Self>> for $vector <S> where
3072      S : Ring + MaybeSerDes
3073    {
3074      type Output = Self;
3075      fn mul (self, rhs : LinearAuto <S, Self>) -> Self::Output {
3076        self * rhs.0
3077      }
3078    }
3079    impl <S> std::ops::Mul <LinearIso <S, Self, Self, $matrix <S>>> for $vector <S> where
3080      S : Ring + MaybeSerDes
3081    {
3082      type Output = Self;
3083      fn mul (self, rhs : LinearIso <S, Self, Self, $matrix <S>>) -> Self::Output {
3084        self * rhs.0
3085      }
3086    }
3087    impl <S> num::Inv for LinearAuto <S, $vector <S>> where
3088      S : Ring + num::real::Real + MaybeSerDes
3089    {
3090      type Output = Self;
3091      fn inv (self) -> Self::Output {
3092        LinearAuto (self.0.inv())
3093      }
3094    }
3095    impl <S> std::ops::Div for LinearAuto <S, $vector <S>> where
3096      S : Ring + num::Float + MaybeSerDes
3097    {
3098      type Output = Self;
3099      fn div (self, rhs : Self) -> Self::Output {
3100        LinearAuto (self.0 / rhs.0)
3101      }
3102    }
3103    impl <S> std::ops::DivAssign <Self> for LinearAuto <S, $vector <S>> where
3104      S : Ring + num::Float + MaybeSerDes
3105    {
3106      fn div_assign (&mut self, rhs : Self) {
3107        self.0 /= rhs.0
3108      }
3109    }
3110    impl <S> From <$unit <S>> for $vector <S> where S : Copy {
3111      fn from (unit : $unit <S>) -> Self {
3112        *unit
3113      }
3114    }
3115    //
3116    //  impl MatrixN
3117    //
3118    #[doc = "Construct a"]
3119    #[doc = $dimension]
3120    #[doc = "matrix"]
3121    pub const fn $matrix_fun <S> ($($component : $vector <S>),+) -> $matrix <S> {
3122      $matrix {
3123        cols: $vector_fun ($($component),+)
3124      }
3125    }
3126    impl <S, V, W> num::Inv for LinearIso <S, V, W, $matrix <S>> where
3127      V : Module <S>,
3128      W : Module <S>,
3129      S : Ring + num::real::Real
3130    {
3131      type Output = LinearIso <S, W, V, $matrix <S>>;
3132      fn inv (self) -> Self::Output {
3133        // TODO: currently the vek crate only implements invert for Mat4
3134        let mat4 = Matrix4::from (self.0);
3135        LinearIso (mat4.inverted().into(), PhantomData::default())
3136      }
3137    }
3138    impl <S, V> std::ops::Div for LinearIso <S, V, V, $matrix <S>> where
3139      V : Module <S, LinearEndo=$matrix <S>>,
3140      S : Ring + num::real::Real
3141    {
3142      type Output = Self;
3143      #[expect(clippy::suspicious_arithmetic_impl)]
3144      fn div (self, rhs : Self) -> Self::Output {
3145        use num::Inv;
3146        self * rhs.inv()
3147      }
3148    }
3149    impl <S, V> std::ops::DivAssign for LinearIso <S, V, V, $matrix <S>> where
3150      V : Module <S, LinearEndo=$matrix <S>>,
3151      S : Ring + num::real::Real
3152    {
3153      #[expect(clippy::suspicious_op_assign_impl)]
3154      fn div_assign (&mut self, rhs : Self) {
3155        use num::Inv;
3156        *self *= rhs.inv()
3157      }
3158    }
3159    $(
3160    impl <S : Copy> Matrix <S> for $matrix <S> {
3161      type Rows = $vector <$vector <S>>;
3162      type Submatrix = $submatrix <S>;
3163      fn rows (self) -> $vector <$vector <S>> {
3164        self.into_row_arrays().map ($vector::from).into()
3165      }
3166      fn submatrix (self, i : usize, j : usize) -> $submatrix <S> {
3167        let a : [S; ($ndims-1) * ($ndims-1)] = std::array::from_fn (|index|{
3168          let i = (i + 1 + index / ($ndims-1)) % $ndims;
3169          let j = (j + 1 + index % ($ndims-1)) % $ndims;
3170          self.cols[i][j]
3171        });
3172        $submatrix::from_col_array (a)
3173      }
3174      /// Fill additional row and column with zeros
3175      fn fill_zeros (submatrix : $submatrix <S>) -> Self where S : num::Zero{
3176        let cols = $vector::from (
3177          std::array::from_fn (|i| if i < $ndims-1 {
3178            $vector::from (submatrix.cols[i])
3179          } else {
3180            $vector::zero()
3181          })
3182        );
3183        $matrix { cols }
3184      }
3185    }
3186    )?
3187    impl <S : Ring> LinearMap <S, $vector <S>, $vector <S>> for $matrix <S> {
3188      fn determinant (self) -> S {
3189        self.determinant()
3190      }
3191      fn transpose (self) -> Self {
3192        self.transposed()
3193      }
3194    }
3195    //
3196    //  impl NonZeroN
3197    //
3198    impl_numcast!($nonzero);
3199    impl <S> $nonzero <S> {
3200      /// Returns 'None' if called with the zero vector
3201      // TODO: move broken doctests to unit tests
3202      // # Example
3203      //
3204      // ```
3205      // # use math_utils::$nonzero;
3206      // assert!($nonzero::new ([1.0, 0.0].into()).is_some());
3207      // assert!($nonzero::new ([0.0, 0.0].into()).is_none());
3208      // ```
3209      pub fn new (vector : $vector <S>) -> Option <Self> where S : Ring {
3210        use num::Zero;
3211        if !vector.is_zero() {
3212          Some ($nonzero (vector))
3213        } else {
3214          None
3215        }
3216      }
3217      /// Panics if zero vector is given.
3218      // TODO: move broken doctests to unit tests
3219      // # Panics
3220      //
3221      // ```should_panic
3222      // # use math_utils::$nonzero;
3223      // let x = $nonzero::noisy ([0.0, 0.0].into());  // panic!
3224      // ```
3225      pub fn noisy (vector : $vector <S>) -> Self where S : Ring {
3226        use num::Zero;
3227        assert!(!vector.is_zero());
3228        $nonzero (vector)
3229      }
3230      /// It is a debug assertion if the given vector is zero.
3231      ///
3232      /// In debug builds, method checks that `vector != Zero::zero()`.
3233      #[inline]
3234      pub fn unchecked (vector : $vector <S>) -> Self where S : Ring + std::fmt::Debug {
3235        debug_assert_ne!(vector, $vector::zero());
3236        $nonzero (vector)
3237      }
3238      /// Map an operation on the underlying vector, panicking if the result is zero
3239      // TODO: move broken doctests to unit tests
3240      // # Panics
3241      //
3242      // Panics of the result is zero:
3243      //
3244      // ```should_panic
3245      // # use math_utils::$nonzero;
3246      // let v = $nonzero::noisy ([1.0, 1.0].into())
3247      //   .map_noisy (|x| 0.0 * x);  // panic!
3248      // ```
3249      #[must_use]
3250      pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3251        where S : Ring
3252      {
3253        Self::noisy (fun (self.0))
3254      }
3255      #[must_use]
3256      pub fn map_unchecked (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3257        where S : Ring + std::fmt::Debug
3258      {
3259        Self::unchecked (fun (self.0))
3260      }
3261    }
3262    impl <S> std::ops::Deref for $nonzero <S> {
3263      type Target = $vector <S>;
3264      fn deref (&self) -> &$vector <S> {
3265        &self.0
3266      }
3267    }
3268    impl <S : Ring> std::ops::Neg for $nonzero <S> {
3269      type Output = Self;
3270      fn neg (self) -> Self {
3271        $nonzero (-self.0)
3272      }
3273    }
3274    impl <S> From <$unit <S>> for $nonzero <S> where S : Ring + std::fmt::Debug {
3275      fn from (unit : $unit <S>) -> Self {
3276        $nonzero::unchecked (*unit)
3277      }
3278    }
3279
3280    //
3281    //  impl UnitN
3282    //
3283    impl_numcast!($unit);
3284    impl <S> $unit <S> {
3285      // TODO: move broken doc tests to unit tests
3286      // ```
3287      // # use math_utils::$unit;
3288      // assert_eq!($unit::axis_x(), $unit::new ([1.0, 0.0].into()).unwrap());
3289      // ```
3290      $(
3291      #[inline]
3292      pub fn $axis_method() -> Self where S : num::One + num::Zero {
3293        $unit ($vector::$unit_method())
3294      }
3295      )+
3296      /// Returns 'None' if called with a non-normalized vector.
3297      ///
3298      /// This method checks whether `vector == vector.normalize()`.
3299      pub fn new (vector : $vector <S>) -> Option <Self> where S : OrderedField + Sqrt {
3300        if vector == *vector.normalize() {
3301          Some ($unit (vector))
3302        } else {
3303          None
3304        }
3305      }
3306      /// Returns 'None' if called with an non-normalized vector determined by checking
3307      /// if the magnitude is approximately one.
3308      ///
3309      /// Note the required `RelativeEq` trait is only implemented for floating-point
3310      /// types.
3311      // TODO: implement required methods to avoid num::real::Real constraint
3312      pub fn new_approx (vector : $vector <S>) -> Option <Self> where
3313        S : num::real::Real + approx::RelativeEq <Epsilon=S>
3314      {
3315        if vector.is_normalized() {
3316          Some ($unit (vector))
3317        } else {
3318          None
3319        }
3320      }
3321      /// Normalizes a given non-zero vector.
3322      // TODO: move broken doc tests to unit tests
3323      // # Example
3324      //
3325      // ```
3326      // # use math_utils::$unit;
3327      // assert_eq!(
3328      //   *$unit::normalize ([2.0, 0.0].into()),
3329      //   [1.0, 0.0].into()
3330      // );
3331      // ```
3332      //
3333      // # Panics
3334      //
3335      // Panics if the zero vector is given:
3336      //
3337      // ```should_panic
3338      // # use math_utils::$unit;
3339      // let x = $unit::normalize ([0.0, 0.0].into());  // panic!
3340      // ```
3341      pub fn normalize (vector : $vector <S>) -> Self where S : OrderedField + Sqrt {
3342        use num::Zero;
3343        assert!(!vector.is_zero());
3344        vector.normalize()
3345      }
3346      /// Normalizes a given non-zero vector only if the vector is not already
3347      /// normalized.
3348      // TODO: implement required methods to avoid num::real::Real constraint
3349      pub fn normalize_approx (vector : $vector <S>) -> Self where
3350        S : num::real::Real + approx::RelativeEq <Epsilon=S>
3351      {
3352        use num::Zero;
3353        assert!(!vector.is_zero());
3354        let vector = if vector.is_normalized() {
3355          vector
3356        } else {
3357          vector.normalized()
3358        };
3359        $unit (vector)
3360      }
3361      /// Panics if a non-normalized vector is given.
3362      ///
3363      /// This method checks whether `vector == vector.normalize()`.
3364      // TODO: move broken doc tests to unit tests
3365      // ```should_panic
3366      // # use math_utils::$unit;
3367      // let x = $unit::noisy ([2.0, 0.0].into());  // panic!
3368      // ```
3369      pub fn noisy (vector : $vector <S>) -> Self where
3370        S : OrderedField + Sqrt + std::fmt::Debug
3371      {
3372        assert_eq!(vector, *vector.normalize());
3373        $unit (vector)
3374      }
3375      /// Panics if a non-normalized vector is given.
3376      ///
3377      /// Checks if the magnitude is approximately one.
3378      // TODO: move broken doc tests to unit tests
3379      // ```should_panic
3380      // # use math_utils::$unit;
3381      // let x = $unit::noisy ([2.0, 0.0].into());  // panic!
3382      // ```
3383      pub fn noisy_approx (vector : $vector <S>) -> Self where
3384        S : num::real::Real + approx::RelativeEq <Epsilon=S>
3385      {
3386        assert!(vector.is_normalized());
3387        $unit (vector)
3388      }
3389      /// It is a debug assertion if the given vector is not normalized.
3390      ///
3391      /// In debug builds, method checks that `vector == vector.normalize()`.
3392      #[inline]
3393      pub fn unchecked (vector : $vector <S>) -> Self where
3394        S : OrderedField + Sqrt + std::fmt::Debug
3395      {
3396        debug_assert_eq!(vector, *vector.normalize());
3397        $unit (vector)
3398      }
3399      /// It is a debug assertion if the given vector is not normalized.
3400      ///
3401      /// In debug builds, checks if the magnitude is approximately one.
3402      // TODO: move broken doc tests to unit tests
3403      // ```should_panic
3404      // # use math_utils::$unit;
3405      // let n = $unit::unchecked ([2.0, 0.0].into());  // panic!
3406      // ```
3407      // TODO: implement required methods to avoid num::real::Real constraint
3408      #[inline]
3409      pub fn unchecked_approx (vector : $vector <S>) -> Self where
3410        S : num::real::Real + approx::RelativeEq <Epsilon=S>
3411      {
3412        debug_assert!(vector.is_normalized());
3413        $unit (vector)
3414      }
3415      /// Generate a random unit vector. Uses naive algorithm.
3416      pub fn random_unit <R : rand::RngExt> (rng : &mut R) -> Self where
3417        S : OrderedField + Sqrt + rand::distr::uniform::SampleUniform
3418      {
3419        let vector = $vector {
3420          $($component: rng.random_range (-S::one()..S::one())),+
3421        };
3422        vector.normalize()
3423      }
3424      /// Return the unit vector pointing in the opposite direction
3425      // TODO: implement num::Inv ?
3426      pub fn invert (mut self) -> Self where S : Ring {
3427        self.0 = -self.0;
3428        self
3429      }
3430      /// Map an operation on the underlying vector, panicking if the result is zero.
3431      ///
3432      /// # Panics
3433      ///
3434      /// Panics of the result is not normalized.
3435      // TODO: move broken doc tests to unit tests
3436      // ```should_panic
3437      // # use math_utils::$unit;
3438      // // panic!
3439      // let v = $unit::noisy ([1.0, 0.0].into()).map_noisy (|x| 2.0 * x);
3440      // ```
3441      #[must_use]
3442      pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3443        S : OrderedField + Sqrt + std::fmt::Debug
3444      {
3445        Self::noisy (fun (self.0))
3446      }
3447      /// Map an operation on the underlying vector, panicking if the result is zero.
3448      ///
3449      /// # Panics
3450      ///
3451      /// Panics of the result is not normalized.
3452      // TODO: move broken doc tests to unit tests
3453      // ```should_panic
3454      // # use math_utils::$unit;
3455      // // panic!
3456      // let v = $unit::noisy ([1.0, 0.0].into()).map_noisy (|x| 2.0 * x);
3457      // ```
3458      #[must_use]
3459      pub fn map_noisy_approx (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3460        S : num::real::Real + approx::RelativeEq <Epsilon=S>
3461      {
3462        Self::noisy_approx (fun (self.0))
3463      }
3464    }
3465    impl <S> std::ops::Deref for $unit <S> {
3466      type Target = $vector <S>;
3467      fn deref (&self) -> &$vector <S> {
3468        &self.0
3469      }
3470    }
3471    impl <S : Ring> std::ops::Neg for $unit <S> {
3472      type Output = Self;
3473      fn neg (self) -> Self {
3474        $unit (-self.0)
3475      }
3476    }
3477    //  end UnitN
3478  }
3479}
3480
3481macro_rules! impl_numcast {
3482  ($type:ident) => {
3483    impl <S> $type <S> {
3484      #[inline]
3485      pub fn numcast <T> (self) -> Option <$type <T>> where
3486        S : num::NumCast,
3487        T : num::NumCast
3488      {
3489        self.0.numcast().map ($type)
3490      }
3491    }
3492  }
3493}
3494
3495macro_rules! impl_numcast_primitive {
3496  ($type:ident) => {
3497    impl <S> $type <S> {
3498      #[inline]
3499      pub fn numcast <T> (self) -> Option <$type <T>> where
3500        S : num::NumCast,
3501        T : num::NumCast
3502      {
3503        T::from (self.0).map ($type)
3504      }
3505    }
3506    impl <S> num::ToPrimitive for $type <S> where S : num::ToPrimitive {
3507      fn to_isize (&self) -> Option <isize> {
3508        self.0.to_isize()
3509      }
3510      fn to_i8 (&self) -> Option <i8> {
3511        self.0.to_i8()
3512      }
3513      fn to_i16 (&self) -> Option <i16> {
3514        self.0.to_i16()
3515      }
3516      fn to_i32 (&self) -> Option <i32> {
3517        self.0.to_i32()
3518      }
3519      fn to_i64 (&self) -> Option <i64> {
3520        self.0.to_i64()
3521      }
3522      fn to_usize (&self) -> Option <usize> {
3523        self.0.to_usize()
3524      }
3525      fn to_u8 (&self) -> Option <u8> {
3526        self.0.to_u8()
3527      }
3528      fn to_u16 (&self) -> Option <u16> {
3529        self.0.to_u16()
3530      }
3531      fn to_u32 (&self) -> Option <u32> {
3532        self.0.to_u32()
3533      }
3534      fn to_u64 (&self) -> Option <u64> {
3535        self.0.to_u64()
3536      }
3537      fn to_f32 (&self) -> Option <f32> {
3538        self.0.to_f32()
3539      }
3540      fn to_f64 (&self) -> Option <f64> {
3541        self.0.to_f64()
3542      }
3543    }
3544  }
3545}
3546
3547macro_rules! impl_numcast_primitive_option {
3548  ($type:ident$(, $bound:path)*) => {
3549    impl_numcast_primitive!{$type}
3550    impl <S> num::NumCast for $type <S> where
3551      S : num::NumCast $(+ $bound)*
3552    {
3553      fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3554        S::from (n).and_then ($type::new)
3555      }
3556    }
3557  }
3558}
3559
3560macro_rules! impl_numcast_primitive_map {
3561  ($type:ident) => {
3562    impl_numcast_primitive!{$type}
3563    impl <S> num::NumCast for $type <S> where S : num::NumCast {
3564      fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3565        S::from (n).map ($type)
3566      }
3567    }
3568  }
3569}
3570
3571#[doc(hidden)]
3572#[macro_export]
3573macro_rules! impl_numcast_fields {
3574  ($type:ident, $($field:ident),+) => {
3575    impl <S> $type <S> {
3576      #[inline]
3577      pub fn numcast <T> (self) -> Option <$type <T>> where
3578        S : num::NumCast + PartialOrd + num::Zero,
3579        T : num::NumCast + PartialOrd + num::Zero
3580      {
3581        Some ($type {
3582          $($field: self.$field.numcast()?),+
3583        })
3584      }
3585    }
3586  }
3587}
3588
3589impl_angle!(Deg, "Degrees");
3590impl_angle!(Rad, "Radians");
3591impl_angle!(Turn, "Turns");
3592impl_angle_wrapped!(AngleWrapped, "Unsigned wrapped angle restricted to $[0, 2\\pi)$");
3593impl_angle_wrapped!(AngleWrappedSigned,
3594  "Signed wrapped angle restricted to $(-\\pi, \\pi]$");
3595
3596impl_dimension!(Point2, Vector2, NonZero2, Unit2, point2, vector2, matrix2, ortho_vec2,
3597  [x, y], [(axis_x, unit_x), (axis_y, unit_y)],
3598  [], [Point3], Matrix2, [], 2, "2D");
3599impl_dimension!(Point3, Vector3, NonZero3, Unit3, point3, vector3, matrix3, ortho_vec3,
3600  [x, y, z], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z)],
3601  [Point2], [Point4], Matrix3, [Matrix2], 3, "3D");
3602impl_dimension!(Point4, Vector4, NonZero4, Unit4, point4, vector4, matrix4, ortho_vec4,
3603  [x, y, z, w], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z), (axis_w, unit_w)],
3604  [Point3], [], Matrix4, [Matrix3], 4, "4D");
3605
3606impl_numcast_primitive_option!(Positive, PartialOrd, num::Zero);
3607impl_numcast_primitive_option!(NonNegative, PartialOrd, num::Zero);
3608impl_numcast_primitive_option!(NonZero, PartialEq, num::Zero);
3609impl_numcast_primitive_option!(Normalized, PartialOrd, num::One, num::Zero);
3610impl_numcast_primitive_option!(NormalSigned, OrderedRing);
3611impl_numcast_fields!(Angles3, yaw, pitch, roll);
3612
3613//
3614//  private
3615//
3616#[inline]
3617fn normalize_quaternion <S : Real> (quaternion : Quaternion <S>) -> Quaternion <S> {
3618  Quaternion::from_vec4 (*quaternion.into_vec4().normalize())
3619}
3620
3621#[cfg(test)]
3622mod tests {
3623  extern crate test;
3624
3625  use super::*;
3626  use crate::num;
3627  use approx;
3628  use rand;
3629  use rand_distr;
3630  use rand_xorshift;
3631
3632  #[expect(clippy::cast_possible_truncation)]
3633  static ORTHO_VEC_BENCH_SEED : std::sync::LazyLock <u64> = std::sync::LazyLock::new (
3634    || std::time::SystemTime::UNIX_EPOCH.elapsed().unwrap().as_nanos() as u64);
3635
3636  #[test]
3637  fn defaults() {
3638    use num::Zero;
3639
3640    assert_eq!(Positive::<f32>::default().0, 0.0);
3641    assert_eq!(NonNegative::<f32>::default().0, 0.0);
3642    assert_eq!(Normalized::<f32>::default().0, 0.0);
3643    assert_eq!(NormalSigned::<f32>::default().0, 0.0);
3644    assert_eq!(Deg::<f32>::default().0, 0.0);
3645    assert_eq!(Rad::<f32>::default().0, 0.0);
3646    assert_eq!(Turn::<f32>::default().0, 0.0);
3647    assert_eq!(
3648      Angles3::<f32>::default(),
3649      Angles3::new (AngleWrapped::zero(), AngleWrapped::zero(), AngleWrapped::zero()));
3650    assert_eq!(
3651      Pose3::<f32>::default(),
3652      Pose3 { position: Point3::origin(), angles: Angles3::default() });
3653    assert_eq!(
3654      LinearIso::<f32, Vector3 <f32>, Vector3 <f32>, Matrix3 <f32>>::default().0,
3655      Matrix3::identity());
3656    assert_eq!(LinearAuto::<f32, Vector3 <f32>>::default().0.0, Matrix3::identity());
3657    assert_eq!(Rotation2::<f32>::default().0.0.0, Matrix2::identity());
3658    assert_eq!(Rotation3::<f32>::default().0.0.0, Matrix3::identity());
3659    assert_eq!(Versor::<f32>::default().0, Quaternion::from_xyzw (0.0, 0.0, 0.0, 1.0));
3660    assert_eq!(
3661      AffineMap::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3662      AffineMap::new (Matrix3::identity(), Vector3::zero()));
3663    assert_eq!(
3664      Affinity::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3665      Affinity::new (LinearIso::new (Matrix3::identity()).unwrap(), Vector3::zero()));
3666    assert_eq!(
3667      Projectivity::<f32, Point4 <f32>, Point4 <f32>, Matrix4 <f32>>::default().0,
3668      LinearIso::new (Matrix4::identity()).unwrap());
3669  }
3670
3671  #[test]
3672  fn numcast() {
3673    let p : Positive <f32> = Positive::<f64>::noisy (0.25).numcast().unwrap();
3674    assert_eq!(p, Positive::noisy (0.25f32));
3675    let v : Vector3 <Positive <f64>> = [
3676      Positive::noisy (1.0),
3677      Positive::noisy (2.0),
3678      Positive::noisy (0.25)
3679    ].into();
3680    let _v : Vector3 <Positive <f32>> = v.numcast().unwrap();
3681  }
3682
3683  #[test]
3684  fn vector_sum() {
3685    let s = [
3686      [0.0, 1.0],
3687      [1.0, 1.0],
3688      [0.5, 0.5]
3689    ].map (Vector2::<f32>::from);
3690    let _sum = s.iter().copied().sum::<Vector2 <f32>>();
3691  }
3692
3693  #[test]
3694  fn vector_outer_product() {
3695    let v1 = vector3 (1.0, 2.0, 3.0);
3696    let v2 = vector3 (2.0, 3.0, 4.0);
3697    assert_eq!(v1.outer_product (v2).into_row_arrays(), [
3698      [2.0, 3.0,  4.0],
3699      [4.0, 6.0,  8.0],
3700      [6.0, 9.0, 12.0]
3701    ]);
3702  }
3703
3704  #[test]
3705  fn rotation3_noisy_approx() {
3706    use rand::{RngExt, SeedableRng};
3707    // construct orthonormal matrices and verify they are orthonormal using noisy_approx
3708    // constructor
3709    let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3710    let up = Vector3::unit_z();
3711    for _ in 0..1000 {
3712      let forward = vector3 (
3713        rng.random_range (-1000.0f32..1000.0),
3714        rng.random_range (-1000.0f32..1000.0),
3715        0.0
3716      ).normalized();
3717      let right = forward.cross (up);
3718      let mat = Matrix3::from_col_arrays ([
3719        right.into_array(),
3720        forward.into_array(),
3721        up.into_array()
3722      ]);
3723      let _ = Rotation3::noisy_approx (mat);
3724    }
3725  }
3726
3727  #[test]
3728  fn rotation3_intrinsic_angles() {
3729    use std::f32::consts::PI;
3730    use num::Zero;
3731    use rand::{RngExt, SeedableRng};
3732    // yaw: CCW quarter turn around Z (world up) axis
3733    let rot = Rotation3::from_angles_intrinsic (
3734      Turn (0.25).into(), Rad::zero(), Rad::zero());
3735    approx::assert_relative_eq!(rot.mat(),
3736      Matrix3::from_col_arrays ([
3737        [ 0.0, 1.0, 0.0],
3738        [-1.0, 0.0, 0.0],
3739        [ 0.0, 0.0, 1.0]
3740      ])
3741    );
3742    assert_eq!((Turn (0.25).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3743    let rot = Rotation3::from_angles_intrinsic (
3744      Turn (0.5).into(), Rad::zero(), Rad::zero());
3745    approx::assert_relative_eq!(rot.mat(),
3746      Matrix3::from_col_arrays ([
3747        [-1.0,  0.0, 0.0],
3748        [ 0.0, -1.0, 0.0],
3749        [ 0.0,  0.0, 1.0]
3750      ])
3751    );
3752    assert_eq!((Turn (0.5).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3753    // pitch: CCW quarter turn around X (world right) axis
3754    let rot = Rotation3::from_angles_intrinsic (
3755      Rad::zero(), Turn (0.25).into(), Rad::zero());
3756    approx::assert_relative_eq!(rot.mat(),
3757      Matrix3::from_col_arrays ([
3758        [1.0,  0.0, 0.0],
3759        [0.0,  0.0, 1.0],
3760        [0.0, -1.0, 0.0]
3761      ])
3762    );
3763    assert_eq!((Rad::zero(), Turn (0.25).into(), Rad::zero()), rot.intrinsic_angles());
3764    // roll: CCW quarter turn around Y (world forward) axis
3765    let rot = Rotation3::from_angles_intrinsic (
3766      Rad::zero(), Rad::zero(), Turn (0.25).into());
3767    approx::assert_relative_eq!(rot.mat(),
3768      Matrix3::from_col_arrays ([
3769        [0.0, 0.0, -1.0],
3770        [0.0, 1.0,  0.0],
3771        [1.0, 0.0,  0.0]
3772      ])
3773    );
3774    assert_eq!((Rad::zero(), Rad::zero(), Turn (0.25).into()), rot.intrinsic_angles());
3775    // pitch after yaw
3776    let rot = Rotation3::from_angles_intrinsic (
3777      Turn (0.25).into(), Turn (0.25).into(), Rad::zero());
3778    approx::assert_relative_eq!(rot.mat(),
3779      Matrix3::from_col_arrays ([
3780        [0.0, 1.0, 0.0],
3781        [0.0, 0.0, 1.0],
3782        [1.0, 0.0, 0.0]
3783      ])
3784    );
3785    let angles = rot.intrinsic_angles();
3786    approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3787    approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3788    approx::assert_relative_eq!(0.0, angles.2.0);
3789    // roll after pitch after yaw
3790    let rot = Rotation3::from_angles_intrinsic (
3791      Turn (0.25).into(), Turn (0.25).into(), Turn (0.25).into());
3792    approx::assert_relative_eq!(rot.mat(),
3793      Matrix3::from_col_arrays ([
3794        [-1.0, 0.0, 0.0],
3795        [ 0.0, 0.0, 1.0],
3796        [ 0.0, 1.0, 0.0]
3797      ])
3798    );
3799    let angles = rot.intrinsic_angles();
3800    approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3801    approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3802    approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.2.0);
3803
3804    // construct random orthonormal matrices and verify the intrinsic angles are in the
3805    // range [-pi, pi]
3806    let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3807    let mut random_vector = || vector3 (
3808      rng.random_range (-100.0f32..100.0),
3809      rng.random_range (-100.0f32..100.0),
3810      rng.random_range (-100.0f32..100.0));
3811    for _ in 0..1000 {
3812      let rot = Rotation3::orthonormalize (
3813        random_vector(), random_vector(), random_vector()
3814      ).unwrap();
3815      let (yaw, pitch, roll) = rot.intrinsic_angles();
3816      assert!(yaw.0   <  PI);
3817      assert!(yaw.0   > -PI);
3818      assert!(pitch.0 <  PI);
3819      assert!(pitch.0 > -PI);
3820      assert!(roll.0  <  PI);
3821      assert!(roll.0  > -PI);
3822    }
3823  }
3824
3825  #[test]
3826  fn rotation3_into_versor() {
3827    use std::f32::consts::PI;
3828    let rot  = Rotation3::from_angle_x (Rad (0.01));
3829    let quat = rot.versor().0;
3830    approx::assert_relative_eq!(rot.mat(), quat.into());
3831    let rot  = Rotation3::from_angle_x (Rad (-0.01));
3832    let quat = rot.versor().0;
3833    approx::assert_relative_eq!(rot.mat(), quat.into());
3834
3835    let rot  = Rotation3::from_angle_y (Rad (0.01));
3836    let quat = rot.versor().0;
3837    approx::assert_relative_eq!(rot.mat(), quat.into());
3838    let rot  = Rotation3::from_angle_y (Rad (-0.01));
3839    let quat = rot.versor().0;
3840    approx::assert_relative_eq!(rot.mat(), quat.into());
3841
3842    let rot  = Rotation3::from_angle_z (Rad (0.01));
3843    let quat = rot.versor().0;
3844    approx::assert_relative_eq!(rot.mat(), quat.into());
3845    let rot  = Rotation3::from_angle_z (Rad (-0.01));
3846    let quat = rot.versor().0;
3847    approx::assert_relative_eq!(rot.mat(), quat.into());
3848
3849    let rot  = Rotation3::from_angle_x (Rad (PI / 2.0));
3850    let quat = rot.versor().0;
3851    approx::assert_relative_eq!(rot.mat(), quat.into());
3852    let rot  = Rotation3::from_angle_x (Rad (-PI / 2.0));
3853    let quat = rot.versor().0;
3854    approx::assert_relative_eq!(rot.mat(), quat.into());
3855
3856    let rot  = Rotation3::from_angle_y (Rad (PI / 2.0));
3857    let quat = rot.versor().0;
3858    approx::assert_relative_eq!(rot.mat(), quat.into());
3859    let rot  = Rotation3::from_angle_y (Rad (-PI / 2.0));
3860    let quat = rot.versor().0;
3861    approx::assert_relative_eq!(rot.mat(), quat.into());
3862
3863    let rot  = Rotation3::from_angle_z (Rad (PI / 2.0));
3864    let quat = rot.versor().0;
3865    approx::assert_relative_eq!(rot.mat(), quat.into());
3866    let rot  = Rotation3::from_angle_z (Rad (-PI / 2.0));
3867    let quat = rot.versor().0;
3868    approx::assert_relative_eq!(rot.mat(), quat.into());
3869  }
3870
3871  // this method bencharked faster than the sign method but slower than the copysign
3872  // method (by about 5-10%); the copysign method is branch-free but relies on
3873  // floating-point copysign method
3874  #[bench]
3875  fn bench_orthogonal_cross_product (b : &mut test::Bencher) {
3876    use approx::AbsDiffEq;
3877    use rand::{RngExt, SeedableRng};
3878    use rand_distr::Distribution;
3879    let epsilon = f64::default_epsilon() * 256.0;
3880    let std_normal = rand_distr::StandardNormal;
3881    println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3882    let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3883    let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3884      let x : f64 = std_normal.sample (rng);
3885      let y : f64 = std_normal.sample (rng);
3886      x / y
3887    };
3888    b.iter(||{
3889      let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3890        0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3891        5     => vector3 (randf (&mut rng), 0.0, 0.0),
3892        6     => vector3 (0.0, randf (&mut rng), 0.0),
3893        7     => vector3 (0.0, 0.0, randf (&mut rng)),
3894        8     => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3895        9     => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3896        10    => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3897        _     => unreachable!()
3898      };
3899      approx::assert_abs_diff_ne!(vec, Vector3::zero(), epsilon = epsilon);
3900      let mut ortho = vec.cross (Vector3::unit_x());
3901      if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) {
3902        ortho = vec.cross (Vector3::unit_y())
3903      }
3904      if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3905        approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3906      {
3907        println!("VEC:   {vec}");
3908        println!("ORTHO: {ortho}");
3909        println!("DOT:   {}", vec.dot (ortho));
3910      }
3911      approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3912      approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3913    });
3914  }
3915
3916  // method by Ken Whatmough: https://math.stackexchange.com/a/4112622
3917  // this method turned out to be the fastest in benchmarks, however it relies on the
3918  // floating-point copysign function
3919  #[bench]
3920  fn bench_orthogonal_copysign (b : &mut test::Bencher) {
3921    use approx::AbsDiffEq;
3922    use rand::{RngExt, SeedableRng};
3923    use rand_distr::Distribution;
3924    let epsilon = f64::default_epsilon() * 2.0.powi (32);
3925    let std_normal = rand_distr::StandardNormal;
3926    println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3927    let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3928    let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3929      let x : f64 = std_normal.sample (rng);
3930      let y : f64 = std_normal.sample (rng);
3931      x / y
3932    };
3933    b.iter(||{
3934      let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3935        0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3936        5     => vector3 (randf (&mut rng), 0.0, 0.0),
3937        6     => vector3 (0.0, randf (&mut rng), 0.0),
3938        7     => vector3 (0.0, 0.0, randf (&mut rng)),
3939        8     => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3940        9     => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3941        10    => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3942        _     => unreachable!()
3943      };
3944      if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3945        return
3946      }
3947      let ortho = vector3 (
3948        vec.z.copysign (vec.x),
3949        vec.z.copysign (vec.y),
3950        // the following two lines should be equivalent
3951        -vec.x.copysign (vec.z) - vec.y.copysign (vec.z)
3952        //-(vec.x.abs() + vec.y.abs()).copysign (vec.z)
3953      );
3954      if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3955        approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3956      {
3957        println!("VEC:   {vec}");
3958        println!("ORTHO: {ortho}");
3959        println!("DOT:   {:.}", vec.dot (ortho));
3960      }
3961      approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3962      approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3963    });
3964  }
3965
3966  // method by Ken Whatmough: https://math.stackexchange.com/a/4112622
3967  // after running benchmarks this method is slower than both the copysign and cross
3968  // product methods, but it doesn't rely on floating-point specific copysign method
3969  #[bench]
3970  fn bench_orthogonal_sign (b : &mut test::Bencher) {
3971    use approx::AbsDiffEq;
3972    use rand::{RngExt, SeedableRng};
3973    use rand_distr::Distribution;
3974    let epsilon = f64::default_epsilon() * 2.0.powi (32);
3975    let std_normal = rand_distr::StandardNormal;
3976    println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3977    let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3978    let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3979      let x : f64 = std_normal.sample (rng);
3980      let y : f64 = std_normal.sample (rng);
3981      x / y
3982    };
3983    b.iter(||{
3984      let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3985        0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3986        5     => vector3 (randf (&mut rng), 0.0, 0.0),
3987        6     => vector3 (0.0, randf (&mut rng), 0.0),
3988        7     => vector3 (0.0, 0.0, randf (&mut rng)),
3989        8     => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3990        9     => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3991        10    => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3992        _     => unreachable!()
3993      };
3994      if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3995        return
3996      }
3997      let sign = |n : f64| if n == 0.0 {
3998        0.0
3999      } else if n > 0.0 {
4000        1.0
4001      } else {
4002        -1.0
4003      };
4004      let signfn = |n : f64| sign ((sign (n) + 0.5) * (sign (vec.z) + 0.5));
4005      let ortho = vector3 (
4006        signfn (vec.x) * vec.z,
4007        signfn (vec.y) * vec.z,
4008        (-signfn (vec.x)).mul_add (vec.x, -signfn (vec.y) * vec.y)
4009        //-signfn (vec.x) * vec.x - signfn (vec.y) * vec.y
4010      );
4011      if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
4012        approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
4013      {
4014        println!("VEC:   {vec}");
4015        println!("ORTHO: {ortho}");
4016        println!("DOT:   {:.}", vec.dot (ortho));
4017      }
4018      approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
4019      approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
4020    });
4021  }
4022}