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
25mod enums {
32 use strum::{EnumCount, EnumIter, FromRepr};
33 #[cfg(feature = "derive_serdes")]
34 use serde::{Deserialize, Serialize};
35
36 #[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 #[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 PosPos,
53 NegPos,
55 PosNeg,
57 NegNeg
59 }
60
61 #[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 PosPosPos,
68 NegPosPos,
70 PosNegPos,
72 NegNegPos,
74 PosPosNeg,
76 NegPosNeg,
78 PosNegNeg,
80 NegNegNeg
82 }
83
84 #[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 #[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 #[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 #[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 #[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 #[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 #[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#[derive(Debug, PartialEq, Eq)]
159pub struct Addition;
160
161#[derive(Debug, PartialEq, Eq)]
163pub struct Multiplication;
164
165#[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#[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#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
177#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
178pub struct NonZero <S> (S);
179
180#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
182#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
183pub struct Normalized <S> (S);
184
185#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
187#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
188pub struct NormalSigned <S> (S);
189
190#[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#[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#[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#[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#[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#[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#[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#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
247#[derive(Clone, Copy, Debug, Eq, PartialEq)]
248pub struct Versor <S> (Quaternion <S>);
249
250#[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#[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#[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
290impl <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
313impl <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
332impl <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
382impl <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
429impl <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
478impl <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
492impl <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
506impl <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
548impl Axis2 {
552 #[inline]
553 pub const fn component (self) -> usize {
554 self as usize
555 }
556}
557
558impl Axis3 {
562 #[inline]
563 pub const fn component (self) -> usize {
564 self as usize
565 }
566}
567
568impl 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
609impl Quadrant {
613 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 #[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}
637impl Octant {
643 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 #[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}
674impl <S> Positive <S> where S : PartialOrd + num::Zero {
680 pub fn new (value : S) -> Option <Self> {
691 if value > S::zero() {
692 Some (Positive (value))
693 } else {
694 None
695 }
696 }
697 pub fn noisy (value : S) -> Self {
706 assert!(value > S::zero());
707 Positive (value)
708 }
709 pub fn unchecked (value : S) -> Self {
718 debug_assert!(value > S::zero());
719 Positive (value)
720 }
721 #[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 pub fn infinity() -> Self {
749 Positive (S::infinity())
750 }
751}
752impl Positive <f32> {
753 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 pub const fn noisy_f32 (value : f32) -> Self {
763 assert!(0.0 < value);
764 Positive (value)
765 }
766 pub const fn unchecked_f32 (value : f32) -> Self {
768 debug_assert!(0.0 < value);
769 Positive (value)
770 }
771}
772impl Positive <f64> {
773 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 pub const fn noisy_f64 (value : f64) -> Self {
783 assert!(0.0 < value);
784 Positive (value)
785 }
786 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}
858impl <S> NonNegative <S> where S : PartialOrd + num::Zero {
864 pub fn new (value : S) -> Option <Self> {
875 if value >= S::zero() {
876 Some (NonNegative (value))
877 } else {
878 None
879 }
880 }
881 pub fn noisy (value : S) -> Self {
890 assert!(S::zero() <= value);
891 NonNegative (value)
892 }
893 pub fn unchecked (value : S) -> Self {
903 debug_assert!(value >= S::zero());
904 NonNegative (value)
905 }
906 #[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 pub fn abs (value : S) -> Self {
939 NonNegative (value.abs())
940 }
941 #[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 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 pub const fn noisy_f32 (value : f32) -> Self {
982 assert!(0.0 <= value);
983 NonNegative (value)
984 }
985 pub const fn unchecked_f32 (value : f32) -> Self {
987 debug_assert!(0.0 <= value);
988 NonNegative (value)
989 }
990}
991impl NonNegative <f64> {
992 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 pub const fn noisy_f64 (value : f64) -> Self {
1002 assert!(0.0 <= value);
1003 NonNegative (value)
1004 }
1005 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> std::ops::Mul for NonNegative <S> where S : std::ops::Mul <Output=S> {
1036 type Output = Self;
1037 fn mul (self, rhs : Self) -> Self::Output {
1038 NonNegative (self.0 * rhs.0)
1039 }
1040}
1041impl <S> std::ops::Mul <Positive <S>> for NonNegative <S> where
1042 S : std::ops::Mul <Output=S>
1043{
1044 type Output = Self;
1045 fn mul (self, rhs : Positive <S>) -> Self::Output {
1046 NonNegative (self.0 * rhs.0)
1047 }
1048}
1049impl <S> std::ops::Div for NonNegative <S> where S : std::ops::Div <Output=S> {
1050 type Output = Self;
1051 fn div (self, rhs : Self) -> Self::Output {
1052 NonNegative (self.0 / rhs.0)
1053 }
1054}
1055impl <S> std::ops::Add for NonNegative <S> where S : std::ops::Add <Output=S> {
1056 type Output = Self;
1057 fn add (self, rhs : Self) -> Self::Output {
1058 NonNegative (self.0 + rhs.0)
1059 }
1060}
1061impl <S> NonZero <S> where S : PartialEq + num::Zero {
1067 #[inline]
1078 pub fn new (value : S) -> Option <Self> {
1079 if S::zero() != value {
1080 Some (NonZero (value))
1081 } else {
1082 None
1083 }
1084 }
1085 #[inline]
1094 pub fn noisy (value : S) -> Self where S : std::fmt::Debug {
1095 assert_ne!(S::zero(), value);
1096 NonZero (value)
1097 }
1098 #[inline]
1107 pub fn unchecked (value : S) -> Self where S : std::fmt::Debug {
1108 debug_assert_ne!(S::zero(), value);
1109 NonZero (value)
1110 }
1111 #[inline]
1131 #[must_use]
1132 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1133 S : std::fmt::Debug,
1134 F : FnMut (S) -> S
1135 {
1136 Self::noisy (fun (self.0))
1137 }
1138}
1139impl NonZero <f32> {
1140 pub const fn new_f32 (value : f32) -> Option <Self> {
1142 if value != 0.0 {
1143 Some (NonZero (value))
1144 } else {
1145 None
1146 }
1147 }
1148 pub const fn noisy_f32 (value : f32) -> Self {
1150 assert!(value != 0.0);
1151 NonZero (value)
1152 }
1153 pub const fn unchecked_f32 (value : f32) -> Self {
1155 debug_assert!(value != 0.0);
1156 NonZero (value)
1157 }
1158}
1159impl NonZero <f64> {
1160 pub const fn new_f64 (value : f64) -> Option <Self> {
1162 if value != 0.0 {
1163 Some (NonZero (value))
1164 } else {
1165 None
1166 }
1167 }
1168 pub const fn noisy_f64 (value : f64) -> Self {
1170 assert!(value != 0.0);
1171 NonZero (value)
1172 }
1173 pub const fn unchecked_f64 (value : f64) -> Self {
1175 debug_assert!(value != 0.0);
1176 NonZero (value)
1177 }
1178}
1179impl <S : MultiplicativeMonoid> num::One for NonZero <S> {
1180 fn one() -> Self {
1181 NonZero (S::one())
1182 }
1183}
1184impl <S> std::ops::Deref for NonZero <S> {
1185 type Target = S;
1186 fn deref (&self) -> &S {
1187 &self.0
1188 }
1189}
1190impl <S : MultiplicativeMonoid> std::ops::Mul for NonZero <S> {
1191 type Output = Self;
1192 fn mul (self, rhs : Self) -> Self::Output {
1193 NonZero (self.0 * rhs.0)
1194 }
1195}
1196impl <S : PartialEq> Eq for NonZero <S> { }
1197impl <S> Normalized <S> where S : num::One + num::Zero {
1203 #[inline]
1205 pub fn zero() -> Self {
1206 Normalized (S::zero())
1207 }
1208 #[inline]
1209 pub fn is_zero (&self) -> bool {
1210 self.0.is_zero()
1211 }
1212 #[inline]
1226 pub fn new (value : S) -> Option <Self> where S : PartialOrd {
1227 if S::zero() <= value && value <= S::one() {
1228 Some (Normalized (value))
1229 } else {
1230 None
1231 }
1232 }
1233 #[inline]
1244 #[expect(clippy::same_name_method)]
1245 pub fn clamp (value : S) -> Self where S : MinMax {
1246 Normalized (value.clamp (S::zero(), S::one()))
1247 }
1248 #[inline]
1262 pub fn noisy (value : S) -> Self where S : PartialOrd {
1263 assert!(value <= S::one());
1264 assert!(S::zero() <= value);
1265 Normalized (value)
1266 }
1267 pub fn unchecked (value : S) -> Self where S : PartialOrd {
1277 debug_assert!(value <= S::one());
1278 debug_assert!(S::zero() <= value);
1279 Normalized (value)
1280 }
1281 #[inline]
1293 #[must_use]
1294 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1295 S : MinMax,
1296 F : FnMut (S) -> S
1297 {
1298 Self::clamp (fun (self.0))
1299 }
1300 #[inline]
1327 #[must_use]
1328 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1329 S : PartialOrd,
1330 F : FnMut (S) -> S
1331 {
1332 Self::noisy (fun (self.0))
1333 }
1334}
1335impl Normalized <f32> {
1336 pub const fn new_f32 (value : f32) -> Option <Self> {
1338 if 0.0 <= value && value <= 1.0 {
1339 Some (Normalized (value))
1340 } else {
1341 None
1342 }
1343 }
1344 pub const fn noisy_f32 (value : f32) -> Self {
1346 assert!(value <= 1.0);
1347 assert!(0.0 <= value);
1348 Normalized (value)
1349 }
1350 pub const fn unchecked_f32 (value : f32) -> Self {
1352 debug_assert!(value <= 1.0);
1353 debug_assert!(0.0 <= value);
1354 Normalized (value)
1355 }
1356}
1357impl Normalized <f64> {
1358 pub const fn new_f64 (value : f64) -> Option <Self> {
1360 if 0.0 <= value && value <= 1.0 {
1361 Some (Normalized (value))
1362 } else {
1363 None
1364 }
1365 }
1366 pub const fn noisy_f64 (value : f64) -> Self {
1368 assert!(value <= 1.0);
1369 assert!(0.0 <= value);
1370 Normalized (value)
1371 }
1372 pub const fn unchecked_f64 (value : f64) -> Self {
1374 debug_assert!(value <= 1.0);
1375 debug_assert!(0.0 <= value);
1376 Normalized (value)
1377 }
1378}
1379impl <S : Ring> num::One for Normalized <S> {
1380 fn one() -> Self {
1381 Normalized (S::one())
1382 }
1383}
1384impl <S> std::ops::Deref for Normalized <S> {
1385 type Target = S;
1386 fn deref (&self) -> &S {
1387 &self.0
1388 }
1389}
1390impl <S : Ring> std::ops::Mul for Normalized <S> {
1391 type Output = Self;
1392 fn mul (self, rhs : Self) -> Self::Output {
1393 Normalized (self.0 * rhs.0)
1394 }
1395}
1396impl <S : PartialEq> Eq for Normalized <S> { }
1397#[expect(clippy::derive_ord_xor_partial_ord)]
1398impl <S : PartialOrd> Ord for Normalized <S> {
1399 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1400 self.partial_cmp (other).unwrap()
1402 }
1403}
1404impl <S> NormalSigned <S> {
1410 #[inline]
1412 pub fn zero() -> Self where S : num::Zero {
1413 NormalSigned (S::zero())
1414 }
1415 #[inline]
1416 pub fn is_zero (&self) -> bool where S : num::Zero {
1417 self.0.is_zero()
1418 }
1419 #[inline]
1433 pub fn new (value : S) -> Option <Self> where S : OrderedRing {
1434 if -S::one() <= value && value <= S::one() {
1435 Some (NormalSigned (value))
1436 } else {
1437 None
1438 }
1439 }
1440 #[inline]
1451 #[expect(clippy::same_name_method)]
1452 pub fn clamp (value : S) -> Self where S : Ring + MinMax {
1453 NormalSigned (value.clamp (-S::one(), S::one()))
1454 }
1455 #[inline]
1469 pub fn noisy (value : S) -> Self where S : OrderedRing {
1470 assert!(value <= S::one());
1471 assert!(-S::one() <= value);
1472 NormalSigned (value)
1473 }
1474 #[inline]
1486 #[must_use]
1487 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1488 S : Ring + MinMax,
1489 F : FnMut (S) -> S
1490 {
1491 Self::clamp (fun (self.0))
1492 }
1493 #[inline]
1520 #[must_use]
1521 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1522 S : OrderedRing,
1523 F : FnMut (S) -> S
1524 {
1525 Self::noisy (fun (self.0))
1526 }
1527}
1528impl NormalSigned <f32> {
1529 pub const fn new_f32 (value : f32) -> Option <Self> {
1531 if -1.0 <= value && value <= 1.0 {
1532 Some (NormalSigned (value))
1533 } else {
1534 None
1535 }
1536 }
1537 pub const fn noisy_f32 (value : f32) -> Self {
1539 assert!(value <= 1.0);
1540 assert!(-1.0 <= value);
1541 NormalSigned (value)
1542 }
1543 pub const fn unchecked_f32 (value : f32) -> Self {
1545 debug_assert!(value <= 1.0);
1546 debug_assert!(-1.0 <= value);
1547 NormalSigned (value)
1548 }
1549}
1550impl NormalSigned <f64> {
1551 pub const fn new_f64 (value : f64) -> Option <Self> {
1553 if -1.0 <= value && value <= 1.0 {
1554 Some (NormalSigned (value))
1555 } else {
1556 None
1557 }
1558 }
1559 pub const fn noisy_f64 (value : f64) -> Self {
1561 assert!(value <= 1.0);
1562 assert!(-1.0 <= value);
1563 NormalSigned (value)
1564 }
1565 pub const fn unchecked_f64 (value : f64) -> Self {
1567 debug_assert!(value <= 1.0);
1568 debug_assert!(-1.0 <= value);
1569 NormalSigned (value)
1570 }
1571}
1572impl <S : OrderedField> From <Normalized <S>> for NormalSigned <S> {
1573 fn from (Normalized (value) : Normalized <S>) -> Self {
1574 debug_assert!(S::zero() <= value);
1575 debug_assert!(value <= S::one());
1576 NormalSigned (value)
1577 }
1578}
1579impl <S : Field> num::One for NormalSigned <S> {
1580 fn one() -> Self {
1581 NormalSigned (S::one())
1582 }
1583}
1584impl <S> std::ops::Deref for NormalSigned <S> {
1585 type Target = S;
1586 fn deref (&self) -> &S {
1587 &self.0
1588 }
1589}
1590impl <S : Field> std::ops::Mul for NormalSigned <S> {
1591 type Output = Self;
1592 fn mul (self, rhs : Self) -> Self::Output {
1593 NormalSigned (self.0 * rhs.0)
1594 }
1595}
1596impl <S : Field> Eq for NormalSigned <S> { }
1597#[expect(clippy::derive_ord_xor_partial_ord)]
1598impl <S : OrderedField> Ord for NormalSigned <S> {
1599 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1600 self.partial_cmp (other).unwrap()
1602 }
1603}
1604impl <S : Field> std::ops::Neg for NormalSigned <S> {
1605 type Output = Self;
1606 fn neg (self) -> Self {
1607 NormalSigned (-self.0)
1608 }
1609}
1610impl <S : Ring> Vector2Ext <S> for Vector2 <S> {
1616 fn exterior_product (self, rhs : Vector2 <S>) -> S {
1618 self.x * rhs.y - self.y * rhs.x
1619 }
1620}
1621fn ortho_vec2 <S> (vector : Vector2 <S>) -> Vector2 <S> {
1622 vector.yx()
1623}
1624
1625fn ortho_vec3 <S> (vector : Vector3 <S>) -> Vector3 <S> where
1629 S : Ring + approx::AbsDiffEq <Epsilon = S>
1630{
1631 let mut ortho = vector.cross (Vector3::unit_x());
1635 if approx::abs_diff_eq!(ortho, Vector3::zero()) {
1636 ortho = vector.cross (Vector3::unit_y())
1637 }
1638 ortho
1639}
1640
1641fn ortho_vec4 <S> (_vector : Vector4 <S>) -> Vector4 <S> {
1645 unimplemented!("TODO")
1646}
1647
1648impl <S> std::ops::Deref for Unit <S> {
1652 type Target = S;
1653 fn deref (&self) -> &S {
1654 &self.0
1655 }
1656}
1657
1658impl <S> Rotation2 <S> where S : Ring + MaybeSerDes {
1662 pub fn identity() -> Self {
1664 use num::One;
1665 Self::one()
1666 }
1667 pub fn new (mat : Matrix2 <S>) -> Option <Self> {
1672 let transform = LinearAuto (LinearIso::new (mat)?);
1673 if !transform.is_rotation() {
1674 None
1675 } else {
1676 Some (Rotation2 (transform))
1677 }
1678 }
1679 pub fn new_approx (mat : Matrix2 <S>) -> Option <Self> where
1685 S : approx::RelativeEq <Epsilon=S>
1686 {
1687 let four = S::one() + S::one() + S::one() + S::one();
1688 let thirtytwo = four * four * (S::one() + S::one());
1689 let epsilon = S::default_epsilon() * thirtytwo;
1690 let max_relative = S::default_max_relative() * four;
1691 if approx::relative_ne!(mat * mat.transposed(), Matrix2::identity(),
1692 max_relative=max_relative, epsilon=epsilon
1693 ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
1694 None
1695 } else {
1696 Some (Rotation2 (LinearAuto (LinearIso (mat, PhantomData))))
1697 }
1698 }
1699 pub fn from_angle (angle : Rad <S>) -> Self where S : num::real::Real {
1701 Rotation2 (LinearAuto (LinearIso (Matrix2::rotation_z (angle.0), PhantomData)))
1702 }
1703 pub fn noisy (mat : Matrix2 <S>) -> Self where S : std::fmt::Debug {
1707 assert_eq!(mat * mat.transposed(), Matrix2::identity());
1708 assert_eq!(mat.determinant(), S::one());
1709 Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1710 }
1711 pub fn noisy_approx (mat : Matrix2 <S>) -> Self where
1716 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1717 {
1718 let four = S::one() + S::one() + S::one() + S::one();
1719 let epsilon = S::default_epsilon() * four;
1720 let max_relative = S::default_max_relative() * four;
1721 approx::assert_relative_eq!(mat * mat.transposed(), Matrix2::identity(),
1722 epsilon=epsilon, max_relative=max_relative);
1723 approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
1724 Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1725 }
1726 pub fn unchecked (mat : Matrix2 <S>) -> Self where S : std::fmt::Debug {
1730 debug_assert!(mat * mat.transposed() == Matrix2::identity());
1731 debug_assert!(mat.determinant() == S::one());
1732 Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1733 }
1734 pub fn unchecked_approx (mat : Matrix2 <S>) -> Self where
1739 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1740 {
1741 if cfg!(debug_assertions) {
1742 let four = S::one() + S::one() + S::one() + S::one();
1743 let epsilon = S::default_epsilon() * four;
1744 let max_relative = S::default_max_relative() * four;
1745 approx::assert_relative_eq!(mat * mat.transposed(), Matrix2::identity(),
1746 max_relative=max_relative, epsilon=epsilon);
1747 approx::assert_relative_eq!(mat.determinant(), S::one(),
1748 max_relative=max_relative);
1749 }
1750 Rotation2 (LinearAuto (LinearIso (mat, PhantomData)))
1751 }
1752 pub const fn mat (self) -> Matrix2 <S> {
1754 self.0.0.0
1755 }
1756 pub fn rotate (self, point : Point2 <S>) -> Point2 <S> {
1758 Point2::from (self * point.0)
1759 }
1760 pub fn rotate_around (self, point : Point2 <S>, center : Point2 <S>) -> Point2 <S> {
1762 self.rotate (point - center.0) + center.0
1763 }
1764}
1765impl <S> std::ops::Deref for Rotation2 <S> where S : Ring + MaybeSerDes {
1766 type Target = LinearAuto <S, Vector2 <S>>;
1767 fn deref (&self) -> &Self::Target {
1768 &self.0
1769 }
1770}
1771impl <S> num::Inv for Rotation2 <S> where S : Ring + MaybeSerDes {
1772 type Output = Self;
1773 fn inv (self) -> Self::Output {
1774 Rotation2 (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
1775 }
1776}
1777impl <S> std::ops::Mul <Vector2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1778 type Output = Vector2 <S>;
1779 fn mul (self, rhs : Vector2 <S>) -> Self::Output {
1780 self.0 * rhs
1781 }
1782}
1783impl <S> std::ops::Mul <NonZero2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1784 type Output = NonZero2 <S>;
1785 fn mul (self, rhs : NonZero2 <S>) -> Self::Output {
1786 NonZero2 (self.0 * rhs.0)
1787 }
1788}
1789impl <S> std::ops::Mul <Unit2 <S>> for Rotation2 <S> where S : Ring + MaybeSerDes {
1790 type Output = Unit2 <S>;
1791 fn mul (self, rhs : Unit2 <S>) -> Self::Output {
1792 Unit2 (self.0 * rhs.0)
1793 }
1794}
1795impl <S> std::ops::Mul <Rotation2 <S>> for Vector2 <S> where S : Ring + MaybeSerDes {
1796 type Output = Vector2 <S>;
1797 fn mul (self, rhs : Rotation2 <S>) -> Self::Output {
1798 self * rhs.0
1799 }
1800}
1801impl <S> std::ops::Mul for Rotation2 <S> where S : Ring + MaybeSerDes {
1802 type Output = Self;
1803 fn mul (self, rhs : Self) -> Self::Output {
1804 Rotation2 (self.0 * rhs.0)
1805 }
1806}
1807impl <S> std::ops::MulAssign <Self> for Rotation2 <S> where S : Ring + MaybeSerDes {
1808 fn mul_assign (&mut self, rhs : Self) {
1809 self.0 *= rhs.0
1810 }
1811}
1812impl <S> std::ops::Div for Rotation2 <S> where S : Ring + MaybeSerDes {
1813 type Output = Self;
1814 #[expect(clippy::suspicious_arithmetic_impl)]
1815 fn div (self, rhs : Self) -> Self::Output {
1816 use num::Inv;
1817 self * rhs.inv()
1818 }
1819}
1820impl <S> std::ops::DivAssign <Self> for Rotation2 <S> where S : Ring + MaybeSerDes {
1821 #[expect(clippy::suspicious_op_assign_impl)]
1822 fn div_assign (&mut self, rhs : Self) {
1823 use num::Inv;
1824 *self *= rhs.inv()
1825 }
1826}
1827impl <S> num::One for Rotation2 <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
1828 fn one() -> Self {
1829 Rotation2 (LinearAuto (LinearIso (Matrix2::identity(), PhantomData)))
1830 }
1831}
1832
1833impl <S> Rotation3 <S> where S : Ring + MaybeSerDes {
1837 pub fn identity() -> Self {
1839 use num::One;
1840 Self::one()
1841 }
1842 pub fn new (mat : Matrix3 <S>) -> Option <Self> {
1847 if mat * mat.transposed() != Matrix3::identity() || mat.determinant() != S::one() {
1848 None
1849 } else {
1850 Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1851 }
1852 }
1853 pub fn new_approx (mat : Matrix3 <S>) -> Option <Self> where
1859 S : approx::RelativeEq <Epsilon=S>
1860 {
1861 let four = S::one() + S::one() + S::one() + S::one();
1862 let thirtytwo = four * four * (S::one() + S::one());
1863 let epsilon = S::default_epsilon() * thirtytwo;
1864 let max_relative = S::default_max_relative() * four;
1865 if approx::relative_ne!(mat * mat.transposed(), Matrix3::identity(),
1866 max_relative=max_relative, epsilon=epsilon
1867 ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
1868 None
1869 } else {
1870 Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1871 }
1872 }
1873 pub fn noisy (mat : Matrix3 <S>) -> Self {
1877 assert!(mat * mat.transposed() == Matrix3::identity());
1878 assert!(mat.determinant() == S::one());
1879 Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1880 }
1881 pub fn noisy_approx (mat : Matrix3 <S>) -> Self where
1886 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1887 {
1888 let four = S::one() + S::one() + S::one() + S::one();
1889 let eight = four + S::one() + S::one() + S::one() + S::one();
1890 let epsilon = S::default_epsilon() * four;
1891 let max_relative = S::default_max_relative() * eight;
1892 approx::assert_relative_eq!(mat * mat.transposed(), Matrix3::identity(),
1893 max_relative=max_relative, epsilon=epsilon);
1894 approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
1895 Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1896 }
1897 pub fn unchecked (mat : Matrix3 <S>) -> Self where S : std::fmt::Debug {
1901 debug_assert!(mat * mat.transposed() == Matrix3::identity());
1902 debug_assert!(mat.determinant() == S::one());
1903 Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1904 }
1905 pub fn unchecked_approx (mat : Matrix3 <S>) -> Self where
1910 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1911 {
1912 if cfg!(debug_assertions) {
1913 let four = S::one() + S::one() + S::one() + S::one();
1914 let epsilon = S::default_epsilon() * four;
1915 let max_relative = S::default_max_relative() * four;
1916 approx::assert_relative_eq!(mat * mat.transposed(), Matrix3::identity(),
1917 max_relative=max_relative, epsilon=epsilon);
1918 approx::assert_relative_eq!(mat.determinant(), S::one(),
1919 max_relative=max_relative);
1920 }
1921 Rotation3 (LinearAuto (LinearIso (mat, PhantomData)))
1922 }
1923 pub fn from_angle_x (angle : Rad <S>) -> Self where S : num::real::Real {
1927 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_x (angle.0), PhantomData)))
1928 }
1929 pub fn from_angle_y (angle : Rad <S>) -> Self where S : num::real::Real {
1933 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_y (angle.0), PhantomData)))
1934 }
1935 pub fn from_angle_z (angle : Rad <S>) -> Self where S : num::real::Real {
1939 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_z (angle.0), PhantomData)))
1940 }
1941 pub fn from_angles_intrinsic (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>)
1947 -> Self where S : num::real::Real
1948 {
1949 let rotation1 = Rotation3::from_angle_z (yaw);
1950 let rotation2 = Rotation3::from_axis_angle (rotation1.cols.x, pitch) * rotation1;
1951 Rotation3::from_axis_angle (rotation2.cols.y, roll) * rotation2
1952 }
1953 pub fn from_axis_angle (axis : Vector3 <S>, angle : Rad <S>) -> Self where
1955 S : num::real::Real
1956 {
1957 Rotation3 (LinearAuto (LinearIso (
1958 Matrix3::rotation_3d (angle.0, axis), PhantomData)))
1959 }
1960 pub fn orthonormalize (v1 : Vector3 <S>, v2 : Vector3 <S>, v3 : Vector3 <S>)
1963 -> Option <Self> where S : OrderedField + Sqrt
1964 {
1965 if Matrix3::from_col_arrays ([v1, v2, v3].map (Vector3::into_array)).determinant()
1966 == S::zero()
1967 {
1968 None
1969 } else {
1970 let project = |u : Vector3 <S>, v : Vector3 <S>| u * (u.dot (v) / *u.self_dot());
1971 let u1 = v1;
1972 let u2 = v2 - project (u1, v2);
1973 let u3 = v3 - project (u1, v3) - project (u2, v3);
1974 Some (Rotation3 (LinearAuto (LinearIso (Matrix3::from_col_arrays ([
1975 u1.normalize().into_array(),
1976 u2.normalize().into_array(),
1977 u3.normalize().into_array()
1978 ]), PhantomData))))
1979 }
1980 }
1981
1982 pub fn look_at (point : Point3 <S>) -> Self where
1986 S : num::Float + num::FloatConst + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1987 {
1988 if point == Point::origin() {
1989 Self::identity()
1990 } else {
1991 use num::Zero;
1992 let forward = point.0.normalized();
1993 let right = {
1994 let projected = forward.with_z (S::zero());
1995 if !projected.is_zero() {
1996 Self::from_angle_z (-Rad (S::FRAC_PI_2())) * projected.normalized()
1997 } else {
1998 Vector3::unit_x()
1999 }
2000 };
2001 let up = right.cross (forward);
2002 let mat =
2003 Matrix3::from_col_arrays ([right, forward, up].map (Vector3::into_array));
2004 Self::noisy_approx (mat)
2005 }
2006 }
2007
2008 pub fn intrinsic_angles (&self) -> (Rad <S>, Rad <S>, Rad <S>) where S : Real {
2013 let cols = &self.cols;
2014 let yaw = Rad (S::atan2 (-cols.y.x, cols.y.y));
2015 let pitch = Rad (S::atan2 (cols.y.z, S::sqrt (S::one() - cols.y.z * cols.y.z)));
2016 let roll = Rad (S::atan2 (-cols.x.z, cols.z.z));
2017 (yaw, pitch, roll)
2018 }
2019
2020 pub fn versor (self) -> Versor <S> where
2024 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
2025 {
2026 let diagonal = self.diagonal();
2027 let t = diagonal.sum();
2028 let m =
2029 MinMax::max (MinMax::max (MinMax::max (diagonal.x, diagonal.y), diagonal.z), t);
2030 let qmax = S::half() * Sqrt::sqrt (S::one() - t + S::two() * m);
2031 let qmax4_recip = (S::four() * qmax).recip();
2032 let [qx, qy, qz, qw] : [S; 4];
2033 let cols = self.cols;
2034 if m == diagonal.x {
2035 qx = qmax;
2036 qy = qmax4_recip * (cols.x.y + cols.y.x);
2037 qz = qmax4_recip * (cols.x.z + cols.z.x);
2038 qw = -qmax4_recip * (cols.z.y - cols.y.z);
2039 } else if m == diagonal.y {
2040 qx = qmax4_recip * (cols.x.y + cols.y.x);
2041 qy = qmax;
2042 qz = qmax4_recip * (cols.y.z + cols.z.y);
2043 qw = -qmax4_recip * (cols.x.z - cols.z.x);
2044 } else if m == diagonal.z {
2045 qx = qmax4_recip * (cols.x.z + cols.z.x);
2046 qy = qmax4_recip * (cols.y.z + cols.z.y);
2047 qz = qmax;
2048 qw = -qmax4_recip * (cols.y.x - cols.x.y);
2049 } else {
2050 qx = -qmax4_recip * (cols.z.y - cols.y.z);
2051 qy = -qmax4_recip * (cols.x.z - cols.z.x);
2052 qz = -qmax4_recip * (cols.y.x - cols.x.y);
2053 qw = qmax;
2054 }
2055 let quat = Quaternion::from_xyzw (qx, qy, qz, qw);
2056 Versor::unchecked_approx (quat)
2057 }
2058 pub const fn mat (self) -> Matrix3 <S> {
2060 self.0.0.0
2061 }
2062 pub fn rotate (self, point : Point3 <S>) -> Point3 <S> {
2064 Point3::from (self * point.0)
2065 }
2066 pub fn rotate_around (self, point : Point3 <S>, center : Point3 <S>) -> Point3 <S> {
2068 self.rotate (point - center.0) + center.0
2069 }
2070}
2071impl <S> std::ops::Deref for Rotation3 <S> where S : Ring + MaybeSerDes {
2072 type Target = LinearAuto <S, Vector3 <S>>;
2073 fn deref (&self) -> &Self::Target {
2074 &self.0
2075 }
2076}
2077impl <S> num::Inv for Rotation3 <S> where S : Ring + MaybeSerDes {
2078 type Output = Self;
2079 fn inv (self) -> Self::Output {
2080 Rotation3 (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
2081 }
2082}
2083impl <S> std::ops::Mul <Vector3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2084 type Output = Vector3 <S>;
2085 fn mul (self, rhs : Vector3 <S>) -> Self::Output {
2086 self.0 * rhs
2087 }
2088}
2089impl <S> std::ops::Mul <NonZero3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2090 type Output = NonZero3 <S>;
2091 fn mul (self, rhs : NonZero3 <S>) -> Self::Output {
2092 NonZero3 (self.0 * rhs.0)
2093 }
2094}
2095impl <S> std::ops::Mul <Unit3 <S>> for Rotation3 <S> where S : Ring + MaybeSerDes {
2096 type Output = Unit3 <S>;
2097 fn mul (self, rhs : Unit3 <S>) -> Self::Output {
2098 Unit3 (self.0 * rhs.0)
2099 }
2100}
2101impl <S> std::ops::Mul <Rotation3 <S>> for Vector3 <S> where S : Ring + MaybeSerDes {
2102 type Output = Vector3 <S>;
2103 fn mul (self, rhs : Rotation3 <S>) -> Self::Output {
2104 self * rhs.0
2105 }
2106}
2107impl <S> std::ops::Mul for Rotation3 <S> where S : Ring + MaybeSerDes {
2108 type Output = Self;
2109 fn mul (self, rhs : Self) -> Self::Output {
2110 Rotation3 (self.0 * rhs.0)
2111 }
2112}
2113impl <S> std::ops::MulAssign <Self> for Rotation3 <S> where S : Ring + MaybeSerDes {
2114 fn mul_assign (&mut self, rhs : Self) {
2115 self.0 *= rhs.0
2116 }
2117}
2118impl <S> std::ops::Div for Rotation3 <S> where S : Ring + MaybeSerDes {
2119 type Output = Self;
2120 #[expect(clippy::suspicious_arithmetic_impl)]
2121 fn div (self, rhs : Self) -> Self::Output {
2122 use num::Inv;
2123 self * rhs.inv()
2124 }
2125}
2126impl <S> std::ops::DivAssign <Self> for Rotation3 <S> where S : Ring + MaybeSerDes {
2127 #[expect(clippy::suspicious_op_assign_impl)]
2128 fn div_assign (&mut self, rhs : Self) {
2129 use num::Inv;
2130 *self *= rhs.inv()
2131 }
2132}
2133impl <S> num::One for Rotation3 <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
2134 fn one() -> Self {
2135 Rotation3 (LinearAuto (LinearIso (Matrix3::identity(), PhantomData)))
2136 }
2137}
2138impl <S> From <Angles3 <S>> for Rotation3 <S> where
2139 S : Real + num::real::Real + MaybeSerDes
2140{
2141 fn from (angles : Angles3 <S>) -> Self {
2142 Rotation3::from_angles_intrinsic (
2143 angles.yaw.angle(), angles.pitch.angle(), angles.roll.angle())
2144 }
2145}
2146
2147impl <S : Real> Versor <S> {
2151 pub fn normalize (quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
2155 assert_ne!(quaternion, Quaternion::zero());
2156 Versor (normalize_quaternion (quaternion))
2157 }
2158 pub fn noisy (unit_quaternion : Quaternion <S>) -> Self where
2162 S : num::real::Real + std::fmt::Debug
2163 {
2164 assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
2165 Versor (unit_quaternion)
2166 }
2167 pub fn noisy_approx (unit_quaternion : Quaternion <S>) -> Self where
2171 S : num::real::Real + approx::RelativeEq <Epsilon=S>
2172 {
2173 assert!(unit_quaternion.into_vec4().is_normalized());
2174 Versor (unit_quaternion)
2175 }
2176 pub fn unchecked (unit_quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
2180 debug_assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
2181 Versor (unit_quaternion)
2182 }
2183 pub fn unchecked_approx (unit_quaternion : Quaternion <S>) -> Self where
2187 S : num::real::Real + approx::RelativeEq <Epsilon=S>
2188 {
2189 debug_assert!(unit_quaternion.into_vec4().is_normalized());
2190 Versor (unit_quaternion)
2191 }
2192 pub fn from_scaled_axis (scaled_axis : Vector3 <S>) -> Self where
2195 S : std::fmt::Debug + num::Float
2196 {
2197 if scaled_axis == Vector3::zero() {
2198 let versor = Quaternion::rotation_3d (S::zero(), scaled_axis);
2199 debug_assert_eq!(versor.magnitude(), S::one());
2200 Versor (versor)
2201 } else {
2202 let angle = scaled_axis.magnitude();
2203 debug_assert!(S::zero() < angle);
2204 let axis = scaled_axis / angle;
2205 Versor (Quaternion::rotation_3d (angle, axis))
2206 }
2207 }
2208}
2209impl <S> Default for Versor <S> where S : num::One + num::Zero {
2210 fn default() -> Self {
2211 Versor (Quaternion::default())
2212 }
2213}
2214impl <S> std::ops::Mul <Vector3 <S>> for Versor <S> where S : Real + num::Float {
2215 type Output = Vector3 <S>;
2216 fn mul (self, rhs : Vector3 <S>) -> Self::Output {
2217 self.0 * rhs
2218 }
2219}
2220impl <S> From <Rotation3 <S>> for Versor <S> where
2221 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
2222{
2223 fn from (rot : Rotation3 <S>) -> Self {
2224 rot.versor()
2225 }
2226}
2227impl <S> std::ops::Deref for Versor <S> {
2228 type Target = Quaternion <S>;
2229 fn deref (&self) -> &Quaternion <S> {
2230 &self.0
2231 }
2232}
2233impl <S, V, W, M> LinearIso <S, V, W, M> where
2239 M : LinearMap <S, V, W> + Copy,
2240 V : Module <S>,
2241 W : Module <S>,
2242 S : Ring
2243{
2244 pub fn mat (self) -> M {
2245 *self
2246 }
2247 pub fn is_invertible (linear_map : M) -> bool {
2249 linear_map.determinant() != S::zero()
2250 }
2251 pub fn is_invertible_approx (linear_map : M, epsilon : Option <S>) -> bool where
2253 S : approx::AbsDiffEq <Epsilon=S>
2254 {
2255 let epsilon = epsilon.unwrap_or_else (||{
2256 let four = S::one() + S::one() + S::one() + S::one();
2257 S::default_epsilon() * four
2258 });
2259 approx::abs_diff_ne!(linear_map.determinant(), S::zero(), epsilon=epsilon)
2260 }
2261 pub fn new (linear_map : M) -> Option <Self> {
2263 if Self::is_invertible (linear_map) {
2264 Some (LinearIso (linear_map, PhantomData))
2265 } else {
2266 None
2267 }
2268 }
2269 pub fn new_approx (linear_map : M, epsilon : Option <S>) -> Option <Self> where
2271 S : approx::RelativeEq <Epsilon=S>
2272 {
2273 if Self::is_invertible_approx (linear_map, epsilon) {
2274 Some (LinearIso (linear_map, PhantomData))
2275 } else {
2276 None
2277 }
2278 }
2279}
2280impl <S, V, W, M> std::ops::Deref for LinearIso <S, V, W, M> where
2281 M : LinearMap <S, V, W>,
2282 V : Module <S>,
2283 W : Module <S>,
2284 S : Ring
2285{
2286 type Target = M;
2287 fn deref (&self) -> &Self::Target {
2288 &self.0
2289 }
2290}
2291impl <S, V> num::One for LinearIso <S, V, V, V::LinearEndo> where
2292 V : Module <S>,
2293 S : Ring
2294{
2295 fn one () -> Self {
2296 LinearIso (V::LinearEndo::one(), PhantomData)
2297 }
2298}
2299impl <S, V, W, M> std::ops::Mul <V> for LinearIso <S, V, W, M> where
2300 M : LinearMap <S, V, W>,
2301 V : Module <S>,
2302 W : Module <S>,
2303 S : Ring
2304{
2305 type Output = W;
2306 fn mul (self, rhs : V) -> Self::Output {
2307 self.0 * rhs
2308 }
2309}
2310impl <S, V> std::ops::Mul for LinearIso <S, V, V, V::LinearEndo> where
2311 V : Module <S>,
2312 S : Ring
2313{
2314 type Output = Self;
2315 fn mul (self, rhs : Self) -> Self::Output {
2316 LinearIso (self.0 * rhs.0, PhantomData)
2317 }
2318}
2319impl <S, V> std::ops::MulAssign for LinearIso <S, V, V, V::LinearEndo> where
2320 V : Module <S>,
2321 S : Ring
2322{
2323 fn mul_assign (&mut self, rhs : Self) {
2324 self.0 *= rhs.0
2325 }
2326}
2327
2328impl <S, V> LinearAuto <S, V> where
2332 V : Module <S>,
2333 V::LinearEndo : MaybeSerDes,
2334 S : Ring
2335{
2336 pub fn mat (self) -> V::LinearEndo {
2337 **self
2338 }
2339 pub fn is_orthonormal (&self) -> bool {
2344 use num::One;
2345 let determinant = self.0.0.determinant();
2346 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2347 (determinant == S::one() || determinant == -S::one())
2348 }
2349 pub fn is_orthonormal_approx (&self) -> bool where
2355 S : approx::RelativeEq <Epsilon=S>,
2356 V::LinearEndo : approx::RelativeEq <Epsilon=S>
2357 {
2358 use num::One;
2359 let four = S::one() + S::one() + S::one() + S::one();
2360 let epsilon = S::default_epsilon() * four;
2361 let max_relative = S::default_max_relative() * four;
2362 let determinant = self.0.0.determinant();
2363 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2364 max_relative=max_relative, epsilon=epsilon) &&
2365 ( approx::relative_eq!(determinant, S::one(), max_relative=max_relative) ||
2366 approx::relative_eq!(determinant, -S::one(), max_relative=max_relative) )
2367 }
2368 pub fn is_rotation (&self) -> bool {
2372 use num::One;
2373 let determinant = self.0.0.determinant();
2374 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2375 determinant == S::one()
2376 }
2377 pub fn is_rotation_approx (&self) -> bool where
2382 S : approx::RelativeEq <Epsilon=S>,
2383 V::LinearEndo : approx::RelativeEq <Epsilon=S>
2384 {
2385 use num::One;
2386 let four = S::one() + S::one() + S::one() + S::one();
2387 let epsilon = S::default_epsilon() * four;
2388 let max_relative = S::default_max_relative() * four;
2389 let determinant = self.0.0.determinant();
2390 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2391 max_relative=max_relative, epsilon=epsilon) &&
2392 approx::relative_eq!(determinant, S::one(), max_relative=max_relative)
2393 }
2394}
2395impl <S, V> std::ops::Mul <V> for LinearAuto <S, V> where
2396 V : Module <S>,
2397 V::LinearEndo : MaybeSerDes,
2398 S : Ring
2399{
2400 type Output = V;
2401 fn mul (self, rhs : V) -> Self::Output {
2402 self.0 * rhs
2403 }
2404}
2405impl <S, V> std::ops::Mul for LinearAuto <S, V> where
2406 V : Module <S>,
2407 V::LinearEndo : MaybeSerDes,
2408 S : Ring
2409{
2410 type Output = Self;
2411 fn mul (self, rhs : Self) -> Self::Output {
2412 LinearAuto (self.0 * rhs.0)
2413 }
2414}
2415impl <S, V> std::ops::MulAssign <Self> for LinearAuto <S, V> where
2416 V : Module <S>,
2417 V::LinearEndo : MaybeSerDes,
2418 S : Ring
2419{
2420 fn mul_assign (&mut self, rhs : Self) {
2421 self.0 *= rhs.0
2422 }
2423}
2424impl <S, V> num::One for LinearAuto <S, V> where
2425 V : Module <S>,
2426 V::LinearEndo : MaybeSerDes,
2427 S : Ring
2428{
2429 fn one() -> Self {
2430 LinearAuto (LinearIso::one())
2431 }
2432}
2433
2434impl <S, A, B, M> AffineMap <S, A, B, M> where
2438 A : AffineSpace <S>,
2439 B : AffineSpace <S>,
2440 M : LinearMap <S, A::Translation, B::Translation>,
2441 S : Field
2442{
2443 pub const fn new (linear_map : M, translation : B::Translation) -> Self {
2444 AffineMap { linear_map, translation, _phantom: PhantomData }
2445 }
2446 pub fn map (self, point : A) -> B {
2447 B::from_vector (self.linear_map * point.to_vector() + self.translation)
2448 }
2449 pub fn transform (self, translation : A::Translation) -> B::Translation {
2450 self.linear_map * translation
2451 }
2452}
2453
2454impl <S, A, B, M> Affinity <S, A, B, M> where
2458 M : LinearMap <S, A::Translation, B::Translation>,
2459 A : AffineSpace <S>,
2460 B : AffineSpace <S>,
2461 S : Field
2462{
2463 pub const fn new (
2464 linear_iso : LinearIso <S, A::Translation, B::Translation, M>,
2465 translation : B::Translation
2466 ) -> Self {
2467 Affinity { linear_iso, translation }
2468 }
2469 pub fn mat (self) -> M {
2470 *self.linear_iso
2471 }
2472 pub fn map (self, point : A) -> B {
2473 B::from_vector (self.linear_iso * point.to_vector() + self.translation)
2474 }
2475 pub fn transform (self, translation : A::Translation) -> B::Translation {
2476 self.linear_iso * translation
2477 }
2478}
2479
2480impl <S, P, Q, M> Projectivity <S, P, Q, M> where
2484 M : LinearMap <S, P::Translation, Q::Translation>,
2485 P : ProjectiveSpace <S>,
2486 Q : ProjectiveSpace <S>,
2487 S : Field
2488{
2489 pub const fn new (linear_iso : LinearIso <S, P::Translation, Q::Translation, M>)
2490 -> Self
2491 {
2492 Projectivity (linear_iso)
2493 }
2494 pub fn mat (self) -> M {
2495 **self
2496 }
2497}
2498impl <S, P, Q, M> std::ops::Deref for Projectivity <S, P, Q, M> where
2499 M : LinearMap <S, P::Translation, Q::Translation>,
2500 P : ProjectiveSpace <S>,
2501 Q : ProjectiveSpace <S>,
2502 S : Field
2503{
2504 type Target = LinearIso <S, P::Translation, Q::Translation, M>;
2505 fn deref (&self) -> &Self::Target {
2506 &self.0
2507 }
2508}
2509impl <S, P, Q, M> std::ops::Mul <P> for Projectivity <S, P, Q, M> where
2510 M : LinearMap <S, P::Translation, Q::Translation>,
2511 P : ProjectiveSpace <S>,
2512 Q : ProjectiveSpace <S>,
2513 S : Field
2514{
2515 type Output = Q;
2516 fn mul (self, rhs : P) -> Q {
2517 Q::from_vector (self.0 * rhs.to_vector())
2518 }
2519}
2520
2521pub macro point {
2522 ($x:expr, $y:expr) => {
2523 $crate::point2 ($x, $y)
2524 },
2525 ($x:expr, $y:expr, $z:expr) => {
2526 $crate::point3 ($x, $y, $z)
2527 },
2528 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2529 $crate::point4 ($x, $y, $z, $w)
2530 }
2531}
2532
2533pub macro vector {
2534 ($x:expr, $y:expr) => {
2535 $crate::vector2 ($x, $y)
2536 },
2537 ($x:expr, $y:expr, $z:expr) => {
2538 $crate::vector3 ($x, $y, $z)
2539 },
2540 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2541 $crate::vector4 ($x, $y, $z, $w)
2542 }
2543}
2544
2545pub macro matrix {
2546 ($v00:expr, $v01:expr; $v10:expr, $v11:expr$(;)?) => {
2547 $crate::Matrix2::new ($v00, $v01, $v10, $v11)
2548 },
2549 ( $v00:expr, $v01:expr, $v02:expr;
2550 $v10:expr, $v11:expr, $v12:expr;
2551 $v20:expr, $v21:expr, $v22:expr$(;)?
2552 ) => {
2553 $crate::Matrix3::new ($v00, $v01, $v02, $v10, $v11, $v12, $v20, $v21, $v22)
2554 },
2555 ( $v00:expr, $v01:expr, $v02:expr, $v03:expr;
2556 $v10:expr, $v11:expr, $v12:expr, $v13:expr;
2557 $v20:expr, $v21:expr, $v22:expr, $v23:expr;
2558 $v30:expr, $v31:expr, $v32:expr, $v33:expr$(;)?
2559 ) => {
2560 $crate::Matrix4::new (
2561 $v00, $v01, $v02, $v03,
2562 $v10, $v11, $v12, $v13,
2563 $v20, $v21, $v22, $v23,
2564 $v30, $v31, $v32, $v33)
2565 }
2566}
2567
2568macro_rules! impl_angle {
2569 ( $angle:ident, $docname:expr ) => {
2570 #[doc = $docname]
2571 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2572 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2574 pub struct $angle <S> (pub S);
2575 impl_numcast_primitive_map!($angle);
2576 impl <S : Ring> std::iter::Sum for $angle <S> {
2577 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2578 use num::Zero;
2579 iter.fold ($angle::zero(), |acc, item| acc + item)
2580 }
2581 }
2582 impl <S : Ring> std::ops::Neg for $angle <S> {
2583 type Output = Self;
2584 fn neg (self) -> Self {
2585 $angle (-self.0)
2586 }
2587 }
2588 impl <S : Ring> std::ops::Add for $angle <S> {
2589 type Output = Self;
2590 fn add (self, other : Self) -> Self::Output {
2591 $angle (self.0 + other.0)
2592 }
2593 }
2594 impl <S : Ring> std::ops::AddAssign for $angle <S> {
2595 fn add_assign (&mut self, other : Self) {
2596 self.0 += other.0
2597 }
2598 }
2599 impl <S : Ring> std::ops::Sub for $angle <S> {
2600 type Output = Self;
2601 fn sub (self, other : Self) -> Self::Output {
2602 $angle (self.0 - other.0)
2603 }
2604 }
2605 impl <S : Ring> std::ops::SubAssign for $angle <S> {
2606 fn sub_assign (&mut self, other : Self) {
2607 self.0 -= other.0
2608 }
2609 }
2610 impl <S : Ring> std::ops::Mul <S> for $angle <S> {
2611 type Output = Self;
2612 fn mul (self, scalar : S) -> Self::Output {
2613 $angle (scalar * self.0)
2614 }
2615 }
2616 impl <S : Ring> std::ops::MulAssign <S> for $angle <S> {
2617 fn mul_assign (&mut self, scalar : S) {
2618 self.0 *= scalar
2619 }
2620 }
2621 impl <S : Field> std::ops::Div for $angle <S> {
2622 type Output = S;
2623 fn div (self, other : Self) -> Self::Output {
2624 self.0 / other.0
2625 }
2626 }
2627 impl <S : Field> std::ops::Div <S> for $angle <S> {
2628 type Output = Self;
2629 fn div (self, scalar : S) -> Self::Output {
2630 $angle (self.0 / scalar)
2631 }
2632 }
2633 impl <S : Field> std::ops::DivAssign <S> for $angle <S> {
2634 fn div_assign (&mut self, scalar : S) {
2635 self.0 /= scalar
2636 }
2637 }
2638 impl <S : OrderedField> std::ops::Rem for $angle <S> {
2639 type Output = Self;
2640 fn rem (self, other : Self) -> Self::Output {
2641 $angle (self.0 % other.0)
2642 }
2643 }
2644 impl <S : Ring> num::Zero for $angle <S> {
2645 fn zero() -> Self {
2646 $angle (S::zero())
2647 }
2648 fn is_zero (&self) -> bool {
2649 self.0.is_zero()
2650 }
2651 }
2652 impl <S : Ring> From <S> for $angle <S> {
2653 fn from (s : S) -> Self {
2654 $angle (s)
2655 }
2656 }
2657 }
2658}
2659
2660macro_rules! impl_angle_wrapped {
2661 ( $angle:ident, $comment:expr ) => {
2662 #[doc = $comment]
2663 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2664 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2665 pub struct $angle <S> (Rad <S>);
2666
2667 impl_numcast!($angle);
2668 impl <S : Copy> $angle <S> {
2669 pub const fn angle (&self) -> Rad <S> {
2670 self.0
2671 }
2672 }
2673 impl <S : Real> std::iter::Sum for $angle <S> {
2674 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2675 use num::Zero;
2676 iter.fold ($angle::zero(), |acc, item| acc + item)
2677 }
2678 }
2679 impl <S : Real> std::ops::Neg for $angle <S> {
2680 type Output = Self;
2681 fn neg (self) -> Self {
2682 self.map (Rad::neg)
2683 }
2684 }
2685 impl <S : Real> std::ops::Add for $angle <S> {
2686 type Output = Self;
2687 fn add (self, other : Self) -> Self::Output {
2688 self.map (|angle| angle + other.0)
2689 }
2690 }
2691 impl <S : Real> std::ops::AddAssign for $angle <S> {
2692 fn add_assign (&mut self, other : Self) {
2693 *self = *self + other
2694 }
2695 }
2696 impl <S : Real> std::ops::Add <Rad <S>> for $angle <S> {
2697 type Output = Self;
2698 fn add (self, angle : Rad <S>) -> Self::Output {
2699 self.map (|a| a + angle)
2700 }
2701 }
2702 impl <S : Real> std::ops::AddAssign <Rad <S>> for $angle <S> {
2703 fn add_assign (&mut self, angle : Rad <S>) {
2704 *self = *self + angle
2705 }
2706 }
2707 impl <S : Real> std::ops::Sub for $angle <S> {
2708 type Output = Self;
2709 fn sub (self, other : Self) -> Self::Output {
2710 self.map (|angle| angle - other.0)
2711 }
2712 }
2713 impl <S : Real> std::ops::SubAssign for $angle <S> {
2714 fn sub_assign (&mut self, other : Self) {
2715 *self = *self - other
2716 }
2717 }
2718 impl <S : Real> std::ops::Sub <Rad <S>> for $angle <S> {
2719 type Output = Self;
2720 fn sub (self, angle : Rad <S>) -> Self::Output {
2721 self.map (|a| a - angle)
2722 }
2723 }
2724 impl <S : Real> std::ops::SubAssign <Rad <S>> for $angle <S> {
2725 fn sub_assign (&mut self, angle : Rad <S>) {
2726 *self = *self - angle
2727 }
2728 }
2729 impl <S : Real> std::ops::Mul <S> for $angle <S> {
2730 type Output = Self;
2731 fn mul (self, scalar : S) -> Self::Output {
2732 self.map (|angle| angle * scalar)
2733 }
2734 }
2735 impl <S : Real> std::ops::MulAssign <S> for $angle <S> {
2736 fn mul_assign (&mut self, scalar : S) {
2737 *self = *self * scalar
2738 }
2739 }
2740 impl <S : Real> std::ops::Div for $angle <S> {
2741 type Output = S;
2742 fn div (self, other : Self) -> Self::Output {
2743 self.0 / other.0
2744 }
2745 }
2746 impl <S : Real> std::ops::Div <S> for $angle <S> {
2747 type Output = Self;
2748 fn div (self, scalar : S) -> Self::Output {
2749 self.map (|angle| angle / scalar)
2750 }
2751 }
2752 impl <S : Real> std::ops::DivAssign <S> for $angle <S> {
2753 fn div_assign (&mut self, scalar : S) {
2754 *self = *self / scalar
2755 }
2756 }
2757 impl <S : Real> std::ops::Rem for $angle <S> {
2758 type Output = Self;
2759 fn rem (self, other : Self) -> Self::Output {
2760 self.map (|angle| angle % other.0)
2761 }
2762 }
2763 impl <S : Real> num::Zero for $angle <S> {
2764 fn zero() -> Self {
2765 $angle (Rad::zero())
2766 }
2767 fn is_zero (&self) -> bool {
2768 self.0.is_zero()
2769 }
2770 }
2771 }
2772}
2773
2774macro_rules! impl_dimension {
2775 ( $point:ident, $vector:ident, $nonzero:ident, $unit:ident,
2776 $point_fun:ident, $vector_fun:ident, $matrix_fun:ident, $ortho_fun:ident,
2777 [$($component:ident),+],
2778 [$(($axis_method:ident, $unit_method:ident)),+], [$($patch:ident)?],
2779 [$($projective:ident)?],
2780 $matrix:ident, [$($submatrix:ident)?], $ndims:expr, $dimension:expr
2781 ) => {
2782 #[doc = $dimension]
2783 #[doc = "position"]
2784 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display, From, Neg)]
2785 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2786 #[display("{}", _0)]
2787 #[repr(C)]
2788 pub struct $point <S> (pub $vector <S>);
2789
2790 #[doc = $dimension]
2791 #[doc = "non-zero vector"]
2792 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2793 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2794 #[display("{}", _0)]
2795 #[repr(C)]
2796 pub struct $nonzero <S> ($vector <S>);
2797
2798 #[doc = $dimension]
2799 #[doc = "unit vector"]
2800 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2801 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2802 #[display("{}", _0)]
2803 #[repr(C)]
2804 pub struct $unit <S> ($vector <S>);
2805
2806 #[doc = "Construct a"]
2810 #[doc = $dimension]
2811 #[doc = "point"]
2812 pub const fn $point_fun <S> ($($component : S),+) -> $point <S> {
2813 $point::new ($($component),+)
2814 }
2815 impl_numcast!($point);
2816 impl <S> $point <S> {
2817 pub const fn new ($($component : S),+) -> Self {
2818 $point ($vector::new ($($component),+))
2819 }
2820 $(
2821 pub const fn $component (&self) -> S where S : Copy {
2822 self.0.$component
2823 }
2824 )+
2825 pub fn distance (self, other : $point <S>) -> NonNegative <S> where
2826 S : OrderedField + Sqrt
2827 {
2828 MetricSpace::distance (self.0, other.0)
2829 }
2830 pub fn distance_squared (self, other : $point <S>) -> NonNegative <S> where
2831 S : OrderedField
2832 {
2833 MetricSpace::distance_squared (self.0, other.0)
2834 }
2835 }
2836 $(
2838 impl <S : Field> ProjectiveSpace <S> for $projective <S> {
2839 type Patch = $point <S>;
2840 }
2841 )?
2842 impl <S : Field> AffineSpace <S> for $point <S> {
2843 type Translation = $vector <S>;
2844 }
2845 impl <S : Ring> Point <$vector <S>> for $point <S> {
2846 fn to_vector (self) -> $vector <S> {
2847 self.0
2848 }
2849 fn from_vector (vector : $vector <S>) -> Self {
2850 $point (vector)
2851 }
2852 }
2853 impl <S> From <[S; $ndims]> for $point <S> {
2854 fn from (array : [S; $ndims]) -> Self {
2855 $point (array.into())
2856 }
2857 }
2858 $(
2859 impl <S> From <($patch <S>, S)> for $point <S> {
2860 fn from ((patch, scale) : ($patch <S>, S)) -> Self {
2861 $point ((patch.0, scale).into())
2862 }
2863 }
2864 )?
2865 impl <S> std::cmp::PartialOrd for $point <S> where S : PartialOrd {
2866 fn partial_cmp (&self, other : &Self) -> Option <std::cmp::Ordering> {
2867 self.0.as_slice().partial_cmp (&other.0.as_slice())
2868 }
2869 }
2870 impl <S> std::ops::Add <$vector <S>> for $point <S> where S : AdditiveGroup {
2871 type Output = Self;
2872 fn add (self, displacement : $vector <S>) -> Self::Output {
2873 $point (self.0 + displacement)
2874 }
2875 }
2876 impl <S> std::ops::Add <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2877 type Output = Self;
2878 fn add (self, displacement : &$vector <S>) -> Self::Output {
2879 $point (self.0 + *displacement)
2880 }
2881 }
2882 impl <S> std::ops::Add <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2883 type Output = $point <S>;
2884 fn add (self, displacement : $vector <S>) -> Self::Output {
2885 $point (self.0 + displacement)
2886 }
2887 }
2888 impl <S> std::ops::Add <&$vector <S>> for &$point <S> where
2889 S : AdditiveGroup + Copy
2890 {
2891 type Output = $point <S>;
2892 fn add (self, displacement : &$vector <S>) -> Self::Output {
2893 $point (self.0 + *displacement)
2894 }
2895 }
2896 impl <S> std::ops::AddAssign <$vector <S>> for $point <S> where S : AdditiveGroup {
2897 fn add_assign (&mut self, displacement : $vector <S>) {
2898 self.0 += displacement
2899 }
2900 }
2901 impl <S> std::ops::AddAssign <&$vector <S>> for $point <S> where
2902 S : AdditiveGroup + Copy
2903 {
2904 fn add_assign (&mut self, displacement : &$vector <S>) {
2905 self.0 += *displacement
2906 }
2907 }
2908 impl <S> std::ops::Sub <$vector <S>> for $point <S> where S : AdditiveGroup {
2909 type Output = Self;
2910 fn sub (self, displacement : $vector <S>) -> Self::Output {
2911 $point (self.0 - displacement)
2912 }
2913 }
2914 impl <S> std::ops::Sub <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2915 type Output = Self;
2916 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2917 $point (self.0 - *displacement)
2918 }
2919 }
2920 impl <S> std::ops::Sub <$point <S>> for $point <S> where S : AdditiveGroup {
2921 type Output = $vector <S>;
2922 fn sub (self, other : Self) -> Self::Output {
2923 self.0 - other.0
2924 }
2925 }
2926 impl <S> std::ops::Sub <&$point <S>> for $point <S> where S : AdditiveGroup + Copy {
2927 type Output = $vector <S>;
2928 fn sub (self, other : &Self) -> Self::Output {
2929 self.0 - other.0
2930 }
2931 }
2932 impl <S> std::ops::Sub <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2933 type Output = $point <S>;
2934 fn sub (self, displacement : $vector <S>) -> Self::Output {
2935 $point (self.0 - displacement)
2936 }
2937 }
2938 impl <S> std::ops::Sub <&$vector <S>> for &$point <S> where
2939 S : AdditiveGroup + Copy
2940 {
2941 type Output = $point <S>;
2942 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2943 $point (self.0 - *displacement)
2944 }
2945 }
2946 impl <S> std::ops::Sub <$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2947 type Output = $vector <S>;
2948 fn sub (self, other : $point <S>) -> Self::Output {
2949 self.0 - other.0
2950 }
2951 }
2952 impl <S> std::ops::Sub <&$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2953 type Output = $vector <S>;
2954 fn sub (self, other : &$point <S>) -> Self::Output {
2955 self.0 - other.0
2956 }
2957 }
2958 impl <S> std::ops::SubAssign <$vector <S>> for $point <S> where
2959 S : AdditiveGroup
2960 {
2961 fn sub_assign (&mut self, displacement : $vector <S>) {
2962 self.0 -= displacement
2963 }
2964 }
2965 impl <S> std::ops::SubAssign <&$vector <S>> for $point <S> where
2966 S : AdditiveGroup + Copy
2967 {
2968 fn sub_assign (&mut self, displacement : &$vector <S>) {
2969 self.0 -= *displacement
2970 }
2971 }
2972 impl <S> approx::AbsDiffEq for $point <S> where
2973 S : approx::AbsDiffEq,
2974 S::Epsilon : Copy
2975 {
2976 type Epsilon = S::Epsilon;
2977 fn default_epsilon() -> Self::Epsilon {
2978 S::default_epsilon()
2979 }
2980 #[inline]
2981 fn abs_diff_eq (&self, other : &Self, epsilon : Self::Epsilon) -> bool {
2982 self.0.abs_diff_eq (&other.0, epsilon)
2983 }
2984 }
2985 impl <S> approx::RelativeEq for $point <S> where
2986 S : approx::RelativeEq,
2987 S::Epsilon : Copy
2988 {
2989 fn default_max_relative() -> Self::Epsilon {
2990 S::default_max_relative()
2991 }
2992 #[inline]
2993 fn relative_eq (&self,
2994 other : &Self, epsilon : Self::Epsilon, max_relative : Self::Epsilon
2995 ) -> bool {
2996 self.0.relative_eq (&other.0, epsilon, max_relative)
2997 }
2998 }
2999 impl <S> approx::UlpsEq for $point <S> where
3000 S : approx::UlpsEq,
3001 S::Epsilon : Copy
3002 {
3003 fn default_max_ulps() -> u32 {
3004 S::default_max_ulps()
3005 }
3006 #[inline]
3007 fn ulps_eq (&self, other : &Self, epsilon : Self::Epsilon, max_ulps : u32)
3008 -> bool
3009 {
3010 self.0.ulps_eq (&other.0, epsilon, max_ulps)
3011 }
3012 }
3013 #[doc = "Construct a"]
3017 #[doc = $dimension]
3018 #[doc = "vector"]
3019 pub const fn $vector_fun <S> ($($component : S),+) -> $vector <S> {
3020 $vector::new ($($component),+)
3021 }
3022 impl <S> NormedVectorSpace <S> for $vector <S> where S : OrderedField {
3023 type Unit = $unit <S>;
3024 fn norm_squared (self) -> NonNegative <S> {
3025 self.self_dot()
3026 }
3027 fn norm_max (self) -> NonNegative <S> where S : SignedExt {
3028 NonNegative::unchecked (self.map (|x| x.abs()).reduce_partial_max())
3029 }
3030 fn normalize (self) -> Self::Unit where S : Sqrt {
3031 $unit (self / *self.norm())
3032 }
3033 }
3034 impl <S> InnerProductSpace <S> for $vector <S> where S : Field {
3035 fn outer_product (self, rhs : Self) -> $matrix <S> {
3036 let mut cols : $vector <$vector <S>> = [$vector::zero(); $ndims].into();
3037 for col in 0..$ndims {
3038 for row in 0..$ndims {
3039 cols[col][row] = self[row] * rhs[col];
3040 }
3041 }
3042 $matrix { cols }
3043 }
3044 fn orthogonal (self) -> Self where S : approx::AbsDiffEq <Epsilon = S> {
3045 $ortho_fun (self)
3046 }
3047 }
3048 impl <S : Ring> Dot <S> for $vector <S> {
3049 fn dot (self, other : Self) -> S {
3050 self.dot (other)
3051 }
3052 }
3053 impl <S : Field> VectorSpace <S> for $vector <S> {
3054 type NonZero = $nonzero <S>;
3055 fn map <F> (self, f : F) -> Self where F : FnMut (S) -> S {
3056 self.map (f)
3057 }
3058 }
3059 impl <S> Module <S> for $vector <S> where S : Ring + num::MulAdd {
3060 type LinearEndo = $matrix <S>;
3061 }
3062 impl <S> GroupAction <Addition, $point <S>> for $vector <S>
3063 where S : AdditiveGroup { }
3064 impl <S> MonoidAction <Addition, $point <S>> for $vector <S>
3065 where S : AdditiveGroup { }
3066 impl <S> SemigroupAction <Addition, $point <S>> for $vector <S>
3067 where S : AdditiveGroup
3068 {
3069 fn action (self, point : $point <S>) -> $point <S> {
3070 point + self
3071 }
3072 }
3073 impl <S> std::ops::Mul <LinearAuto <S, Self>> for $vector <S> where
3074 S : Ring + MaybeSerDes
3075 {
3076 type Output = Self;
3077 fn mul (self, rhs : LinearAuto <S, Self>) -> Self::Output {
3078 self * rhs.0
3079 }
3080 }
3081 impl <S> std::ops::Mul <LinearIso <S, Self, Self, $matrix <S>>> for $vector <S> where
3082 S : Ring + MaybeSerDes
3083 {
3084 type Output = Self;
3085 fn mul (self, rhs : LinearIso <S, Self, Self, $matrix <S>>) -> Self::Output {
3086 self * rhs.0
3087 }
3088 }
3089 impl <S> num::Inv for LinearAuto <S, $vector <S>> where
3090 S : Ring + num::real::Real + MaybeSerDes
3091 {
3092 type Output = Self;
3093 fn inv (self) -> Self::Output {
3094 LinearAuto (self.0.inv())
3095 }
3096 }
3097 impl <S> std::ops::Div for LinearAuto <S, $vector <S>> where
3098 S : Ring + num::Float + MaybeSerDes
3099 {
3100 type Output = Self;
3101 fn div (self, rhs : Self) -> Self::Output {
3102 LinearAuto (self.0 / rhs.0)
3103 }
3104 }
3105 impl <S> std::ops::DivAssign <Self> for LinearAuto <S, $vector <S>> where
3106 S : Ring + num::Float + MaybeSerDes
3107 {
3108 fn div_assign (&mut self, rhs : Self) {
3109 self.0 /= rhs.0
3110 }
3111 }
3112 impl <S> From <$unit <S>> for $vector <S> where S : Copy {
3113 fn from (unit : $unit <S>) -> Self {
3114 *unit
3115 }
3116 }
3117 #[doc = "Construct a"]
3121 #[doc = $dimension]
3122 #[doc = "matrix"]
3123 pub const fn $matrix_fun <S> ($($component : $vector <S>),+) -> $matrix <S> {
3124 $matrix {
3125 cols: $vector_fun ($($component),+)
3126 }
3127 }
3128 impl <S, V, W> num::Inv for LinearIso <S, V, W, $matrix <S>> where
3129 V : Module <S>,
3130 W : Module <S>,
3131 S : Ring + num::real::Real
3132 {
3133 type Output = LinearIso <S, W, V, $matrix <S>>;
3134 fn inv (self) -> Self::Output {
3135 let mat4 = Matrix4::from (self.0);
3137 LinearIso (mat4.inverted().into(), PhantomData::default())
3138 }
3139 }
3140 impl <S, V> std::ops::Div for LinearIso <S, V, V, $matrix <S>> where
3141 V : Module <S, LinearEndo=$matrix <S>>,
3142 S : Ring + num::real::Real
3143 {
3144 type Output = Self;
3145 #[expect(clippy::suspicious_arithmetic_impl)]
3146 fn div (self, rhs : Self) -> Self::Output {
3147 use num::Inv;
3148 self * rhs.inv()
3149 }
3150 }
3151 impl <S, V> std::ops::DivAssign for LinearIso <S, V, V, $matrix <S>> where
3152 V : Module <S, LinearEndo=$matrix <S>>,
3153 S : Ring + num::real::Real
3154 {
3155 #[expect(clippy::suspicious_op_assign_impl)]
3156 fn div_assign (&mut self, rhs : Self) {
3157 use num::Inv;
3158 *self *= rhs.inv()
3159 }
3160 }
3161 $(
3162 impl <S : Copy> Matrix <S> for $matrix <S> {
3163 type Rows = $vector <$vector <S>>;
3164 type Submatrix = $submatrix <S>;
3165 fn rows (self) -> $vector <$vector <S>> {
3166 self.into_row_arrays().map ($vector::from).into()
3167 }
3168 fn submatrix (self, i : usize, j : usize) -> $submatrix <S> {
3169 let a : [S; ($ndims-1) * ($ndims-1)] = std::array::from_fn (|index|{
3170 let i = (i + 1 + index / ($ndims-1)) % $ndims;
3171 let j = (j + 1 + index % ($ndims-1)) % $ndims;
3172 self.cols[i][j]
3173 });
3174 $submatrix::from_col_array (a)
3175 }
3176 fn fill_zeros (submatrix : $submatrix <S>) -> Self where S : num::Zero{
3178 let cols = $vector::from (
3179 std::array::from_fn (|i| if i < $ndims-1 {
3180 $vector::from (submatrix.cols[i])
3181 } else {
3182 $vector::zero()
3183 })
3184 );
3185 $matrix { cols }
3186 }
3187 }
3188 )?
3189 impl <S : Ring> LinearMap <S, $vector <S>, $vector <S>> for $matrix <S> {
3190 fn determinant (self) -> S {
3191 self.determinant()
3192 }
3193 fn transpose (self) -> Self {
3194 self.transposed()
3195 }
3196 }
3197 impl_numcast!($nonzero);
3201 impl <S> $nonzero <S> {
3202 pub fn new (vector : $vector <S>) -> Option <Self> where S : Ring {
3212 use num::Zero;
3213 if !vector.is_zero() {
3214 Some ($nonzero (vector))
3215 } else {
3216 None
3217 }
3218 }
3219 pub fn noisy (vector : $vector <S>) -> Self where S : Ring {
3228 use num::Zero;
3229 assert!(!vector.is_zero());
3230 $nonzero (vector)
3231 }
3232 #[inline]
3236 pub fn unchecked (vector : $vector <S>) -> Self where S : Ring + std::fmt::Debug {
3237 debug_assert_ne!(vector, $vector::zero());
3238 $nonzero (vector)
3239 }
3240 #[must_use]
3252 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3253 where S : Ring
3254 {
3255 Self::noisy (fun (self.0))
3256 }
3257 #[must_use]
3258 pub fn map_unchecked (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3259 where S : Ring + std::fmt::Debug
3260 {
3261 Self::unchecked (fun (self.0))
3262 }
3263 }
3264 impl <S> std::ops::Deref for $nonzero <S> {
3265 type Target = $vector <S>;
3266 fn deref (&self) -> &$vector <S> {
3267 &self.0
3268 }
3269 }
3270 impl <S : Ring> std::ops::Neg for $nonzero <S> {
3271 type Output = Self;
3272 fn neg (self) -> Self {
3273 $nonzero (-self.0)
3274 }
3275 }
3276 impl <S> From <$unit <S>> for $nonzero <S> where S : Ring + std::fmt::Debug {
3277 fn from (unit : $unit <S>) -> Self {
3278 $nonzero::unchecked (*unit)
3279 }
3280 }
3281
3282 impl_numcast!($unit);
3286 impl <S> $unit <S> {
3287 $(
3293 #[inline]
3294 pub fn $axis_method() -> Self where S : num::One + num::Zero {
3295 $unit ($vector::$unit_method())
3296 }
3297 )+
3298 pub fn new (vector : $vector <S>) -> Option <Self> where S : OrderedField + Sqrt {
3302 if vector == *vector.normalize() {
3303 Some ($unit (vector))
3304 } else {
3305 None
3306 }
3307 }
3308 pub fn new_approx (vector : $vector <S>) -> Option <Self> where
3315 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3316 {
3317 if vector.is_normalized() {
3318 Some ($unit (vector))
3319 } else {
3320 None
3321 }
3322 }
3323 pub fn normalize (vector : $vector <S>) -> Self where S : OrderedField + Sqrt {
3344 use num::Zero;
3345 assert!(!vector.is_zero());
3346 vector.normalize()
3347 }
3348 pub fn normalize_approx (vector : $vector <S>) -> Self where
3352 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3353 {
3354 use num::Zero;
3355 assert!(!vector.is_zero());
3356 let vector = if vector.is_normalized() {
3357 vector
3358 } else {
3359 vector.normalized()
3360 };
3361 $unit (vector)
3362 }
3363 pub fn noisy (vector : $vector <S>) -> Self where
3372 S : OrderedField + Sqrt + std::fmt::Debug
3373 {
3374 assert_eq!(vector, *vector.normalize());
3375 $unit (vector)
3376 }
3377 pub fn noisy_approx (vector : $vector <S>) -> Self where
3386 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3387 {
3388 assert!(vector.is_normalized());
3389 $unit (vector)
3390 }
3391 #[inline]
3395 pub fn unchecked (vector : $vector <S>) -> Self where
3396 S : OrderedField + Sqrt + std::fmt::Debug
3397 {
3398 debug_assert_eq!(vector, *vector.normalize());
3399 $unit (vector)
3400 }
3401 #[inline]
3411 pub fn unchecked_approx (vector : $vector <S>) -> Self where
3412 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3413 {
3414 debug_assert!(vector.is_normalized());
3415 $unit (vector)
3416 }
3417 pub fn random_unit <R : rand::RngExt> (rng : &mut R) -> Self where
3419 S : OrderedField + Sqrt + rand::distr::uniform::SampleUniform
3420 {
3421 let vector = $vector {
3422 $($component: rng.random_range (-S::one()..S::one())),+
3423 };
3424 vector.normalize()
3425 }
3426 pub fn invert (mut self) -> Self where S : Ring {
3429 self.0 = -self.0;
3430 self
3431 }
3432 #[must_use]
3444 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3445 S : OrderedField + Sqrt + std::fmt::Debug
3446 {
3447 Self::noisy (fun (self.0))
3448 }
3449 #[must_use]
3461 pub fn map_noisy_approx (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3462 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3463 {
3464 Self::noisy_approx (fun (self.0))
3465 }
3466 }
3467 impl <S> std::ops::Deref for $unit <S> {
3468 type Target = $vector <S>;
3469 fn deref (&self) -> &$vector <S> {
3470 &self.0
3471 }
3472 }
3473 impl <S : Ring> std::ops::Neg for $unit <S> {
3474 type Output = Self;
3475 fn neg (self) -> Self {
3476 $unit (-self.0)
3477 }
3478 }
3479 }
3481}
3482
3483macro_rules! impl_numcast {
3484 ($type:ident) => {
3485 impl <S> $type <S> {
3486 #[inline]
3487 pub fn numcast <T> (self) -> Option <$type <T>> where
3488 S : num::NumCast,
3489 T : num::NumCast
3490 {
3491 self.0.numcast().map ($type)
3492 }
3493 }
3494 }
3495}
3496
3497macro_rules! impl_numcast_primitive {
3498 ($type:ident) => {
3499 impl <S> $type <S> {
3500 #[inline]
3501 pub fn numcast <T> (self) -> Option <$type <T>> where
3502 S : num::NumCast,
3503 T : num::NumCast
3504 {
3505 T::from (self.0).map ($type)
3506 }
3507 }
3508 impl <S> num::ToPrimitive for $type <S> where S : num::ToPrimitive {
3509 fn to_isize (&self) -> Option <isize> {
3510 self.0.to_isize()
3511 }
3512 fn to_i8 (&self) -> Option <i8> {
3513 self.0.to_i8()
3514 }
3515 fn to_i16 (&self) -> Option <i16> {
3516 self.0.to_i16()
3517 }
3518 fn to_i32 (&self) -> Option <i32> {
3519 self.0.to_i32()
3520 }
3521 fn to_i64 (&self) -> Option <i64> {
3522 self.0.to_i64()
3523 }
3524 fn to_usize (&self) -> Option <usize> {
3525 self.0.to_usize()
3526 }
3527 fn to_u8 (&self) -> Option <u8> {
3528 self.0.to_u8()
3529 }
3530 fn to_u16 (&self) -> Option <u16> {
3531 self.0.to_u16()
3532 }
3533 fn to_u32 (&self) -> Option <u32> {
3534 self.0.to_u32()
3535 }
3536 fn to_u64 (&self) -> Option <u64> {
3537 self.0.to_u64()
3538 }
3539 fn to_f32 (&self) -> Option <f32> {
3540 self.0.to_f32()
3541 }
3542 fn to_f64 (&self) -> Option <f64> {
3543 self.0.to_f64()
3544 }
3545 }
3546 }
3547}
3548
3549macro_rules! impl_numcast_primitive_option {
3550 ($type:ident$(, $bound:path)*) => {
3551 impl_numcast_primitive!{$type}
3552 impl <S> num::NumCast for $type <S> where
3553 S : num::NumCast $(+ $bound)*
3554 {
3555 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3556 S::from (n).and_then ($type::new)
3557 }
3558 }
3559 }
3560}
3561
3562macro_rules! impl_numcast_primitive_map {
3563 ($type:ident) => {
3564 impl_numcast_primitive!{$type}
3565 impl <S> num::NumCast for $type <S> where S : num::NumCast {
3566 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3567 S::from (n).map ($type)
3568 }
3569 }
3570 }
3571}
3572
3573#[doc(hidden)]
3574#[macro_export]
3575macro_rules! impl_numcast_fields {
3576 ($type:ident, $($field:ident),+) => {
3577 impl <S> $type <S> {
3578 #[inline]
3579 pub fn numcast <T> (self) -> Option <$type <T>> where
3580 S : num::NumCast + PartialOrd + num::Zero,
3581 T : num::NumCast + PartialOrd + num::Zero
3582 {
3583 Some ($type {
3584 $($field: self.$field.numcast()?),+
3585 })
3586 }
3587 }
3588 }
3589}
3590
3591impl_angle!(Deg, "Degrees");
3592impl_angle!(Rad, "Radians");
3593impl_angle!(Turn, "Turns");
3594impl_angle_wrapped!(AngleWrapped, "Unsigned wrapped angle restricted to $[0, 2\\pi)$");
3595impl_angle_wrapped!(AngleWrappedSigned,
3596 "Signed wrapped angle restricted to $(-\\pi, \\pi]$");
3597
3598impl_dimension!(Point2, Vector2, NonZero2, Unit2, point2, vector2, matrix2, ortho_vec2,
3599 [x, y], [(axis_x, unit_x), (axis_y, unit_y)],
3600 [], [Point3], Matrix2, [], 2, "2D");
3601impl_dimension!(Point3, Vector3, NonZero3, Unit3, point3, vector3, matrix3, ortho_vec3,
3602 [x, y, z], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z)],
3603 [Point2], [Point4], Matrix3, [Matrix2], 3, "3D");
3604impl_dimension!(Point4, Vector4, NonZero4, Unit4, point4, vector4, matrix4, ortho_vec4,
3605 [x, y, z, w], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z), (axis_w, unit_w)],
3606 [Point3], [], Matrix4, [Matrix3], 4, "4D");
3607
3608impl_numcast_primitive_option!(Positive, PartialOrd, num::Zero);
3609impl_numcast_primitive_option!(NonNegative, PartialOrd, num::Zero);
3610impl_numcast_primitive_option!(NonZero, PartialEq, num::Zero);
3611impl_numcast_primitive_option!(Normalized, PartialOrd, num::One, num::Zero);
3612impl_numcast_primitive_option!(NormalSigned, OrderedRing);
3613impl_numcast_fields!(Angles3, yaw, pitch, roll);
3614
3615#[inline]
3619fn normalize_quaternion <S : Real> (quaternion : Quaternion <S>) -> Quaternion <S> {
3620 Quaternion::from_vec4 (*quaternion.into_vec4().normalize())
3621}
3622
3623#[cfg(test)]
3624mod tests {
3625 extern crate test;
3626
3627 use super::*;
3628 use crate::num;
3629 use approx;
3630 use rand;
3631 use rand_distr;
3632 use rand_xorshift;
3633
3634 #[expect(clippy::cast_possible_truncation)]
3635 static ORTHO_VEC_BENCH_SEED : std::sync::LazyLock <u64> = std::sync::LazyLock::new (
3636 || std::time::SystemTime::UNIX_EPOCH.elapsed().unwrap().as_nanos() as u64);
3637
3638 #[test]
3639 fn defaults() {
3640 use num::Zero;
3641
3642 assert_eq!(Positive::<f32>::default().0, 0.0);
3643 assert_eq!(NonNegative::<f32>::default().0, 0.0);
3644 assert_eq!(Normalized::<f32>::default().0, 0.0);
3645 assert_eq!(NormalSigned::<f32>::default().0, 0.0);
3646 assert_eq!(Deg::<f32>::default().0, 0.0);
3647 assert_eq!(Rad::<f32>::default().0, 0.0);
3648 assert_eq!(Turn::<f32>::default().0, 0.0);
3649 assert_eq!(
3650 Angles3::<f32>::default(),
3651 Angles3::new (AngleWrapped::zero(), AngleWrapped::zero(), AngleWrapped::zero()));
3652 assert_eq!(
3653 Pose3::<f32>::default(),
3654 Pose3 { position: Point3::origin(), angles: Angles3::default() });
3655 assert_eq!(
3656 LinearIso::<f32, Vector3 <f32>, Vector3 <f32>, Matrix3 <f32>>::default().0,
3657 Matrix3::identity());
3658 assert_eq!(LinearAuto::<f32, Vector3 <f32>>::default().0.0, Matrix3::identity());
3659 assert_eq!(Rotation2::<f32>::default().0.0.0, Matrix2::identity());
3660 assert_eq!(Rotation3::<f32>::default().0.0.0, Matrix3::identity());
3661 assert_eq!(Versor::<f32>::default().0, Quaternion::from_xyzw (0.0, 0.0, 0.0, 1.0));
3662 assert_eq!(
3663 AffineMap::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3664 AffineMap::new (Matrix3::identity(), Vector3::zero()));
3665 assert_eq!(
3666 Affinity::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3667 Affinity::new (LinearIso::new (Matrix3::identity()).unwrap(), Vector3::zero()));
3668 assert_eq!(
3669 Projectivity::<f32, Point4 <f32>, Point4 <f32>, Matrix4 <f32>>::default().0,
3670 LinearIso::new (Matrix4::identity()).unwrap());
3671 }
3672
3673 #[test]
3674 fn numcast() {
3675 let p : Positive <f32> = Positive::<f64>::noisy (0.25).numcast().unwrap();
3676 assert_eq!(p, Positive::noisy (0.25f32));
3677 let v : Vector3 <Positive <f64>> = [
3678 Positive::noisy (1.0),
3679 Positive::noisy (2.0),
3680 Positive::noisy (0.25)
3681 ].into();
3682 let _v : Vector3 <Positive <f32>> = v.numcast().unwrap();
3683 }
3684
3685 #[test]
3686 fn vector_sum() {
3687 let s = [
3688 [0.0, 1.0],
3689 [1.0, 1.0],
3690 [0.5, 0.5]
3691 ].map (Vector2::<f32>::from);
3692 let _sum = s.iter().copied().sum::<Vector2 <f32>>();
3693 }
3694
3695 #[test]
3696 fn vector_outer_product() {
3697 let v1 = vector3 (1.0, 2.0, 3.0);
3698 let v2 = vector3 (2.0, 3.0, 4.0);
3699 assert_eq!(v1.outer_product (v2).into_row_arrays(), [
3700 [2.0, 3.0, 4.0],
3701 [4.0, 6.0, 8.0],
3702 [6.0, 9.0, 12.0]
3703 ]);
3704 }
3705
3706 #[test]
3707 fn rotation3_noisy_approx() {
3708 use rand::{RngExt, SeedableRng};
3709 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3712 let up = Vector3::unit_z();
3713 for _ in 0..1000 {
3714 let forward = vector3 (
3715 rng.random_range (-1000.0f32..1000.0),
3716 rng.random_range (-1000.0f32..1000.0),
3717 0.0
3718 ).normalized();
3719 let right = forward.cross (up);
3720 let mat = Matrix3::from_col_arrays ([
3721 right.into_array(),
3722 forward.into_array(),
3723 up.into_array()
3724 ]);
3725 let _ = Rotation3::noisy_approx (mat);
3726 }
3727 }
3728
3729 #[test]
3730 fn rotation3_intrinsic_angles() {
3731 use std::f32::consts::PI;
3732 use num::Zero;
3733 use rand::{RngExt, SeedableRng};
3734 let rot = Rotation3::from_angles_intrinsic (
3736 Turn (0.25).into(), Rad::zero(), Rad::zero());
3737 approx::assert_relative_eq!(rot.mat(),
3738 Matrix3::from_col_arrays ([
3739 [ 0.0, 1.0, 0.0],
3740 [-1.0, 0.0, 0.0],
3741 [ 0.0, 0.0, 1.0]
3742 ])
3743 );
3744 assert_eq!((Turn (0.25).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3745 let rot = Rotation3::from_angles_intrinsic (
3746 Turn (0.5).into(), Rad::zero(), Rad::zero());
3747 approx::assert_relative_eq!(rot.mat(),
3748 Matrix3::from_col_arrays ([
3749 [-1.0, 0.0, 0.0],
3750 [ 0.0, -1.0, 0.0],
3751 [ 0.0, 0.0, 1.0]
3752 ])
3753 );
3754 assert_eq!((Turn (0.5).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3755 let rot = Rotation3::from_angles_intrinsic (
3757 Rad::zero(), Turn (0.25).into(), Rad::zero());
3758 approx::assert_relative_eq!(rot.mat(),
3759 Matrix3::from_col_arrays ([
3760 [1.0, 0.0, 0.0],
3761 [0.0, 0.0, 1.0],
3762 [0.0, -1.0, 0.0]
3763 ])
3764 );
3765 assert_eq!((Rad::zero(), Turn (0.25).into(), Rad::zero()), rot.intrinsic_angles());
3766 let rot = Rotation3::from_angles_intrinsic (
3768 Rad::zero(), Rad::zero(), Turn (0.25).into());
3769 approx::assert_relative_eq!(rot.mat(),
3770 Matrix3::from_col_arrays ([
3771 [0.0, 0.0, -1.0],
3772 [0.0, 1.0, 0.0],
3773 [1.0, 0.0, 0.0]
3774 ])
3775 );
3776 assert_eq!((Rad::zero(), Rad::zero(), Turn (0.25).into()), rot.intrinsic_angles());
3777 let rot = Rotation3::from_angles_intrinsic (
3779 Turn (0.25).into(), Turn (0.25).into(), Rad::zero());
3780 approx::assert_relative_eq!(rot.mat(),
3781 Matrix3::from_col_arrays ([
3782 [0.0, 1.0, 0.0],
3783 [0.0, 0.0, 1.0],
3784 [1.0, 0.0, 0.0]
3785 ])
3786 );
3787 let angles = rot.intrinsic_angles();
3788 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3789 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3790 approx::assert_relative_eq!(0.0, angles.2.0);
3791 let rot = Rotation3::from_angles_intrinsic (
3793 Turn (0.25).into(), Turn (0.25).into(), Turn (0.25).into());
3794 approx::assert_relative_eq!(rot.mat(),
3795 Matrix3::from_col_arrays ([
3796 [-1.0, 0.0, 0.0],
3797 [ 0.0, 0.0, 1.0],
3798 [ 0.0, 1.0, 0.0]
3799 ])
3800 );
3801 let angles = rot.intrinsic_angles();
3802 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3803 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3804 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.2.0);
3805
3806 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3809 let mut random_vector = || vector3 (
3810 rng.random_range (-100.0f32..100.0),
3811 rng.random_range (-100.0f32..100.0),
3812 rng.random_range (-100.0f32..100.0));
3813 for _ in 0..1000 {
3814 let rot = Rotation3::orthonormalize (
3815 random_vector(), random_vector(), random_vector()
3816 ).unwrap();
3817 let (yaw, pitch, roll) = rot.intrinsic_angles();
3818 assert!(yaw.0 < PI);
3819 assert!(yaw.0 > -PI);
3820 assert!(pitch.0 < PI);
3821 assert!(pitch.0 > -PI);
3822 assert!(roll.0 < PI);
3823 assert!(roll.0 > -PI);
3824 }
3825 }
3826
3827 #[test]
3828 fn rotation3_into_versor() {
3829 use std::f32::consts::PI;
3830 let rot = Rotation3::from_angle_x (Rad (0.01));
3831 let quat = rot.versor().0;
3832 approx::assert_relative_eq!(rot.mat(), quat.into());
3833 let rot = Rotation3::from_angle_x (Rad (-0.01));
3834 let quat = rot.versor().0;
3835 approx::assert_relative_eq!(rot.mat(), quat.into());
3836
3837 let rot = Rotation3::from_angle_y (Rad (0.01));
3838 let quat = rot.versor().0;
3839 approx::assert_relative_eq!(rot.mat(), quat.into());
3840 let rot = Rotation3::from_angle_y (Rad (-0.01));
3841 let quat = rot.versor().0;
3842 approx::assert_relative_eq!(rot.mat(), quat.into());
3843
3844 let rot = Rotation3::from_angle_z (Rad (0.01));
3845 let quat = rot.versor().0;
3846 approx::assert_relative_eq!(rot.mat(), quat.into());
3847 let rot = Rotation3::from_angle_z (Rad (-0.01));
3848 let quat = rot.versor().0;
3849 approx::assert_relative_eq!(rot.mat(), quat.into());
3850
3851 let rot = Rotation3::from_angle_x (Rad (PI / 2.0));
3852 let quat = rot.versor().0;
3853 approx::assert_relative_eq!(rot.mat(), quat.into());
3854 let rot = Rotation3::from_angle_x (Rad (-PI / 2.0));
3855 let quat = rot.versor().0;
3856 approx::assert_relative_eq!(rot.mat(), quat.into());
3857
3858 let rot = Rotation3::from_angle_y (Rad (PI / 2.0));
3859 let quat = rot.versor().0;
3860 approx::assert_relative_eq!(rot.mat(), quat.into());
3861 let rot = Rotation3::from_angle_y (Rad (-PI / 2.0));
3862 let quat = rot.versor().0;
3863 approx::assert_relative_eq!(rot.mat(), quat.into());
3864
3865 let rot = Rotation3::from_angle_z (Rad (PI / 2.0));
3866 let quat = rot.versor().0;
3867 approx::assert_relative_eq!(rot.mat(), quat.into());
3868 let rot = Rotation3::from_angle_z (Rad (-PI / 2.0));
3869 let quat = rot.versor().0;
3870 approx::assert_relative_eq!(rot.mat(), quat.into());
3871 }
3872
3873 #[bench]
3877 fn bench_orthogonal_cross_product (b : &mut test::Bencher) {
3878 use approx::AbsDiffEq;
3879 use rand::{RngExt, SeedableRng};
3880 use rand_distr::Distribution;
3881 let epsilon = f64::default_epsilon() * 256.0;
3882 let std_normal = rand_distr::StandardNormal;
3883 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3884 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3885 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3886 let x : f64 = std_normal.sample (rng);
3887 let y : f64 = std_normal.sample (rng);
3888 x / y
3889 };
3890 b.iter(||{
3891 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3892 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3893 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3894 6 => vector3 (0.0, randf (&mut rng), 0.0),
3895 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3896 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3897 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3898 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3899 _ => unreachable!()
3900 };
3901 approx::assert_abs_diff_ne!(vec, Vector3::zero(), epsilon = epsilon);
3902 let mut ortho = vec.cross (Vector3::unit_x());
3903 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) {
3904 ortho = vec.cross (Vector3::unit_y())
3905 }
3906 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3907 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3908 {
3909 println!("VEC: {vec}");
3910 println!("ORTHO: {ortho}");
3911 println!("DOT: {}", vec.dot (ortho));
3912 }
3913 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3914 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3915 });
3916 }
3917
3918 #[bench]
3922 fn bench_orthogonal_copysign (b : &mut test::Bencher) {
3923 use approx::AbsDiffEq;
3924 use rand::{RngExt, SeedableRng};
3925 use rand_distr::Distribution;
3926 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3927 let std_normal = rand_distr::StandardNormal;
3928 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3929 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3930 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3931 let x : f64 = std_normal.sample (rng);
3932 let y : f64 = std_normal.sample (rng);
3933 x / y
3934 };
3935 b.iter(||{
3936 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3937 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3938 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3939 6 => vector3 (0.0, randf (&mut rng), 0.0),
3940 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3941 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3942 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3943 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3944 _ => unreachable!()
3945 };
3946 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3947 return
3948 }
3949 let ortho = vector3 (
3950 vec.z.copysign (vec.x),
3951 vec.z.copysign (vec.y),
3952 -vec.x.copysign (vec.z) - vec.y.copysign (vec.z)
3954 );
3956 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3957 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3958 {
3959 println!("VEC: {vec}");
3960 println!("ORTHO: {ortho}");
3961 println!("DOT: {:.}", vec.dot (ortho));
3962 }
3963 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3964 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3965 });
3966 }
3967
3968 #[bench]
3972 fn bench_orthogonal_sign (b : &mut test::Bencher) {
3973 use approx::AbsDiffEq;
3974 use rand::{RngExt, SeedableRng};
3975 use rand_distr::Distribution;
3976 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3977 let std_normal = rand_distr::StandardNormal;
3978 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3979 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3980 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3981 let x : f64 = std_normal.sample (rng);
3982 let y : f64 = std_normal.sample (rng);
3983 x / y
3984 };
3985 b.iter(||{
3986 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3987 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3988 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3989 6 => vector3 (0.0, randf (&mut rng), 0.0),
3990 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3991 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3992 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3993 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3994 _ => unreachable!()
3995 };
3996 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3997 return
3998 }
3999 let sign = |n : f64| if n == 0.0 {
4000 0.0
4001 } else if n > 0.0 {
4002 1.0
4003 } else {
4004 -1.0
4005 };
4006 let signfn = |n : f64| sign ((sign (n) + 0.5) * (sign (vec.z) + 0.5));
4007 let ortho = vector3 (
4008 signfn (vec.x) * vec.z,
4009 signfn (vec.y) * vec.z,
4010 (-signfn (vec.x)).mul_add (vec.x, -signfn (vec.y) * vec.y)
4011 );
4013 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
4014 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
4015 {
4016 println!("VEC: {vec}");
4017 println!("ORTHO: {ortho}");
4018 println!("DOT: {:.}", vec.dot (ortho));
4019 }
4020 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
4021 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
4022 });
4023 }
4024}