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