1use std::marker::PhantomData;
2use derive_more::{Deref, Display, From, Neg};
3
4use crate::{approx, num};
5
6#[cfg(feature = "derive_serdes")]
7use serde::{Deserialize, Serialize};
8
9use crate::traits::*;
10
11pub mod coordinate;
12pub mod frame;
13
14pub use vek::Vec2 as Vector2;
15pub use vek::Vec3 as Vector3;
16pub use vek::Vec4 as Vector4;
17pub use vek::Mat2 as Matrix2;
18pub use vek::Mat3 as Matrix3;
19pub use vek::Mat4 as Matrix4;
20pub use vek::Quaternion as Quaternion;
21
22pub use self::enums::{Axis2, Axis3, Axis4, Octant, Quadrant, Sign, SignedAxis1,
23 SignedAxis2, SignedAxis3, SignedAxis4};
24
25mod enums {
32 use strum::{EnumCount, EnumIter, FromRepr};
33 #[cfg(feature = "derive_serdes")]
34 use serde::{Deserialize, Serialize};
35
36 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
38 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
39 #[repr(u8)]
40 pub enum Sign {
41 Negative,
42 Zero,
43 Positive
44 }
45
46 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
48 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
49 #[repr(u8)]
50 pub enum Quadrant {
51 PosPos,
53 NegPos,
55 PosNeg,
57 NegNeg
59 }
60
61 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
63 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
64 #[repr(u8)]
65 pub enum Octant {
66 PosPosPos,
68 NegPosPos,
70 PosNegPos,
72 NegNegPos,
74 PosPosNeg,
76 NegPosNeg,
78 PosNegNeg,
80 NegNegNeg
82 }
83
84 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
86 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
87 #[repr(u8)]
88 pub enum Axis2 {
89 X=0,
90 Y
91 }
92
93 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
95 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
96 #[repr(u8)]
97 pub enum Axis3 {
98 X=0,
99 Y,
100 Z
101 }
102
103 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
105 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
106 #[repr(u8)]
107 pub enum Axis4 {
108 X=0,
109 Y,
110 Z,
111 W
112 }
113
114 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
116 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
117 #[repr(u8)]
118 pub enum SignedAxis1 {
119 NegX, PosX
120 }
121
122 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
124 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
125 #[repr(u8)]
126 pub enum SignedAxis2 {
127 NegX, PosX,
128 NegY, PosY
129 }
130
131 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
133 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
134 #[repr(u8)]
135 pub enum SignedAxis3 {
136 NegX, PosX,
137 NegY, PosY,
138 NegZ, PosZ
139 }
140
141 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
143 #[derive(Clone, Copy, Debug, Eq, PartialEq, EnumCount, EnumIter, FromRepr)]
144 #[repr(u8)]
145 pub enum SignedAxis4 {
146 NegX, PosX,
147 NegY, PosY,
148 NegZ, PosZ,
149 NegW, PosW
150 }
151}
152
153#[derive(Debug, PartialEq, Eq)]
159pub struct Addition;
160
161#[derive(Debug, PartialEq, Eq)]
163pub struct Multiplication;
164
165#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
167#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
168pub struct Positive <S> (S);
169
170#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
172#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
173pub struct NonNegative <S> (S);
174
175#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
177#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
178pub struct NonZero <S> (S);
179
180#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
182#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
183pub struct Normalized <S> (S);
184
185#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
187#[derive(Clone, Copy, Debug, Default, PartialEq, PartialOrd, Display)]
188pub struct NormalSigned <S> (S);
189
190#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
192#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd, Display)]
193pub struct Unit <S> (S);
194
195#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
201#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
202pub struct Angles3 <S> {
203 pub yaw : AngleWrapped <S>,
204 pub pitch : AngleWrapped <S>,
205 pub roll : AngleWrapped <S>
206}
207
208#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
210#[derive(Clone, Copy, Debug, Default, Eq, PartialEq)]
211pub struct Pose3 <S> {
212 pub position : Point3 <S>,
213 pub angles : Angles3 <S>
214}
215
216#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
218#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
219#[display("{}", _0)]
220pub struct LinearIso <S, V, W, M> (M, PhantomData <(S, V, W)>);
221
222#[derive(Clone, Copy, Debug, Default, PartialEq, Deref, Display, From)]
224#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
225#[display("{}", _0)]
226pub struct LinearAuto <S, V> (pub LinearIso <S, V, V, V::LinearEndo>) where
227 V : Module <S>,
228 V::LinearEndo : MaybeSerDes,
229 S : Ring;
230
231#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
234#[cfg_attr(feature = "derive_serdes", serde(bound = "S : MaybeSerDes"))]
235#[derive(Clone, Copy, Debug, Default, PartialEq)]
236pub struct Rotation2 <S> (LinearAuto <S, Vector2 <S>>) where S : Ring + MaybeSerDes;
237
238#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
241#[cfg_attr(feature = "derive_serdes", serde(bound = "S : MaybeSerDes"))]
242#[derive(Clone, Copy, Debug, Default, PartialEq)]
243pub struct Rotation3 <S> (LinearAuto <S, Vector3 <S>>) where S : Ring + MaybeSerDes;
244
245#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
247#[derive(Clone, Copy, Debug, Eq, PartialEq)]
248pub struct Versor <S> (Quaternion <S>);
249
250#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
252#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
253#[display("({}, {})", linear_map, translation)]
254pub struct AffineMap <S, A, B, M> where
255 A : AffineSpace <S>,
256 B : AffineSpace <S>,
257 S : Field,
258 M : LinearMap <S, A::Translation, B::Translation>
259{
260 pub linear_map : M,
261 pub translation : B::Translation,
262 pub _phantom : PhantomData <(A, S)>
263}
264
265#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
267#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
268#[display("({}, {})", linear_iso, translation)]
269pub struct Affinity <S, A, B, M> where
270 S : Field,
271 A : AffineSpace <S>,
272 B : AffineSpace <S>,
273 M : LinearMap <S, A::Translation, B::Translation>
274{
275 pub linear_iso : LinearIso <S, A::Translation, B::Translation, M>,
276 pub translation : B::Translation
277}
278
279#[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display)]
281#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
282#[display("{}", _0)]
283pub struct Projectivity <S, P, Q, M> (
284 pub LinearIso <S, P::Translation, Q::Translation, M>
285) where
286 S : Field,
287 P : ProjectiveSpace <S>,
288 Q : ProjectiveSpace <S>;
289
290impl <G : AdditiveGroup> Group <G> for Addition {
298 fn inverse (elem : G) -> G {
299 -elem
300 }
301}
302impl <M : AdditiveMonoid> Monoid <M> for Addition {
303 fn identity() -> M {
304 M::zero()
305 }
306}
307impl <S : std::ops::Add <Output=S>> Semigroup <S> for Addition {
308 fn operation (a : S, b : S) -> S {
309 a + b
310 }
311}
312
313impl <G : MultiplicativeGroup> Group <G> for Multiplication {
317 fn inverse (elem : G) -> G {
318 elem.inv()
319 }
320}
321impl <M : MultiplicativeMonoid> Monoid <M> for Multiplication {
322 fn identity() -> M {
323 M::one()
324 }
325}
326impl <S : std::ops::Mul <Output=S>> Semigroup <S> for Multiplication {
327 fn operation (a : S, b : S) -> S {
328 a * b
329 }
330}
331
332impl <S : OrderedField> Angle <S> for Deg <S> {
336 fn full_turn() -> Self {
337 Deg (S::ten() * S::nine() * S::two() * S::two())
338 }
339 fn half_turn() -> Self {
340 Deg (S::ten() * S::nine() * S::two())
341 }
342}
343impl <S : Real> Trig for Deg <S> {
344 fn sin (self) -> Self {
345 Rad::from (self).sin().into()
346 }
347 fn sin_cos (self) -> (Self, Self) {
348 let (sin, cos) = Rad::from (self).sin_cos();
349 (sin.into(), cos.into())
350 }
351 fn cos (self) -> Self {
352 Rad::from (self).cos().into()
353 }
354 fn tan (self) -> Self {
355 Rad::from (self).tan().into()
356 }
357 fn asin (self) -> Self {
358 Rad::from (self).asin().into()
359 }
360 fn acos (self) -> Self {
361 Rad::from (self).acos().into()
362 }
363 fn atan (self) -> Self {
364 Rad::from (self).atan().into()
365 }
366 fn atan2 (self, other : Self) -> Self {
367 Rad::from (self).atan2 (Rad::from (other)).into()
368 }
369}
370impl <S : Real> From <Rad <S>> for Deg <S> {
371 fn from (radians : Rad <S>) -> Self {
372 let full_turns = radians / Rad::full_turn();
373 Deg::full_turn() * full_turns
374 }
375}
376impl <S : OrderedField> From <Turn <S>> for Deg <S> {
377 fn from (turns : Turn <S>) -> Self {
378 Deg (turns.0 * Deg::full_turn().0)
379 }
380}
381
382impl <S : Real> Angle <S> for Rad <S> {
386 fn full_turn() -> Self {
387 Rad (S::pi() * S::two())
388 }
389}
390impl <S : Trig> Trig for Rad <S> {
391 fn sin (self) -> Self {
392 Rad (self.0.sin())
393 }
394 fn sin_cos (self) -> (Self, Self) {
395 let (sin, cos) = self.0.sin_cos();
396 (Rad (sin), Rad (cos))
397 }
398 fn cos (self) -> Self {
399 Rad (self.0.cos())
400 }
401 fn tan (self) -> Self {
402 Rad (self.0.tan())
403 }
404 fn asin (self) -> Self {
405 Rad (self.0.asin())
406 }
407 fn acos (self) -> Self {
408 Rad (self.0.acos())
409 }
410 fn atan (self) -> Self {
411 Rad (self.0.atan())
412 }
413 fn atan2 (self, other : Self) -> Self {
414 Rad (self.0.atan2 (other.0))
415 }
416}
417impl <S : Real> From <Deg <S>> for Rad <S> {
418 fn from (degrees : Deg <S>) -> Self {
419 let full_turns = degrees / Deg::full_turn();
420 Rad::full_turn() * full_turns
421 }
422}
423impl <S : Real> From <Turn <S>> for Rad <S> {
424 fn from (turns : Turn <S>) -> Self {
425 Rad (turns.0 * Rad::full_turn().0)
426 }
427}
428
429impl <S : OrderedField> Angle <S> for Turn <S> {
433 fn full_turn() -> Self {
434 Turn (S::one())
435 }
436 fn half_turn() -> Self {
437 Turn (S::half())
438 }
439}
440impl <S : Real> Trig for Turn <S> {
441 fn sin (self) -> Self {
442 Rad::from (self).sin().into()
443 }
444 fn sin_cos (self) -> (Self, Self) {
445 let (sin, cos) = Rad::from (self).sin_cos();
446 (sin.into(), cos.into())
447 }
448 fn cos (self) -> Self {
449 Rad::from (self).cos().into()
450 }
451 fn tan (self) -> Self {
452 Rad::from (self).tan().into()
453 }
454 fn asin (self) -> Self {
455 Rad::from (self).asin().into()
456 }
457 fn acos (self) -> Self {
458 Rad::from (self).acos().into()
459 }
460 fn atan (self) -> Self {
461 Rad::from (self).atan().into()
462 }
463 fn atan2 (self, other : Self) -> Self {
464 Rad::from (self).atan2 (Rad::from (other)).into()
465 }
466}
467impl <S : Real> From <Rad <S>> for Turn <S> {
468 fn from (radians : Rad <S>) -> Self {
469 Turn (radians.0 / Rad::full_turn().0)
470 }
471}
472impl <S : OrderedField> From <Deg <S>> for Turn <S> {
473 fn from (degrees : Deg <S>) -> Self {
474 Turn (degrees.0 / Deg::full_turn().0)
475 }
476}
477
478impl <S : Real> AngleWrapped <S> {
482 pub fn wrap (angle : Rad <S>) -> Self {
483 AngleWrapped (angle.wrap_unsigned())
484 }
485
486 #[must_use]
487 pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
488 AngleWrapped (f (self.0).wrap_unsigned())
489 }
490}
491
492impl <S : Real> AngleWrappedSigned <S> {
496 pub fn wrap (angle : Rad <S>) -> Self {
497 AngleWrappedSigned (angle.wrap_signed())
498 }
499
500 #[must_use]
501 pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
502 AngleWrappedSigned (f (self.0).wrap_signed())
503 }
504}
505
506impl <S> Angles3 <S> {
511 pub const fn new (
512 yaw : AngleWrapped <S>,
513 pitch : AngleWrapped <S>,
514 roll : AngleWrapped <S>
515 ) -> Self {
516 Angles3 { yaw, pitch, roll }
517 }
518
519 pub fn zero() -> Self where S : Real {
520 use num::Zero;
521 Angles3::new (
522 AngleWrapped::zero(),
523 AngleWrapped::zero(),
524 AngleWrapped::zero())
525 }
526
527 pub fn wrap (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>) -> Self where S : Real {
528 let yaw = AngleWrapped::wrap (yaw);
529 let pitch = AngleWrapped::wrap (pitch);
530 let roll = AngleWrapped::wrap (roll);
531 Angles3 { yaw, pitch, roll }
532 }
533}
534
535impl <S> From <Rotation3 <S>> for Angles3 <S> where S : Real + MaybeSerDes {
536 fn from (rotation : Rotation3 <S>) -> Self {
537 let (yaw, pitch, roll) = {
538 let (yaw, pitch, roll) = rotation.intrinsic_angles();
539 ( AngleWrapped::wrap (yaw),
540 AngleWrapped::wrap (pitch),
541 AngleWrapped::wrap (roll)
542 )
543 };
544 Angles3 { yaw, pitch, roll }
545 }
546}
547
548impl Axis2 {
552 #[inline]
553 pub const fn component (self) -> usize {
554 self as usize
555 }
556}
557
558impl Axis3 {
562 #[inline]
563 pub const fn component (self) -> usize {
564 self as usize
565 }
566}
567
568impl SignedAxis3 {
572 pub fn to_vec <S : Field> (self) -> Vector3 <S> {
573 match self {
574 SignedAxis3::NegX => [-S::one(), S::zero(), S::zero()].into(),
575 SignedAxis3::PosX => [ S::one(), S::zero(), S::zero()].into(),
576 SignedAxis3::NegY => [ S::zero(), -S::one(), S::zero()].into(),
577 SignedAxis3::PosY => [ S::zero(), S::one(), S::zero()].into(),
578 SignedAxis3::NegZ => [ S::zero(), S::zero(), -S::one() ].into(),
579 SignedAxis3::PosZ => [ S::zero(), S::zero(), S::one() ].into()
580 }
581 }
582 #[inline]
583 pub fn to_unit <S> (self) -> Unit3 <S> where S : Real + std::fmt::Debug {
584 Unit3::unchecked (self.to_vec())
585 }
586}
587impl <S : Field> TryFrom <Vector3 <S>> for SignedAxis3 {
588 type Error = Vector3 <S>;
589 fn try_from (vector : Vector3 <S>) -> Result <Self, Self::Error> {
590 let axis = if vector == Vector3::unit_x() {
591 SignedAxis3::PosX
592 } else if vector == -Vector3::unit_x() {
593 SignedAxis3::NegX
594 } else if vector == Vector3::unit_y() {
595 SignedAxis3::PosY
596 } else if vector == -Vector3::unit_y() {
597 SignedAxis3::NegY
598 } else if vector == Vector3::unit_z() {
599 SignedAxis3::PosZ
600 } else if vector == -Vector3::unit_z() {
601 SignedAxis3::NegZ
602 } else {
603 return Err (vector)
604 };
605 Ok (axis)
606 }
607}
608
609impl Quadrant {
613 pub fn from_vec_strict <S> (vector : Vector2 <S>) -> Option <Quadrant> where
616 S : Ring + SignedExt
617 {
618 let sign_x = vector.x.sign();
619 let sign_y = vector.y.sign();
620 let quadrant = match (sign_x, sign_y) {
621 (Sign::Zero, _) | (_, Sign::Zero) => return None,
622 (Sign::Positive, Sign::Positive) => Quadrant::PosPos,
623 (Sign::Negative, Sign::Positive) => Quadrant::NegPos,
624 (Sign::Positive, Sign::Negative) => Quadrant::PosNeg,
625 (Sign::Negative, Sign::Negative) => Quadrant::NegNeg
626 };
627 Some (quadrant)
628 }
629 #[inline]
631 pub fn from_point_strict <S> (point : Point2 <S>) -> Option <Quadrant> where
632 S : Ring + SignedExt
633 {
634 Quadrant::from_vec_strict (point.to_vector())
635 }
636}
637impl Octant {
643 pub fn from_vec_strict <S> (vector : Vector3 <S>) -> Option <Octant> where
646 S : Ring + SignedExt
647 {
648 let sign_x = vector.x.sign();
649 let sign_y = vector.y.sign();
650 let sign_z = vector.z.sign();
651 let octant = match (sign_x, sign_y, sign_z) {
652 (Sign::Zero, _, _) |
653 ( _, Sign::Zero, _) |
654 ( _, _, Sign::Zero) => return None,
655 (Sign::Positive, Sign::Positive, Sign::Positive) => Octant::PosPosPos,
656 (Sign::Negative, Sign::Positive, Sign::Positive) => Octant::NegPosPos,
657 (Sign::Positive, Sign::Negative, Sign::Positive) => Octant::PosNegPos,
658 (Sign::Negative, Sign::Negative, Sign::Positive) => Octant::NegNegPos,
659 (Sign::Positive, Sign::Positive, Sign::Negative) => Octant::PosPosNeg,
660 (Sign::Negative, Sign::Positive, Sign::Negative) => Octant::NegPosNeg,
661 (Sign::Positive, Sign::Negative, Sign::Negative) => Octant::PosNegNeg,
662 (Sign::Negative, Sign::Negative, Sign::Negative) => Octant::NegNegNeg
663 };
664 Some (octant)
665 }
666 #[inline]
668 pub fn from_point_strict <S> (point : Point3 <S>) -> Option <Octant> where
669 S : Ring + SignedExt
670 {
671 Octant::from_vec_strict (point.to_vector())
672 }
673}
674impl <S> Positive <S> where S : PartialOrd + num::Zero {
680 pub fn new (value : S) -> Option <Self> {
691 if value > S::zero() {
692 Some (Positive (value))
693 } else {
694 None
695 }
696 }
697 pub fn noisy (value : S) -> Self {
706 assert!(value > S::zero());
707 Positive (value)
708 }
709 pub fn unchecked (value : S) -> Self {
718 debug_assert!(value > S::zero());
719 Positive (value)
720 }
721 #[must_use]
742 pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
743 Self::noisy (fun (self.0))
744 }
745}
746impl <S> Positive <S> where S : num::float::FloatCore {
747 pub fn infinity() -> Self {
749 Positive (S::infinity())
750 }
751}
752impl Positive <f32> {
753 pub const fn new_f32 (value : f32) -> Option <Self> {
755 if 0.0 < value {
756 Some (Positive (value))
757 } else {
758 None
759 }
760 }
761 pub const fn noisy_f32 (value : f32) -> Self {
763 assert!(0.0 < value);
764 Positive (value)
765 }
766 pub const fn unchecked_f32 (value : f32) -> Self {
768 debug_assert!(0.0 < value);
769 Positive (value)
770 }
771}
772impl Positive <f64> {
773 pub const fn new_f64 (value : f64) -> Option <Self> {
775 if 0.0 < value {
776 Some (Positive (value))
777 } else {
778 None
779 }
780 }
781 pub const fn noisy_f64 (value : f64) -> Self {
783 assert!(0.0 < value);
784 Positive (value)
785 }
786 pub const fn unchecked_f64 (value : f64) -> Self {
788 debug_assert!(0.0 < value);
789 Positive (value)
790 }
791}
792impl <S : Ring> num::One for Positive <S> {
793 fn one() -> Self {
794 Positive (S::one())
795 }
796}
797impl <S> std::ops::Deref for Positive <S> {
798 type Target = S;
799 fn deref (&self) -> &S {
800 &self.0
801 }
802}
803impl <S : Ring> std::ops::Mul <Positive <S>> for Positive <S> {
804 type Output = Positive <S>;
805 fn mul (self, rhs : Positive <S>) -> Self::Output {
806 Positive (self.0 * *rhs)
807 }
808}
809impl <S : Ring> std::ops::MulAssign <Positive <S>> for Positive <S> {
810 fn mul_assign (&mut self, rhs : Positive <S>) {
811 self.0 *= *rhs
812 }
813}
814impl <S : Ring> std::ops::Mul <NonNegative <S>> for Positive <S> {
815 type Output = NonNegative <S>;
816 fn mul (self, rhs : NonNegative <S>) -> Self::Output {
817 NonNegative (self.0 * *rhs)
818 }
819}
820impl <S : Ring> std::ops::MulAssign <NonNegative <S>> for Positive <S> {
821 fn mul_assign (&mut self, rhs : NonNegative <S>) {
822 self.0 *= *rhs
823 }
824}
825impl <S : Field> std::ops::Div for Positive <S> {
826 type Output = Self;
827 fn div (self, rhs : Self) -> Self::Output {
828 Positive (self.0 / rhs.0)
829 }
830}
831impl <S : Field> std::ops::DivAssign for Positive <S> {
832 fn div_assign (&mut self, rhs : Self) {
833 self.0 /= rhs.0
834 }
835}
836impl <S : Ring> std::ops::Add for Positive <S> {
837 type Output = Self;
838 fn add (self, rhs : Self) -> Self::Output {
839 Positive (self.0 + rhs.0)
840 }
841}
842impl <S : Ring> std::ops::AddAssign for Positive <S> {
843 fn add_assign (&mut self, rhs : Self) {
844 self.0 += rhs.0
845 }
846}
847impl <S : Ring> std::ops::Add <NonNegative <S>> for Positive <S> {
848 type Output = Self;
849 fn add (self, rhs : NonNegative <S>) -> Self::Output {
850 Positive (self.0 + rhs.0)
851 }
852}
853impl <S : Ring> std::ops::AddAssign <NonNegative <S>> for Positive <S> {
854 fn add_assign (&mut self, rhs : NonNegative <S>) {
855 self.0 += rhs.0
856 }
857}
858impl <S> NonNegative <S> where S : PartialOrd + num::Zero {
864 pub fn new (value : S) -> Option <Self> {
875 if value >= S::zero() {
876 Some (NonNegative (value))
877 } else {
878 None
879 }
880 }
881 pub fn noisy (value : S) -> Self {
890 assert!(S::zero() <= value);
891 NonNegative (value)
892 }
893 pub fn unchecked (value : S) -> Self {
903 debug_assert!(value >= S::zero());
904 NonNegative (value)
905 }
906 #[must_use]
924 pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
925 Self::noisy (fun (self.0))
926 }
927}
928impl <S> NonNegative <S> where S : num::Signed {
929 pub fn abs (value : S) -> Self {
939 NonNegative (value.abs())
940 }
941 #[must_use]
951 pub fn map_abs <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
952 Self::abs (fun (self.0))
953 }
954}
955impl <S : Sqrt> Sqrt for NonNegative <S> {
956 fn sqrt (self) -> Self {
957 NonNegative (self.0.sqrt())
958 }
959}
960impl <S : MinMax> MinMax for NonNegative <S> {
961 fn min (self, other : Self) -> Self {
962 NonNegative (S::min (self.0, other.0))
963 }
964 fn max (self, other : Self) -> Self {
965 NonNegative (S::max (self.0, other.0))
966 }
967 fn clamp (self, min : Self, max : Self) -> Self {
968 NonNegative (self.0.clamp (min.0, max.0))
969 }
970}
971impl NonNegative <f32> {
972 pub const fn new_f32 (value : f32) -> Option <Self> {
974 if 0.0 <= value {
975 Some (NonNegative (value))
976 } else {
977 None
978 }
979 }
980 pub const fn noisy_f32 (value : f32) -> Self {
982 assert!(0.0 <= value);
983 NonNegative (value)
984 }
985 pub const fn unchecked_f32 (value : f32) -> Self {
987 debug_assert!(0.0 <= value);
988 NonNegative (value)
989 }
990}
991impl NonNegative <f64> {
992 pub const fn new_f64 (value : f64) -> Option <Self> {
994 if 0.0 <= value {
995 Some (NonNegative (value))
996 } else {
997 None
998 }
999 }
1000 pub const fn noisy_f64 (value : f64) -> Self {
1002 assert!(0.0 <= value);
1003 NonNegative (value)
1004 }
1005 pub const fn unchecked_f64 (value : f64) -> Self {
1007 debug_assert!(0.0 <= value);
1008 NonNegative (value)
1009 }
1010}
1011impl <S : Ring> From <Positive <S>> for NonNegative <S> {
1012 fn from (positive : Positive <S>) -> Self {
1013 NonNegative (*positive)
1014 }
1015}
1016impl <S : Ring> num::Zero for NonNegative <S> {
1017 fn zero() -> Self {
1018 NonNegative (S::zero())
1019 }
1020 fn is_zero (&self) -> bool {
1021 self.0.is_zero()
1022 }
1023}
1024impl <S : Field> num::One for NonNegative <S> {
1025 fn one() -> Self {
1026 NonNegative (S::one())
1027 }
1028}
1029impl <S> std::ops::Deref for NonNegative <S> {
1030 type Target = S;
1031 fn deref (&self) -> &S {
1032 &self.0
1033 }
1034}
1035impl <S> std::ops::Mul for NonNegative <S> where S : std::ops::Mul <Output=S> {
1036 type Output = Self;
1037 fn mul (self, rhs : Self) -> Self::Output {
1038 NonNegative (self.0 * rhs.0)
1039 }
1040}
1041impl <S> std::ops::Mul <Positive <S>> for NonNegative <S> where
1042 S : std::ops::Mul <Output=S>
1043{
1044 type Output = Self;
1045 fn mul (self, rhs : Positive <S>) -> Self::Output {
1046 NonNegative (self.0 * rhs.0)
1047 }
1048}
1049impl <S> std::ops::Div for NonNegative <S> where S : std::ops::Div <Output=S> {
1050 type Output = Self;
1051 fn div (self, rhs : Self) -> Self::Output {
1052 NonNegative (self.0 / rhs.0)
1053 }
1054}
1055impl <S> std::ops::Add for NonNegative <S> where S : std::ops::Add <Output=S> {
1056 type Output = Self;
1057 fn add (self, rhs : Self) -> Self::Output {
1058 NonNegative (self.0 + rhs.0)
1059 }
1060}
1061impl <S> NonZero <S> where S : PartialEq + num::Zero {
1067 #[inline]
1078 pub fn new (value : S) -> Option <Self> {
1079 if S::zero() != value {
1080 Some (NonZero (value))
1081 } else {
1082 None
1083 }
1084 }
1085 #[inline]
1094 pub fn noisy (value : S) -> Self where S : std::fmt::Debug {
1095 assert_ne!(S::zero(), value);
1096 NonZero (value)
1097 }
1098 #[inline]
1107 pub fn unchecked (value : S) -> Self where S : std::fmt::Debug {
1108 debug_assert_ne!(S::zero(), value);
1109 NonZero (value)
1110 }
1111 #[inline]
1131 #[must_use]
1132 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1133 S : std::fmt::Debug,
1134 F : FnMut (S) -> S
1135 {
1136 Self::noisy (fun (self.0))
1137 }
1138}
1139impl NonZero <f32> {
1140 pub const fn new_f32 (value : f32) -> Option <Self> {
1142 if value != 0.0 {
1143 Some (NonZero (value))
1144 } else {
1145 None
1146 }
1147 }
1148 pub const fn noisy_f32 (value : f32) -> Self {
1150 assert!(value != 0.0);
1151 NonZero (value)
1152 }
1153 pub const fn unchecked_f32 (value : f32) -> Self {
1155 debug_assert!(value != 0.0);
1156 NonZero (value)
1157 }
1158}
1159impl NonZero <f64> {
1160 pub const fn new_f64 (value : f64) -> Option <Self> {
1162 if value != 0.0 {
1163 Some (NonZero (value))
1164 } else {
1165 None
1166 }
1167 }
1168 pub const fn noisy_f64 (value : f64) -> Self {
1170 assert!(value != 0.0);
1171 NonZero (value)
1172 }
1173 pub const fn unchecked_f64 (value : f64) -> Self {
1175 debug_assert!(value != 0.0);
1176 NonZero (value)
1177 }
1178}
1179impl <S : MultiplicativeMonoid> num::One for NonZero <S> {
1180 fn one() -> Self {
1181 NonZero (S::one())
1182 }
1183}
1184impl <S> std::ops::Deref for NonZero <S> {
1185 type Target = S;
1186 fn deref (&self) -> &S {
1187 &self.0
1188 }
1189}
1190impl <S : MultiplicativeMonoid> std::ops::Mul for NonZero <S> {
1191 type Output = Self;
1192 fn mul (self, rhs : Self) -> Self::Output {
1193 NonZero (self.0 * rhs.0)
1194 }
1195}
1196impl <S : PartialEq> Eq for NonZero <S> { }
1197impl <S> Normalized <S> where S : num::One + num::Zero {
1203 #[inline]
1205 pub fn zero() -> Self {
1206 Normalized (S::zero())
1207 }
1208 #[inline]
1209 pub fn is_zero (&self) -> bool {
1210 self.0.is_zero()
1211 }
1212 #[inline]
1226 pub fn new (value : S) -> Option <Self> where S : PartialOrd {
1227 if S::zero() <= value && value <= S::one() {
1228 Some (Normalized (value))
1229 } else {
1230 None
1231 }
1232 }
1233 #[inline]
1244 #[expect(clippy::same_name_method)]
1245 pub fn clamp (value : S) -> Self where S : MinMax {
1246 Normalized (value.clamp (S::zero(), S::one()))
1247 }
1248 #[inline]
1262 pub fn noisy (value : S) -> Self where S : PartialOrd {
1263 assert!(value <= S::one());
1264 assert!(S::zero() <= value);
1265 Normalized (value)
1266 }
1267 pub fn unchecked (value : S) -> Self where S : PartialOrd {
1277 debug_assert!(value <= S::one());
1278 debug_assert!(S::zero() <= value);
1279 Normalized (value)
1280 }
1281 #[inline]
1293 #[must_use]
1294 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1295 S : MinMax,
1296 F : FnMut (S) -> S
1297 {
1298 Self::clamp (fun (self.0))
1299 }
1300 #[inline]
1327 #[must_use]
1328 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1329 S : PartialOrd,
1330 F : FnMut (S) -> S
1331 {
1332 Self::noisy (fun (self.0))
1333 }
1334}
1335impl Normalized <f32> {
1336 pub const fn new_f32 (value : f32) -> Option <Self> {
1338 if 0.0 <= value && value <= 1.0 {
1339 Some (Normalized (value))
1340 } else {
1341 None
1342 }
1343 }
1344 pub const fn noisy_f32 (value : f32) -> Self {
1346 assert!(value <= 1.0);
1347 assert!(0.0 <= value);
1348 Normalized (value)
1349 }
1350 pub const fn unchecked_f32 (value : f32) -> Self {
1352 debug_assert!(value <= 1.0);
1353 debug_assert!(0.0 <= value);
1354 Normalized (value)
1355 }
1356}
1357impl Normalized <f64> {
1358 pub const fn new_f64 (value : f64) -> Option <Self> {
1360 if 0.0 <= value && value <= 1.0 {
1361 Some (Normalized (value))
1362 } else {
1363 None
1364 }
1365 }
1366 pub const fn noisy_f64 (value : f64) -> Self {
1368 assert!(value <= 1.0);
1369 assert!(0.0 <= value);
1370 Normalized (value)
1371 }
1372 pub const fn unchecked_f64 (value : f64) -> Self {
1374 debug_assert!(value <= 1.0);
1375 debug_assert!(0.0 <= value);
1376 Normalized (value)
1377 }
1378}
1379impl <S : Ring> num::One for Normalized <S> {
1380 fn one() -> Self {
1381 Normalized (S::one())
1382 }
1383}
1384impl <S> std::ops::Deref for Normalized <S> {
1385 type Target = S;
1386 fn deref (&self) -> &S {
1387 &self.0
1388 }
1389}
1390impl <S : Ring> std::ops::Mul for Normalized <S> {
1391 type Output = Self;
1392 fn mul (self, rhs : Self) -> Self::Output {
1393 Normalized (self.0 * rhs.0)
1394 }
1395}
1396impl <S : PartialEq> Eq for Normalized <S> { }
1397#[expect(clippy::derive_ord_xor_partial_ord)]
1398impl <S : PartialOrd> Ord for Normalized <S> {
1399 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1400 self.partial_cmp (other).unwrap()
1402 }
1403}
1404impl <S> NormalSigned <S> {
1410 #[inline]
1412 pub fn zero() -> Self where S : num::Zero {
1413 NormalSigned (S::zero())
1414 }
1415 #[inline]
1416 pub fn is_zero (&self) -> bool where S : num::Zero {
1417 self.0.is_zero()
1418 }
1419 #[inline]
1433 pub fn new (value : S) -> Option <Self> where S : OrderedRing {
1434 if -S::one() <= value && value <= S::one() {
1435 Some (NormalSigned (value))
1436 } else {
1437 None
1438 }
1439 }
1440 #[inline]
1451 #[expect(clippy::same_name_method)]
1452 pub fn clamp (value : S) -> Self where S : Ring + MinMax {
1453 NormalSigned (value.clamp (-S::one(), S::one()))
1454 }
1455 #[inline]
1469 pub fn noisy (value : S) -> Self where S : OrderedRing {
1470 assert!(value <= S::one());
1471 assert!(-S::one() <= value);
1472 NormalSigned (value)
1473 }
1474 #[inline]
1486 #[must_use]
1487 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1488 S : Ring + MinMax,
1489 F : FnMut (S) -> S
1490 {
1491 Self::clamp (fun (self.0))
1492 }
1493 #[inline]
1520 #[must_use]
1521 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1522 S : OrderedRing,
1523 F : FnMut (S) -> S
1524 {
1525 Self::noisy (fun (self.0))
1526 }
1527}
1528impl NormalSigned <f32> {
1529 pub const fn new_f32 (value : f32) -> Option <Self> {
1531 if -1.0 <= value && value <= 1.0 {
1532 Some (NormalSigned (value))
1533 } else {
1534 None
1535 }
1536 }
1537 pub const fn noisy_f32 (value : f32) -> Self {
1539 assert!(value <= 1.0);
1540 assert!(-1.0 <= value);
1541 NormalSigned (value)
1542 }
1543 pub const fn unchecked_f32 (value : f32) -> Self {
1545 debug_assert!(value <= 1.0);
1546 debug_assert!(-1.0 <= value);
1547 NormalSigned (value)
1548 }
1549}
1550impl NormalSigned <f64> {
1551 pub const fn new_f64 (value : f64) -> Option <Self> {
1553 if -1.0 <= value && value <= 1.0 {
1554 Some (NormalSigned (value))
1555 } else {
1556 None
1557 }
1558 }
1559 pub const fn noisy_f64 (value : f64) -> Self {
1561 assert!(value <= 1.0);
1562 assert!(-1.0 <= value);
1563 NormalSigned (value)
1564 }
1565 pub const fn unchecked_f64 (value : f64) -> Self {
1567 debug_assert!(value <= 1.0);
1568 debug_assert!(-1.0 <= value);
1569 NormalSigned (value)
1570 }
1571}
1572impl <S : OrderedField> From <Normalized <S>> for NormalSigned <S> {
1573 fn from (Normalized (value) : Normalized <S>) -> Self {
1574 debug_assert!(S::zero() <= value);
1575 debug_assert!(value <= S::one());
1576 NormalSigned (value)
1577 }
1578}
1579impl <S : Field> num::One for NormalSigned <S> {
1580 fn one() -> Self {
1581 NormalSigned (S::one())
1582 }
1583}
1584impl <S> std::ops::Deref for NormalSigned <S> {
1585 type Target = S;
1586 fn deref (&self) -> &S {
1587 &self.0
1588 }
1589}
1590impl <S : Field> std::ops::Mul for NormalSigned <S> {
1591 type Output = Self;
1592 fn mul (self, rhs : Self) -> Self::Output {
1593 NormalSigned (self.0 * rhs.0)
1594 }
1595}
1596impl <S : Field> Eq for NormalSigned <S> { }
1597#[expect(clippy::derive_ord_xor_partial_ord)]
1598impl <S : OrderedField> Ord for NormalSigned <S> {
1599 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1600 self.partial_cmp (other).unwrap()
1602 }
1603}
1604impl <S : Field> std::ops::Neg for NormalSigned <S> {
1605 type Output = Self;
1606 fn neg (self) -> Self {
1607 NormalSigned (-self.0)
1608 }
1609}
1610impl <S : Ring> Vector2Ext <S> for Vector2 <S> {
1616 fn exterior_product (self, rhs : Vector2 <S>) -> S {
1618 self.x * rhs.y - self.y * rhs.x
1619 }
1620}
1621fn ortho_vec2 <S> (vector : Vector2 <S>) -> Vector2 <S> {
1622 vector.yx()
1623}
1624
1625fn ortho_vec3 <S> (vector : Vector3 <S>) -> Vector3 <S> where
1629 S : Ring + approx::AbsDiffEq <Epsilon = S>
1630{
1631 let mut ortho = vector.cross (Vector3::unit_x());
1635 if approx::abs_diff_eq!(ortho, Vector3::zero()) {
1636 ortho = vector.cross (Vector3::unit_y())
1637 }
1638 ortho
1639}
1640
1641fn ortho_vec4 <S> (_vector : Vector4 <S>) -> Vector4 <S> {
1645 unimplemented!("TODO")
1646}
1647
1648impl <S> std::ops::Deref for Unit <S> {
1652 type Target = S;
1653 fn deref (&self) -> &S {
1654 &self.0
1655 }
1656}
1657
1658impl <S> Rotation2 <S> where S : Ring + MaybeSerDes {
1662 pub fn new (mat : Matrix2 <S>) -> Option <Self> {
1667 let transform = LinearAuto (LinearIso::new (mat)?);
1668 if !transform.is_rotation() {
1669 None
1670 } else {
1671 Some (Rotation2 (transform))
1672 }
1673 }
1674 pub fn from_angle (angle : Rad <S>) -> Self where S : num::real::Real {
1676 Rotation2 (LinearAuto (LinearIso (Matrix2::rotation_z (angle.0), PhantomData)))
1677 }
1678}
1679impl <S> From <Rad <S>> for Rotation2 <S> where
1680 S : Ring + num::real::Real + MaybeSerDes
1681{
1682 fn from (angle : Rad <S>) -> Self {
1683 Rotation2::from_angle (angle)
1684 }
1685}
1686
1687impl <S> Rotation3 <S> where S : Ring + MaybeSerDes {
1691 pub fn new (mat : Matrix3 <S>) -> Option <Self> {
1696 if mat * mat.transposed() != Matrix3::identity() || mat.determinant() != S::one() {
1697 None
1698 } else {
1699 Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1700 }
1701 }
1702 pub fn from_angle_x (angle : Rad <S>) -> Self where S : num::real::Real {
1706 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_x (angle.0), PhantomData)))
1707 }
1708 pub fn from_angle_y (angle : Rad <S>) -> Self where S : num::real::Real {
1712 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_y (angle.0), PhantomData)))
1713 }
1714 pub fn from_angle_z (angle : Rad <S>) -> Self where S : num::real::Real {
1718 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_z (angle.0), PhantomData)))
1719 }
1720 pub fn from_angles_intrinsic (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>)
1726 -> Self where S : num::real::Real
1727 {
1728 let rotation1 = Rotation3::from_angle_z (yaw);
1729 let rotation2 = Rotation3::from_axis_angle (rotation1.cols.x, pitch) * rotation1;
1730 Rotation3::from_axis_angle (rotation2.cols.y, roll) * rotation2
1731 }
1732 pub fn from_axis_angle (axis : Vector3 <S>, angle : Rad <S>) -> Self where
1734 S : num::real::Real
1735 {
1736 Rotation3 (LinearAuto (LinearIso (
1737 Matrix3::rotation_3d (angle.0, axis), PhantomData)))
1738 }
1739 pub fn orthonormalize (v1 : Vector3 <S>, v2 : Vector3 <S>, v3 : Vector3 <S>)
1742 -> Option <Self> where S : OrderedField + Sqrt
1743 {
1744 if Matrix3::from_col_arrays ([v1, v2, v3].map (Vector3::into_array)).determinant()
1745 == S::zero()
1746 {
1747 None
1748 } else {
1749 let project = |u : Vector3 <S>, v : Vector3 <S>| u * (u.dot (v) / *u.self_dot());
1750 let u1 = v1;
1751 let u2 = v2 - project (u1, v2);
1752 let u3 = v3 - project (u1, v3) - project (u2, v3);
1753 Some (Rotation3 (LinearAuto (LinearIso (Matrix3::from_col_arrays ([
1754 u1.normalize().into_array(),
1755 u2.normalize().into_array(),
1756 u3.normalize().into_array()
1757 ]), PhantomData))))
1758 }
1759 }
1760
1761 pub fn look_at (point : Point3 <S>) -> Self where
1765 S : num::Float + num::FloatConst + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1766 {
1767 if point == Point::origin() {
1768 Self::identity()
1769 } else {
1770 use num::Zero;
1771 let forward = point.0.normalized();
1772 let right = {
1773 let projected = forward.with_z (S::zero());
1774 if !projected.is_zero() {
1775 Self::from_angle_z (-Rad (S::FRAC_PI_2())) * projected.normalized()
1776 } else {
1777 Vector3::unit_x()
1778 }
1779 };
1780 let up = right.cross (forward);
1781 let mat =
1782 Matrix3::from_col_arrays ([right, forward, up].map (Vector3::into_array));
1783 Self::noisy_approx (mat)
1784 }
1785 }
1786
1787 pub fn intrinsic_angles (&self) -> (Rad <S>, Rad <S>, Rad <S>) where S : Real {
1792 let cols = &self.cols;
1793 let yaw = Rad (S::atan2 (-cols.y.x, cols.y.y));
1794 let pitch = Rad (S::atan2 (cols.y.z, S::sqrt (S::one() - cols.y.z * cols.y.z)));
1795 let roll = Rad (S::atan2 (-cols.x.z, cols.z.z));
1796 (yaw, pitch, roll)
1797 }
1798
1799 pub fn versor (self) -> Versor <S> where
1803 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
1804 {
1805 let diagonal = self.diagonal();
1806 let t = diagonal.sum();
1807 let m =
1808 MinMax::max (MinMax::max (MinMax::max (diagonal.x, diagonal.y), diagonal.z), t);
1809 let qmax = S::half() * Sqrt::sqrt (S::one() - t + S::two() * m);
1810 let qmax4_recip = (S::four() * qmax).recip();
1811 let [qx, qy, qz, qw] : [S; 4];
1812 let cols = self.cols;
1813 if m == diagonal.x {
1814 qx = qmax;
1815 qy = qmax4_recip * (cols.x.y + cols.y.x);
1816 qz = qmax4_recip * (cols.x.z + cols.z.x);
1817 qw = -qmax4_recip * (cols.z.y - cols.y.z);
1818 } else if m == diagonal.y {
1819 qx = qmax4_recip * (cols.x.y + cols.y.x);
1820 qy = qmax;
1821 qz = qmax4_recip * (cols.y.z + cols.z.y);
1822 qw = -qmax4_recip * (cols.x.z - cols.z.x);
1823 } else if m == diagonal.z {
1824 qx = qmax4_recip * (cols.x.z + cols.z.x);
1825 qy = qmax4_recip * (cols.y.z + cols.z.y);
1826 qz = qmax;
1827 qw = -qmax4_recip * (cols.y.x - cols.x.y);
1828 } else {
1829 qx = -qmax4_recip * (cols.z.y - cols.y.z);
1830 qy = -qmax4_recip * (cols.x.z - cols.z.x);
1831 qz = -qmax4_recip * (cols.y.x - cols.x.y);
1832 qw = qmax;
1833 }
1834 let quat = Quaternion::from_xyzw (qx, qy, qz, qw);
1835 Versor::unchecked_approx (quat)
1836 }
1837}
1838impl <S> From <Angles3 <S>> for Rotation3 <S> where
1839 S : Real + num::real::Real + MaybeSerDes
1840{
1841 fn from (angles : Angles3 <S>) -> Self {
1842 Rotation3::from_angles_intrinsic (
1843 angles.yaw.angle(), angles.pitch.angle(), angles.roll.angle())
1844 }
1845}
1846
1847impl <S : Real> Versor <S> {
1851 pub fn normalize (quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
1855 assert_ne!(quaternion, Quaternion::zero());
1856 Versor (normalize_quaternion (quaternion))
1857 }
1858 pub fn noisy (unit_quaternion : Quaternion <S>) -> Self where
1862 S : num::real::Real + std::fmt::Debug
1863 {
1864 assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
1865 Versor (unit_quaternion)
1866 }
1867 pub fn noisy_approx (unit_quaternion : Quaternion <S>) -> Self where
1871 S : num::real::Real + approx::RelativeEq <Epsilon=S>
1872 {
1873 assert!(unit_quaternion.into_vec4().is_normalized());
1874 Versor (unit_quaternion)
1875 }
1876 pub fn unchecked (unit_quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
1880 debug_assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
1881 Versor (unit_quaternion)
1882 }
1883 pub fn unchecked_approx (unit_quaternion : Quaternion <S>) -> Self where
1887 S : num::real::Real + approx::RelativeEq <Epsilon=S>
1888 {
1889 debug_assert!(unit_quaternion.into_vec4().is_normalized());
1890 Versor (unit_quaternion)
1891 }
1892 pub fn from_scaled_axis (scaled_axis : Vector3 <S>) -> Self where
1895 S : std::fmt::Debug + num::Float
1896 {
1897 if scaled_axis == Vector3::zero() {
1898 let versor = Quaternion::rotation_3d (S::zero(), scaled_axis);
1899 debug_assert_eq!(versor.magnitude(), S::one());
1900 Versor (versor)
1901 } else {
1902 let angle = scaled_axis.magnitude();
1903 debug_assert!(S::zero() < angle);
1904 let axis = scaled_axis / angle;
1905 Versor (Quaternion::rotation_3d (angle, axis))
1906 }
1907 }
1908}
1909impl <S> Default for Versor <S> where S : num::One + num::Zero {
1910 fn default() -> Self {
1911 Versor (Quaternion::default())
1912 }
1913}
1914impl <S> std::ops::Mul <Vector3 <S>> for Versor <S> where S : Real + num::Float {
1915 type Output = Vector3 <S>;
1916 fn mul (self, rhs : Vector3 <S>) -> Self::Output {
1917 self.0 * rhs
1918 }
1919}
1920impl <S> From <Rotation3 <S>> for Versor <S> where
1921 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
1922{
1923 fn from (rot : Rotation3 <S>) -> Self {
1924 rot.versor()
1925 }
1926}
1927impl <S> std::ops::Deref for Versor <S> {
1928 type Target = Quaternion <S>;
1929 fn deref (&self) -> &Quaternion <S> {
1930 &self.0
1931 }
1932}
1933impl <S, V, W, M> LinearIso <S, V, W, M> where
1939 M : LinearMap <S, V, W> + Copy,
1940 V : Module <S>,
1941 W : Module <S>,
1942 S : Ring
1943{
1944 pub fn mat (self) -> M {
1945 *self
1946 }
1947 pub fn is_invertible (linear_map : M) -> bool {
1949 linear_map.determinant() != S::zero()
1950 }
1951 pub fn is_invertible_approx (linear_map : M, epsilon : Option <S>) -> bool where
1953 S : approx::AbsDiffEq <Epsilon=S>
1954 {
1955 let epsilon = epsilon.unwrap_or_else (||{
1956 let four = S::one() + S::one() + S::one() + S::one();
1957 S::default_epsilon() * four
1958 });
1959 approx::abs_diff_ne!(linear_map.determinant(), S::zero(), epsilon=epsilon)
1960 }
1961 pub fn new (linear_map : M) -> Option <Self> {
1963 if Self::is_invertible (linear_map) {
1964 Some (LinearIso (linear_map, PhantomData))
1965 } else {
1966 None
1967 }
1968 }
1969 pub fn new_approx (linear_map : M, epsilon : Option <S>) -> Option <Self> where
1971 S : approx::RelativeEq <Epsilon=S>
1972 {
1973 if Self::is_invertible_approx (linear_map, epsilon) {
1974 Some (LinearIso (linear_map, PhantomData))
1975 } else {
1976 None
1977 }
1978 }
1979}
1980impl <S, V, W, M> std::ops::Deref for LinearIso <S, V, W, M> where
1981 M : LinearMap <S, V, W>,
1982 V : Module <S>,
1983 W : Module <S>,
1984 S : Ring
1985{
1986 type Target = M;
1987 fn deref (&self) -> &Self::Target {
1988 &self.0
1989 }
1990}
1991impl <S, V> num::One for LinearIso <S, V, V, V::LinearEndo> where
1992 V : Module <S>,
1993 S : Ring
1994{
1995 fn one () -> Self {
1996 LinearIso (V::LinearEndo::one(), PhantomData)
1997 }
1998}
1999impl <S, V, W, M> std::ops::Mul <V> for LinearIso <S, V, W, M> where
2000 M : LinearMap <S, V, W>,
2001 V : Module <S>,
2002 W : Module <S>,
2003 S : Ring
2004{
2005 type Output = W;
2006 fn mul (self, rhs : V) -> Self::Output {
2007 self.0 * rhs
2008 }
2009}
2010impl <S, V> std::ops::Mul for LinearIso <S, V, V, V::LinearEndo> where
2011 V : Module <S>,
2012 S : Ring
2013{
2014 type Output = Self;
2015 fn mul (self, rhs : Self) -> Self::Output {
2016 LinearIso (self.0 * rhs.0, PhantomData)
2017 }
2018}
2019impl <S, V> std::ops::MulAssign for LinearIso <S, V, V, V::LinearEndo> where
2020 V : Module <S>,
2021 S : Ring
2022{
2023 fn mul_assign (&mut self, rhs : Self) {
2024 self.0 *= rhs.0
2025 }
2026}
2027
2028impl <S, V> LinearAuto <S, V> where
2032 V : Module <S>,
2033 V::LinearEndo : MaybeSerDes,
2034 S : Ring
2035{
2036 pub fn mat (self) -> V::LinearEndo {
2037 **self
2038 }
2039 pub fn is_orthonormal (&self) -> bool {
2044 use num::One;
2045 let determinant = self.0.0.determinant();
2046 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2047 (determinant == S::one() || determinant == -S::one())
2048 }
2049 pub fn is_orthonormal_approx (&self) -> bool where
2055 S : approx::RelativeEq <Epsilon=S>,
2056 V::LinearEndo : approx::RelativeEq <Epsilon=S>
2057 {
2058 use num::One;
2059 let four = S::one() + S::one() + S::one() + S::one();
2060 let epsilon = S::default_epsilon() * four;
2061 let max_relative = S::default_max_relative() * four;
2062 let determinant = self.0.0.determinant();
2063 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2064 max_relative=max_relative, epsilon=epsilon) &&
2065 ( approx::relative_eq!(determinant, S::one(), max_relative=max_relative) ||
2066 approx::relative_eq!(determinant, -S::one(), max_relative=max_relative) )
2067 }
2068 pub fn is_rotation (&self) -> bool {
2072 use num::One;
2073 let determinant = self.0.0.determinant();
2074 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
2075 determinant == S::one()
2076 }
2077 pub fn is_rotation_approx (&self) -> bool where
2082 S : approx::RelativeEq <Epsilon=S>,
2083 V::LinearEndo : approx::RelativeEq <Epsilon=S>
2084 {
2085 use num::One;
2086 let four = S::one() + S::one() + S::one() + S::one();
2087 let epsilon = S::default_epsilon() * four;
2088 let max_relative = S::default_max_relative() * four;
2089 let determinant = self.0.0.determinant();
2090 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2091 max_relative=max_relative, epsilon=epsilon) &&
2092 approx::relative_eq!(determinant, S::one(), max_relative=max_relative)
2093 }
2094}
2095impl <S, V> std::ops::Mul <V> for LinearAuto <S, V> where
2096 V : Module <S>,
2097 V::LinearEndo : MaybeSerDes,
2098 S : Ring
2099{
2100 type Output = V;
2101 fn mul (self, rhs : V) -> Self::Output {
2102 self.0 * rhs
2103 }
2104}
2105impl <S, V> std::ops::Mul for LinearAuto <S, V> where
2106 V : Module <S>,
2107 V::LinearEndo : MaybeSerDes,
2108 S : Ring
2109{
2110 type Output = Self;
2111 fn mul (self, rhs : Self) -> Self::Output {
2112 LinearAuto (self.0 * rhs.0)
2113 }
2114}
2115impl <S, V> std::ops::MulAssign <Self> for LinearAuto <S, V> where
2116 V : Module <S>,
2117 V::LinearEndo : MaybeSerDes,
2118 S : Ring
2119{
2120 fn mul_assign (&mut self, rhs : Self) {
2121 self.0 *= rhs.0
2122 }
2123}
2124impl <S, V> num::One for LinearAuto <S, V> where
2125 V : Module <S>,
2126 V::LinearEndo : MaybeSerDes,
2127 S : Ring
2128{
2129 fn one() -> Self {
2130 LinearAuto (LinearIso::one())
2131 }
2132}
2133
2134impl <S, A, B, M> AffineMap <S, A, B, M> where
2138 A : AffineSpace <S>,
2139 B : AffineSpace <S>,
2140 M : LinearMap <S, A::Translation, B::Translation>,
2141 S : Field
2142{
2143 pub const fn new (linear_map : M, translation : B::Translation) -> Self {
2144 AffineMap { linear_map, translation, _phantom: PhantomData }
2145 }
2146 pub fn map (self, point : A) -> B {
2147 B::from_vector (self.linear_map * point.to_vector() + self.translation)
2148 }
2149 pub fn transform (self, translation : A::Translation) -> B::Translation {
2150 self.linear_map * translation
2151 }
2152}
2153
2154impl <S, A, B, M> Affinity <S, A, B, M> where
2158 M : LinearMap <S, A::Translation, B::Translation>,
2159 A : AffineSpace <S>,
2160 B : AffineSpace <S>,
2161 S : Field
2162{
2163 pub const fn new (
2164 linear_iso : LinearIso <S, A::Translation, B::Translation, M>,
2165 translation : B::Translation
2166 ) -> Self {
2167 Affinity { linear_iso, translation }
2168 }
2169 pub fn mat (self) -> M {
2170 *self.linear_iso
2171 }
2172 pub fn map (self, point : A) -> B {
2173 B::from_vector (self.linear_iso * point.to_vector() + self.translation)
2174 }
2175 pub fn transform (self, translation : A::Translation) -> B::Translation {
2176 self.linear_iso * translation
2177 }
2178}
2179
2180impl <S, P, Q, M> Projectivity <S, P, Q, M> where
2184 M : LinearMap <S, P::Translation, Q::Translation>,
2185 P : ProjectiveSpace <S>,
2186 Q : ProjectiveSpace <S>,
2187 S : Field
2188{
2189 pub const fn new (linear_iso : LinearIso <S, P::Translation, Q::Translation, M>)
2190 -> Self
2191 {
2192 Projectivity (linear_iso)
2193 }
2194 pub fn mat (self) -> M {
2195 **self
2196 }
2197}
2198impl <S, P, Q, M> std::ops::Deref for Projectivity <S, P, Q, M> where
2199 M : LinearMap <S, P::Translation, Q::Translation>,
2200 P : ProjectiveSpace <S>,
2201 Q : ProjectiveSpace <S>,
2202 S : Field
2203{
2204 type Target = LinearIso <S, P::Translation, Q::Translation, M>;
2205 fn deref (&self) -> &Self::Target {
2206 &self.0
2207 }
2208}
2209impl <S, P, Q, M> std::ops::Mul <P> for Projectivity <S, P, Q, M> where
2210 M : LinearMap <S, P::Translation, Q::Translation>,
2211 P : ProjectiveSpace <S>,
2212 Q : ProjectiveSpace <S>,
2213 S : Field
2214{
2215 type Output = Q;
2216 fn mul (self, rhs : P) -> Q {
2217 Q::from_vector (self.0 * rhs.to_vector())
2218 }
2219}
2220
2221pub macro nonzero {
2222 ($x:expr) => {
2223 $crate::NonZero::new ($x)
2224 },
2225 ($x:expr, $y:expr) => {
2226 $crate::NonZero2::new (Vector2::new ($x, $y))
2227 },
2228 ($x:expr, $y:expr, $z:expr) => {
2229 $crate::NonZero3::new (Vector3::new ($x, $y, $z))
2230 },
2231 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2232 $crate::NonZero4::new (Vector4::new ($x, $y, $z, $w))
2233 }
2234}
2235
2236pub macro point {
2237 ($x:expr, $y:expr) => {
2238 $crate::point2 ($x, $y)
2239 },
2240 ($x:expr, $y:expr, $z:expr) => {
2241 $crate::point3 ($x, $y, $z)
2242 },
2243 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2244 $crate::point4 ($x, $y, $z, $w)
2245 }
2246}
2247
2248pub macro vector {
2249 ($x:expr, $y:expr) => {
2250 $crate::vector2 ($x, $y)
2251 },
2252 ($x:expr, $y:expr, $z:expr) => {
2253 $crate::vector3 ($x, $y, $z)
2254 },
2255 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2256 $crate::vector4 ($x, $y, $z, $w)
2257 }
2258}
2259
2260pub macro matrix {
2261 ($v00:expr, $v01:expr; $v10:expr, $v11:expr$(;)?) => {
2262 $crate::Matrix2::new ($v00, $v01, $v10, $v11)
2263 },
2264 ( $v00:expr, $v01:expr, $v02:expr;
2265 $v10:expr, $v11:expr, $v12:expr;
2266 $v20:expr, $v21:expr, $v22:expr$(;)?
2267 ) => {
2268 $crate::Matrix3::new ($v00, $v01, $v02, $v10, $v11, $v12, $v20, $v21, $v22)
2269 },
2270 ( $v00:expr, $v01:expr, $v02:expr, $v03:expr;
2271 $v10:expr, $v11:expr, $v12:expr, $v13:expr;
2272 $v20:expr, $v21:expr, $v22:expr, $v23:expr;
2273 $v30:expr, $v31:expr, $v32:expr, $v33:expr$(;)?
2274 ) => {
2275 $crate::Matrix4::new (
2276 $v00, $v01, $v02, $v03,
2277 $v10, $v11, $v12, $v13,
2278 $v20, $v21, $v22, $v23,
2279 $v30, $v31, $v32, $v33)
2280 }
2281}
2282
2283macro_rules! impl_angle {
2284 ( $angle:ident, $docname:expr ) => {
2285 #[doc = $docname]
2286 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2287 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2289 pub struct $angle <S> (pub S);
2290 impl_numcast_primitive_map!($angle);
2291 impl <S : Ring> std::iter::Sum for $angle <S> {
2292 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2293 use num::Zero;
2294 iter.fold ($angle::zero(), |acc, item| acc + item)
2295 }
2296 }
2297 impl <S : Ring> std::ops::Neg for $angle <S> {
2298 type Output = Self;
2299 fn neg (self) -> Self {
2300 $angle (-self.0)
2301 }
2302 }
2303 impl <S : Ring> std::ops::Add for $angle <S> {
2304 type Output = Self;
2305 fn add (self, other : Self) -> Self::Output {
2306 $angle (self.0 + other.0)
2307 }
2308 }
2309 impl <S : Ring> std::ops::AddAssign for $angle <S> {
2310 fn add_assign (&mut self, other : Self) {
2311 self.0 += other.0
2312 }
2313 }
2314 impl <S : Ring> std::ops::Sub for $angle <S> {
2315 type Output = Self;
2316 fn sub (self, other : Self) -> Self::Output {
2317 $angle (self.0 - other.0)
2318 }
2319 }
2320 impl <S : Ring> std::ops::SubAssign for $angle <S> {
2321 fn sub_assign (&mut self, other : Self) {
2322 self.0 -= other.0
2323 }
2324 }
2325 impl <S : Ring> std::ops::Mul <S> for $angle <S> {
2326 type Output = Self;
2327 fn mul (self, scalar : S) -> Self::Output {
2328 $angle (scalar * self.0)
2329 }
2330 }
2331 impl <S : Ring> std::ops::MulAssign <S> for $angle <S> {
2332 fn mul_assign (&mut self, scalar : S) {
2333 self.0 *= scalar
2334 }
2335 }
2336 impl <S : Field> std::ops::Div for $angle <S> {
2337 type Output = S;
2338 fn div (self, other : Self) -> Self::Output {
2339 self.0 / other.0
2340 }
2341 }
2342 impl <S : Field> std::ops::Div <S> for $angle <S> {
2343 type Output = Self;
2344 fn div (self, scalar : S) -> Self::Output {
2345 $angle (self.0 / scalar)
2346 }
2347 }
2348 impl <S : Field> std::ops::DivAssign <S> for $angle <S> {
2349 fn div_assign (&mut self, scalar : S) {
2350 self.0 /= scalar
2351 }
2352 }
2353 impl <S : OrderedField> std::ops::Rem for $angle <S> {
2354 type Output = Self;
2355 fn rem (self, other : Self) -> Self::Output {
2356 $angle (self.0 % other.0)
2357 }
2358 }
2359 impl <S : Ring> num::Zero for $angle <S> {
2360 fn zero() -> Self {
2361 $angle (S::zero())
2362 }
2363 fn is_zero (&self) -> bool {
2364 self.0.is_zero()
2365 }
2366 }
2367 impl <S : Ring> From <S> for $angle <S> {
2368 fn from (s : S) -> Self {
2369 $angle (s)
2370 }
2371 }
2372 }
2373}
2374
2375macro_rules! impl_angle_wrapped {
2376 ( $angle:ident, $comment:expr ) => {
2377 #[doc = $comment]
2378 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2379 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2380 pub struct $angle <S> (Rad <S>);
2381
2382 impl_numcast!($angle);
2383 impl <S : Copy> $angle <S> {
2384 pub const fn angle (&self) -> Rad <S> {
2385 self.0
2386 }
2387 }
2388 impl <S : Real> std::iter::Sum for $angle <S> {
2389 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2390 use num::Zero;
2391 iter.fold ($angle::zero(), |acc, item| acc + item)
2392 }
2393 }
2394 impl <S : Real> std::ops::Neg for $angle <S> {
2395 type Output = Self;
2396 fn neg (self) -> Self {
2397 self.map (Rad::neg)
2398 }
2399 }
2400 impl <S : Real> std::ops::Add for $angle <S> {
2401 type Output = Self;
2402 fn add (self, other : Self) -> Self::Output {
2403 self.map (|angle| angle + other.0)
2404 }
2405 }
2406 impl <S : Real> std::ops::AddAssign for $angle <S> {
2407 fn add_assign (&mut self, other : Self) {
2408 *self = *self + other
2409 }
2410 }
2411 impl <S : Real> std::ops::Add <Rad <S>> for $angle <S> {
2412 type Output = Self;
2413 fn add (self, angle : Rad <S>) -> Self::Output {
2414 self.map (|a| a + angle)
2415 }
2416 }
2417 impl <S : Real> std::ops::AddAssign <Rad <S>> for $angle <S> {
2418 fn add_assign (&mut self, angle : Rad <S>) {
2419 *self = *self + angle
2420 }
2421 }
2422 impl <S : Real> std::ops::Sub for $angle <S> {
2423 type Output = Self;
2424 fn sub (self, other : Self) -> Self::Output {
2425 self.map (|angle| angle - other.0)
2426 }
2427 }
2428 impl <S : Real> std::ops::SubAssign for $angle <S> {
2429 fn sub_assign (&mut self, other : Self) {
2430 *self = *self - other
2431 }
2432 }
2433 impl <S : Real> std::ops::Sub <Rad <S>> for $angle <S> {
2434 type Output = Self;
2435 fn sub (self, angle : Rad <S>) -> Self::Output {
2436 self.map (|a| a - angle)
2437 }
2438 }
2439 impl <S : Real> std::ops::SubAssign <Rad <S>> for $angle <S> {
2440 fn sub_assign (&mut self, angle : Rad <S>) {
2441 *self = *self - angle
2442 }
2443 }
2444 impl <S : Real> std::ops::Mul <S> for $angle <S> {
2445 type Output = Self;
2446 fn mul (self, scalar : S) -> Self::Output {
2447 self.map (|angle| angle * scalar)
2448 }
2449 }
2450 impl <S : Real> std::ops::MulAssign <S> for $angle <S> {
2451 fn mul_assign (&mut self, scalar : S) {
2452 *self = *self * scalar
2453 }
2454 }
2455 impl <S : Real> std::ops::Div for $angle <S> {
2456 type Output = S;
2457 fn div (self, other : Self) -> Self::Output {
2458 self.0 / other.0
2459 }
2460 }
2461 impl <S : Real> std::ops::Div <S> for $angle <S> {
2462 type Output = Self;
2463 fn div (self, scalar : S) -> Self::Output {
2464 self.map (|angle| angle / scalar)
2465 }
2466 }
2467 impl <S : Real> std::ops::DivAssign <S> for $angle <S> {
2468 fn div_assign (&mut self, scalar : S) {
2469 *self = *self / scalar
2470 }
2471 }
2472 impl <S : Real> std::ops::Rem for $angle <S> {
2473 type Output = Self;
2474 fn rem (self, other : Self) -> Self::Output {
2475 self.map (|angle| angle % other.0)
2476 }
2477 }
2478 impl <S : Real> num::Zero for $angle <S> {
2479 fn zero() -> Self {
2480 $angle (Rad::zero())
2481 }
2482 fn is_zero (&self) -> bool {
2483 self.0.is_zero()
2484 }
2485 }
2486 }
2487}
2488
2489macro_rules! impl_dimension {
2490 ( $point:ident, $vector:ident, $nonzero:ident, $unit:ident,
2491 $point_fun:ident, $vector_fun:ident, $matrix_fun:ident, $ortho_fun:ident,
2492 [$($component:ident),+],
2493 [$(($axis_method:ident, $unit_method:ident)),+], [$($patch:ident)?],
2494 [$($projective:ident)?],
2495 $matrix:ident, [$($submatrix:ident)?], [$($rotation:ident)?],
2496 $ndims:expr, $dimension:expr
2497 ) => {
2498 #[doc = $dimension]
2499 #[doc = "position"]
2500 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display, From, Neg)]
2501 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2502 #[display("{}", _0)]
2503 #[repr(C)]
2504 pub struct $point <S> (pub $vector <S>);
2505
2506 #[doc = $dimension]
2507 #[doc = "non-zero vector"]
2508 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2509 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2510 #[display("{}", _0)]
2511 #[repr(C)]
2512 pub struct $nonzero <S> ($vector <S>);
2513
2514 #[doc = $dimension]
2515 #[doc = "unit vector"]
2516 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2517 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2518 #[display("{}", _0)]
2519 #[repr(C)]
2520 pub struct $unit <S> ($vector <S>);
2521
2522 #[doc = "Construct a"]
2526 #[doc = $dimension]
2527 #[doc = "point"]
2528 pub const fn $point_fun <S> ($($component : S),+) -> $point <S> {
2529 $point::new ($($component),+)
2530 }
2531 impl_numcast!($point);
2532 impl <S> $point <S> {
2533 pub const fn new ($($component : S),+) -> Self {
2534 $point ($vector::new ($($component),+))
2535 }
2536 $(
2537 pub const fn $component (&self) -> S where S : Copy {
2538 self.0.$component
2539 }
2540 )+
2541 pub fn distance (self, other : $point <S>) -> NonNegative <S> where
2542 S : OrderedField + Sqrt
2543 {
2544 MetricSpace::distance (self.0, other.0)
2545 }
2546 pub fn distance_squared (self, other : $point <S>) -> NonNegative <S> where
2547 S : OrderedField
2548 {
2549 MetricSpace::distance_squared (self.0, other.0)
2550 }
2551 }
2552 $(
2554 impl <S : Field> ProjectiveSpace <S> for $projective <S> {
2555 type Patch = $point <S>;
2556 }
2557 )?
2558 impl <S : Field> AffineSpace <S> for $point <S> {
2559 type Translation = $vector <S>;
2560 }
2561 impl <S : Ring> Point <$vector <S>> for $point <S> {
2562 fn to_vector (self) -> $vector <S> {
2563 self.0
2564 }
2565 fn from_vector (vector : $vector <S>) -> Self {
2566 $point (vector)
2567 }
2568 }
2569 impl <S> From <[S; $ndims]> for $point <S> {
2570 fn from (array : [S; $ndims]) -> Self {
2571 $point (array.into())
2572 }
2573 }
2574 $(
2575 impl <S> From <($patch <S>, S)> for $point <S> {
2576 fn from ((patch, scale) : ($patch <S>, S)) -> Self {
2577 $point ((patch.0, scale).into())
2578 }
2579 }
2580 )?
2581 impl <S> std::cmp::PartialOrd for $point <S> where S : PartialOrd {
2582 fn partial_cmp (&self, other : &Self) -> Option <std::cmp::Ordering> {
2583 self.0.as_slice().partial_cmp (&other.0.as_slice())
2584 }
2585 }
2586 impl <S> std::ops::Add <$vector <S>> for $point <S> where S : AdditiveGroup {
2587 type Output = Self;
2588 fn add (self, displacement : $vector <S>) -> Self::Output {
2589 $point (self.0 + displacement)
2590 }
2591 }
2592 impl <S> std::ops::Add <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2593 type Output = Self;
2594 fn add (self, displacement : &$vector <S>) -> Self::Output {
2595 $point (self.0 + *displacement)
2596 }
2597 }
2598 impl <S> std::ops::Add <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2599 type Output = $point <S>;
2600 fn add (self, displacement : $vector <S>) -> Self::Output {
2601 $point (self.0 + displacement)
2602 }
2603 }
2604 impl <S> std::ops::Add <&$vector <S>> for &$point <S> where
2605 S : AdditiveGroup + Copy
2606 {
2607 type Output = $point <S>;
2608 fn add (self, displacement : &$vector <S>) -> Self::Output {
2609 $point (self.0 + *displacement)
2610 }
2611 }
2612 impl <S> std::ops::AddAssign <$vector <S>> for $point <S> where S : AdditiveGroup {
2613 fn add_assign (&mut self, displacement : $vector <S>) {
2614 self.0 += displacement
2615 }
2616 }
2617 impl <S> std::ops::AddAssign <&$vector <S>> for $point <S> where
2618 S : AdditiveGroup + Copy
2619 {
2620 fn add_assign (&mut self, displacement : &$vector <S>) {
2621 self.0 += *displacement
2622 }
2623 }
2624 impl <S> std::ops::Sub <$vector <S>> for $point <S> where S : AdditiveGroup {
2625 type Output = Self;
2626 fn sub (self, displacement : $vector <S>) -> Self::Output {
2627 $point (self.0 - displacement)
2628 }
2629 }
2630 impl <S> std::ops::Sub <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2631 type Output = Self;
2632 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2633 $point (self.0 - *displacement)
2634 }
2635 }
2636 impl <S> std::ops::Sub <$point <S>> for $point <S> where S : AdditiveGroup {
2637 type Output = $vector <S>;
2638 fn sub (self, other : Self) -> Self::Output {
2639 self.0 - other.0
2640 }
2641 }
2642 impl <S> std::ops::Sub <&$point <S>> for $point <S> where S : AdditiveGroup + Copy {
2643 type Output = $vector <S>;
2644 fn sub (self, other : &Self) -> Self::Output {
2645 self.0 - other.0
2646 }
2647 }
2648 impl <S> std::ops::Sub <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2649 type Output = $point <S>;
2650 fn sub (self, displacement : $vector <S>) -> Self::Output {
2651 $point (self.0 - displacement)
2652 }
2653 }
2654 impl <S> std::ops::Sub <&$vector <S>> for &$point <S> where
2655 S : AdditiveGroup + Copy
2656 {
2657 type Output = $point <S>;
2658 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2659 $point (self.0 - *displacement)
2660 }
2661 }
2662 impl <S> std::ops::Sub <$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2663 type Output = $vector <S>;
2664 fn sub (self, other : $point <S>) -> Self::Output {
2665 self.0 - other.0
2666 }
2667 }
2668 impl <S> std::ops::Sub <&$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2669 type Output = $vector <S>;
2670 fn sub (self, other : &$point <S>) -> Self::Output {
2671 self.0 - other.0
2672 }
2673 }
2674 impl <S> std::ops::SubAssign <$vector <S>> for $point <S> where
2675 S : AdditiveGroup
2676 {
2677 fn sub_assign (&mut self, displacement : $vector <S>) {
2678 self.0 -= displacement
2679 }
2680 }
2681 impl <S> std::ops::SubAssign <&$vector <S>> for $point <S> where
2682 S : AdditiveGroup + Copy
2683 {
2684 fn sub_assign (&mut self, displacement : &$vector <S>) {
2685 self.0 -= *displacement
2686 }
2687 }
2688 impl <S> approx::AbsDiffEq for $point <S> where
2689 S : approx::AbsDiffEq,
2690 S::Epsilon : Copy
2691 {
2692 type Epsilon = S::Epsilon;
2693 fn default_epsilon() -> Self::Epsilon {
2694 S::default_epsilon()
2695 }
2696 #[inline]
2697 fn abs_diff_eq (&self, other : &Self, epsilon : Self::Epsilon) -> bool {
2698 self.0.abs_diff_eq (&other.0, epsilon)
2699 }
2700 }
2701 impl <S> approx::RelativeEq for $point <S> where
2702 S : approx::RelativeEq,
2703 S::Epsilon : Copy
2704 {
2705 fn default_max_relative() -> Self::Epsilon {
2706 S::default_max_relative()
2707 }
2708 #[inline]
2709 fn relative_eq (&self,
2710 other : &Self, epsilon : Self::Epsilon, max_relative : Self::Epsilon
2711 ) -> bool {
2712 self.0.relative_eq (&other.0, epsilon, max_relative)
2713 }
2714 }
2715 impl <S> approx::UlpsEq for $point <S> where
2716 S : approx::UlpsEq,
2717 S::Epsilon : Copy
2718 {
2719 fn default_max_ulps() -> u32 {
2720 S::default_max_ulps()
2721 }
2722 #[inline]
2723 fn ulps_eq (&self, other : &Self, epsilon : Self::Epsilon, max_ulps : u32)
2724 -> bool
2725 {
2726 self.0.ulps_eq (&other.0, epsilon, max_ulps)
2727 }
2728 }
2729 #[doc = "Construct a"]
2733 #[doc = $dimension]
2734 #[doc = "vector"]
2735 pub const fn $vector_fun <S> ($($component : S),+) -> $vector <S> {
2736 $vector::new ($($component),+)
2737 }
2738 impl <S> NormedVectorSpace <S> for $vector <S> where S : OrderedField {
2739 type Unit = $unit <S>;
2740 fn norm_squared (self) -> NonNegative <S> {
2741 self.self_dot()
2742 }
2743 fn norm_max (self) -> NonNegative <S> where S : SignedExt {
2744 NonNegative::unchecked (self.map (|x| x.abs()).reduce_partial_max())
2745 }
2746 fn normalize (self) -> Self::Unit where S : Sqrt {
2747 $unit (self / *self.norm())
2748 }
2749 }
2750 impl <S> InnerProductSpace <S> for $vector <S> where S : Field {
2751 fn outer_product (self, rhs : Self) -> $matrix <S> {
2752 let mut cols : $vector <$vector <S>> = [$vector::zero(); $ndims].into();
2753 for col in 0..$ndims {
2754 for row in 0..$ndims {
2755 cols[col][row] = self[row] * rhs[col];
2756 }
2757 }
2758 $matrix { cols }
2759 }
2760 fn orthogonal (self) -> Self where S : approx::AbsDiffEq <Epsilon = S> {
2761 $ortho_fun (self)
2762 }
2763 }
2764 impl <S : Ring> Dot <S> for $vector <S> {
2765 fn dot (self, other : Self) -> S {
2766 self.dot (other)
2767 }
2768 }
2769 impl <S : Field> VectorSpace <S> for $vector <S> {
2770 type NonZero = $nonzero <S>;
2771 fn map <F> (self, f : F) -> Self where F : FnMut (S) -> S {
2772 self.map (f)
2773 }
2774 }
2775 impl <S> Module <S> for $vector <S> where S : Ring + num::MulAdd {
2776 type LinearEndo = $matrix <S>;
2777 }
2778 impl <S> GroupAction <Addition, $point <S>> for $vector <S>
2779 where S : AdditiveGroup { }
2780 impl <S> MonoidAction <Addition, $point <S>> for $vector <S>
2781 where S : AdditiveGroup { }
2782 impl <S> SemigroupAction <Addition, $point <S>> for $vector <S>
2783 where S : AdditiveGroup
2784 {
2785 fn action (self, point : $point <S>) -> $point <S> {
2786 point + self
2787 }
2788 }
2789 impl <S> std::ops::Mul <LinearAuto <S, Self>> for $vector <S> where
2790 S : Ring + MaybeSerDes
2791 {
2792 type Output = Self;
2793 fn mul (self, rhs : LinearAuto <S, Self>) -> Self::Output {
2794 self * rhs.0
2795 }
2796 }
2797 impl <S> std::ops::Mul <LinearIso <S, Self, Self, $matrix <S>>> for $vector <S> where
2798 S : Ring + MaybeSerDes
2799 {
2800 type Output = Self;
2801 fn mul (self, rhs : LinearIso <S, Self, Self, $matrix <S>>) -> Self::Output {
2802 self * rhs.0
2803 }
2804 }
2805 impl <S> num::Inv for LinearAuto <S, $vector <S>> where
2806 S : Ring + num::real::Real + MaybeSerDes
2807 {
2808 type Output = Self;
2809 fn inv (self) -> Self::Output {
2810 LinearAuto (self.0.inv())
2811 }
2812 }
2813 impl <S> std::ops::Div for LinearAuto <S, $vector <S>> where
2814 S : Ring + num::Float + MaybeSerDes
2815 {
2816 type Output = Self;
2817 fn div (self, rhs : Self) -> Self::Output {
2818 LinearAuto (self.0 / rhs.0)
2819 }
2820 }
2821 impl <S> std::ops::DivAssign <Self> for LinearAuto <S, $vector <S>> where
2822 S : Ring + num::Float + MaybeSerDes
2823 {
2824 fn div_assign (&mut self, rhs : Self) {
2825 self.0 /= rhs.0
2826 }
2827 }
2828 impl <S> From <$unit <S>> for $vector <S> where S : Copy {
2829 fn from (unit : $unit <S>) -> Self {
2830 *unit
2831 }
2832 }
2833 #[doc = "Construct a"]
2837 #[doc = $dimension]
2838 #[doc = "matrix"]
2839 pub const fn $matrix_fun <S> ($($component : $vector <S>),+) -> $matrix <S> {
2840 $matrix {
2841 cols: $vector_fun ($($component),+)
2842 }
2843 }
2844 impl <S, V, W> num::Inv for LinearIso <S, V, W, $matrix <S>> where
2845 V : Module <S>,
2846 W : Module <S>,
2847 S : Ring + num::real::Real
2848 {
2849 type Output = LinearIso <S, W, V, $matrix <S>>;
2850 fn inv (self) -> Self::Output {
2851 let mat4 = Matrix4::from (self.0);
2853 LinearIso (mat4.inverted().into(), PhantomData::default())
2854 }
2855 }
2856 impl <S, V> std::ops::Div for LinearIso <S, V, V, $matrix <S>> where
2857 V : Module <S, LinearEndo=$matrix <S>>,
2858 S : Ring + num::real::Real
2859 {
2860 type Output = Self;
2861 #[expect(clippy::suspicious_arithmetic_impl)]
2862 fn div (self, rhs : Self) -> Self::Output {
2863 use num::Inv;
2864 self * rhs.inv()
2865 }
2866 }
2867 impl <S, V> std::ops::DivAssign for LinearIso <S, V, V, $matrix <S>> where
2868 V : Module <S, LinearEndo=$matrix <S>>,
2869 S : Ring + num::real::Real
2870 {
2871 #[expect(clippy::suspicious_op_assign_impl)]
2872 fn div_assign (&mut self, rhs : Self) {
2873 use num::Inv;
2874 *self *= rhs.inv()
2875 }
2876 }
2877 $(
2878 impl <S : Copy> Matrix <S> for $matrix <S> {
2879 type Rows = $vector <$vector <S>>;
2880 type Submatrix = $submatrix <S>;
2881 fn rows (self) -> $vector <$vector <S>> {
2882 self.into_row_arrays().map ($vector::from).into()
2883 }
2884 fn submatrix (self, i : usize, j : usize) -> $submatrix <S> {
2885 let a : [S; ($ndims-1) * ($ndims-1)] = std::array::from_fn (|index|{
2886 let i = (i + 1 + index / ($ndims-1)) % $ndims;
2887 let j = (j + 1 + index % ($ndims-1)) % $ndims;
2888 self.cols[i][j]
2889 });
2890 $submatrix::from_col_array (a)
2891 }
2892 fn fill_zeros (submatrix : $submatrix <S>) -> Self where S : num::Zero{
2894 let cols = $vector::from (
2895 std::array::from_fn (|i| if i < $ndims-1 {
2896 $vector::from (submatrix.cols[i])
2897 } else {
2898 $vector::zero()
2899 })
2900 );
2901 $matrix { cols }
2902 }
2903 }
2904 )?
2905 impl <S : Ring> LinearMap <S, $vector <S>, $vector <S>> for $matrix <S> {
2906 fn determinant (self) -> S {
2907 self.determinant()
2908 }
2909 fn transpose (self) -> Self {
2910 self.transposed()
2911 }
2912 }
2913 $(
2917 impl <S> $rotation <S> where S : Ring + MaybeSerDes {
2918 pub fn identity() -> Self {
2920 use num::One;
2921 Self::one()
2922 }
2923 pub fn new_approx (mat : $matrix <S>) -> Option <Self> where
2929 S : approx::RelativeEq <Epsilon=S>
2930 {
2931 let four = S::one() + S::one() + S::one() + S::one();
2932 let thirtytwo = four * four * (S::one() + S::one());
2933 let epsilon = S::default_epsilon() * thirtytwo;
2934 let max_relative = S::default_max_relative() * four;
2935 if approx::relative_ne!(mat * mat.transposed(), $matrix::identity(),
2936 max_relative=max_relative, epsilon=epsilon
2937 ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
2938 None
2939 } else {
2940 Some ($rotation (LinearAuto (LinearIso (mat, PhantomData))))
2941 }
2942 }
2943 pub fn noisy (mat : $matrix <S>) -> Self where S : std::fmt::Debug {
2947 assert_eq!(mat * mat.transposed(), $matrix::identity());
2948 assert_eq!(mat.determinant(), S::one());
2949 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2950 }
2951 pub fn noisy_approx (mat : $matrix <S>) -> Self where
2956 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
2957 {
2958 let four = S::one() + S::one() + S::one() + S::one();
2959 let epsilon = S::default_epsilon() * four;
2960 let max_relative = S::default_max_relative() * four;
2961 approx::assert_relative_eq!(mat * mat.transposed(), $matrix::identity(),
2962 epsilon=epsilon, max_relative=max_relative);
2963 approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
2964 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2965 }
2966 pub fn unchecked (mat : $matrix <S>) -> Self where S : std::fmt::Debug {
2970 debug_assert!(mat * mat.transposed() == $matrix::identity());
2971 debug_assert!(mat.determinant() == S::one());
2972 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2973 }
2974 pub fn unchecked_approx (mat : $matrix <S>) -> Self where
2979 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
2980 {
2981 if cfg!(debug_assertions) {
2982 let four = S::one() + S::one() + S::one() + S::one();
2983 let epsilon = S::default_epsilon() * four;
2984 let max_relative = S::default_max_relative() * four;
2985 approx::assert_relative_eq!(mat * mat.transposed(), $matrix::identity(),
2986 max_relative=max_relative, epsilon=epsilon);
2987 approx::assert_relative_eq!(mat.determinant(), S::one(),
2988 max_relative=max_relative);
2989 }
2990 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2991 }
2992 pub const fn mat (self) -> $matrix <S> {
2994 self.0.0.0
2995 }
2996 pub fn rotate (self, point : $point <S>) -> $point <S> {
2998 $point::from (self * point.0)
2999 }
3000 pub fn rotate_around (self, point : $point <S>, center : $point <S>)
3002 -> $point <S>
3003 {
3004 self.rotate (point - center.0) + center.0
3005 }
3006 }
3007 impl <S> std::ops::Deref for $rotation <S> where S : Ring + MaybeSerDes {
3008 type Target = LinearAuto <S, $vector <S>>;
3009 fn deref (&self) -> &Self::Target {
3010 &self.0
3011 }
3012 }
3013 impl <S> num::Inv for $rotation <S> where S : Ring + MaybeSerDes {
3014 type Output = Self;
3015 fn inv (self) -> Self::Output {
3016 $rotation (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
3017 }
3018 }
3019 impl <S> std::ops::Mul <$vector <S>> for $rotation <S> where
3020 S : Ring + MaybeSerDes
3021 {
3022 type Output = $vector <S>;
3023 fn mul (self, rhs : $vector <S>) -> Self::Output {
3024 self.0 * rhs
3025 }
3026 }
3027 impl <S> std::ops::Mul <$nonzero <S>> for $rotation <S> where
3028 S : Ring + MaybeSerDes
3029 {
3030 type Output = $nonzero <S>;
3031 fn mul (self, rhs : $nonzero <S>) -> Self::Output {
3032 $nonzero (self.0 * rhs.0)
3033 }
3034 }
3035 impl <S> std::ops::Mul <$unit <S>> for $rotation <S> where S : Ring + MaybeSerDes {
3036 type Output = $unit <S>;
3037 fn mul (self, rhs : $unit <S>) -> Self::Output {
3038 $unit (self.0 * rhs.0)
3039 }
3040 }
3041 impl <S> std::ops::Mul <$rotation <S>> for $vector <S> where S : Ring + MaybeSerDes {
3042 type Output = $vector <S>;
3043 fn mul (self, rhs : $rotation <S>) -> Self::Output {
3044 self * rhs.0
3045 }
3046 }
3047 impl <S> std::ops::Mul for $rotation <S> where S : Ring + MaybeSerDes {
3048 type Output = Self;
3049 fn mul (self, rhs : Self) -> Self::Output {
3050 $rotation (self.0 * rhs.0)
3051 }
3052 }
3053 impl <S> std::ops::MulAssign <Self> for $rotation <S> where S : Ring + MaybeSerDes {
3054 fn mul_assign (&mut self, rhs : Self) {
3055 self.0 *= rhs.0
3056 }
3057 }
3058 impl <S> std::ops::Div for $rotation <S> where S : Ring + MaybeSerDes {
3059 type Output = Self;
3060 #[expect(clippy::suspicious_arithmetic_impl)]
3061 fn div (self, rhs : Self) -> Self::Output {
3062 use num::Inv;
3063 self * rhs.inv()
3064 }
3065 }
3066 impl <S> std::ops::DivAssign <Self> for $rotation <S> where S : Ring + MaybeSerDes {
3067 #[expect(clippy::suspicious_op_assign_impl)]
3068 fn div_assign (&mut self, rhs : Self) {
3069 use num::Inv;
3070 *self *= rhs.inv()
3071 }
3072 }
3073 impl <S> num::One for $rotation <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
3074 fn one() -> Self {
3075 $rotation (LinearAuto (LinearIso ($matrix::identity(), PhantomData)))
3076 }
3077 }
3078 )?
3079
3080 impl_numcast!($nonzero);
3084 impl <S> $nonzero <S> {
3085 pub fn new (vector : $vector <S>) -> Option <Self> where S : Ring {
3095 use num::Zero;
3096 if !vector.is_zero() {
3097 Some ($nonzero (vector))
3098 } else {
3099 None
3100 }
3101 }
3102 pub fn noisy (vector : $vector <S>) -> Self where S : Ring {
3111 use num::Zero;
3112 assert!(!vector.is_zero());
3113 $nonzero (vector)
3114 }
3115 #[inline]
3119 pub fn unchecked (vector : $vector <S>) -> Self where S : Ring + std::fmt::Debug {
3120 debug_assert_ne!(vector, $vector::zero());
3121 $nonzero (vector)
3122 }
3123 #[must_use]
3135 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3136 where S : Ring
3137 {
3138 Self::noisy (fun (self.0))
3139 }
3140 #[must_use]
3141 pub fn map_unchecked (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3142 where S : Ring + std::fmt::Debug
3143 {
3144 Self::unchecked (fun (self.0))
3145 }
3146 }
3147 impl <S> std::ops::Deref for $nonzero <S> {
3148 type Target = $vector <S>;
3149 fn deref (&self) -> &$vector <S> {
3150 &self.0
3151 }
3152 }
3153 impl <S : Ring> std::ops::Neg for $nonzero <S> {
3154 type Output = Self;
3155 fn neg (self) -> Self {
3156 $nonzero (-self.0)
3157 }
3158 }
3159 impl <S> From <$unit <S>> for $nonzero <S> where S : Ring + std::fmt::Debug {
3160 fn from (unit : $unit <S>) -> Self {
3161 $nonzero::unchecked (*unit)
3162 }
3163 }
3164
3165 impl_numcast!($unit);
3169 impl <S> $unit <S> {
3170 $(
3176 #[inline]
3177 pub fn $axis_method() -> Self where S : num::One + num::Zero {
3178 $unit ($vector::$unit_method())
3179 }
3180 )+
3181 pub fn new (vector : $vector <S>) -> Option <Self> where S : OrderedField + Sqrt {
3185 if vector == *vector.normalize() {
3186 Some ($unit (vector))
3187 } else {
3188 None
3189 }
3190 }
3191 pub fn new_approx (vector : $vector <S>) -> Option <Self> where
3198 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3199 {
3200 if vector.is_normalized() {
3201 Some ($unit (vector))
3202 } else {
3203 None
3204 }
3205 }
3206 pub fn normalize (vector : $vector <S>) -> Self where S : OrderedField + Sqrt {
3227 use num::Zero;
3228 assert!(!vector.is_zero());
3229 vector.normalize()
3230 }
3231 pub fn normalize_approx (vector : $vector <S>) -> Self where
3235 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3236 {
3237 use num::Zero;
3238 assert!(!vector.is_zero());
3239 let vector = if vector.is_normalized() {
3240 vector
3241 } else {
3242 vector.normalized()
3243 };
3244 $unit (vector)
3245 }
3246 pub fn noisy (vector : $vector <S>) -> Self where
3255 S : OrderedField + Sqrt + std::fmt::Debug
3256 {
3257 assert_eq!(vector, *vector.normalize());
3258 $unit (vector)
3259 }
3260 pub fn noisy_approx (vector : $vector <S>) -> Self where
3269 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3270 {
3271 assert!(vector.is_normalized());
3272 $unit (vector)
3273 }
3274 #[inline]
3278 pub fn unchecked (vector : $vector <S>) -> Self where
3279 S : OrderedField + Sqrt + std::fmt::Debug
3280 {
3281 debug_assert_eq!(vector, *vector.normalize());
3282 $unit (vector)
3283 }
3284 #[inline]
3294 pub fn unchecked_approx (vector : $vector <S>) -> Self where
3295 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3296 {
3297 debug_assert!(vector.is_normalized());
3298 $unit (vector)
3299 }
3300 pub fn random_unit <R : rand::RngExt> (rng : &mut R) -> Self where
3302 S : OrderedField + Sqrt + rand::distr::uniform::SampleUniform
3303 {
3304 let vector = $vector {
3305 $($component: rng.random_range (-S::one()..S::one())),+
3306 };
3307 vector.normalize()
3308 }
3309 pub fn invert (mut self) -> Self where S : Ring {
3312 self.0 = -self.0;
3313 self
3314 }
3315 #[must_use]
3327 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3328 S : OrderedField + Sqrt + std::fmt::Debug
3329 {
3330 Self::noisy (fun (self.0))
3331 }
3332 #[must_use]
3344 pub fn map_noisy_approx (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3345 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3346 {
3347 Self::noisy_approx (fun (self.0))
3348 }
3349 }
3350 impl <S> std::ops::Deref for $unit <S> {
3351 type Target = $vector <S>;
3352 fn deref (&self) -> &$vector <S> {
3353 &self.0
3354 }
3355 }
3356 impl <S : Ring> std::ops::Neg for $unit <S> {
3357 type Output = Self;
3358 fn neg (self) -> Self {
3359 $unit (-self.0)
3360 }
3361 }
3362 }
3364}
3365
3366macro_rules! impl_numcast {
3367 ($type:ident) => {
3368 impl <S> $type <S> {
3369 #[inline]
3370 pub fn numcast <T> (self) -> Option <$type <T>> where
3371 S : num::NumCast,
3372 T : num::NumCast
3373 {
3374 self.0.numcast().map ($type)
3375 }
3376 }
3377 }
3378}
3379
3380macro_rules! impl_numcast_primitive {
3381 ($type:ident) => {
3382 impl <S> $type <S> {
3383 #[inline]
3384 pub fn numcast <T> (self) -> Option <$type <T>> where
3385 S : num::NumCast,
3386 T : num::NumCast
3387 {
3388 T::from (self.0).map ($type)
3389 }
3390 }
3391 impl <S> num::ToPrimitive for $type <S> where S : num::ToPrimitive {
3392 fn to_isize (&self) -> Option <isize> {
3393 self.0.to_isize()
3394 }
3395 fn to_i8 (&self) -> Option <i8> {
3396 self.0.to_i8()
3397 }
3398 fn to_i16 (&self) -> Option <i16> {
3399 self.0.to_i16()
3400 }
3401 fn to_i32 (&self) -> Option <i32> {
3402 self.0.to_i32()
3403 }
3404 fn to_i64 (&self) -> Option <i64> {
3405 self.0.to_i64()
3406 }
3407 fn to_usize (&self) -> Option <usize> {
3408 self.0.to_usize()
3409 }
3410 fn to_u8 (&self) -> Option <u8> {
3411 self.0.to_u8()
3412 }
3413 fn to_u16 (&self) -> Option <u16> {
3414 self.0.to_u16()
3415 }
3416 fn to_u32 (&self) -> Option <u32> {
3417 self.0.to_u32()
3418 }
3419 fn to_u64 (&self) -> Option <u64> {
3420 self.0.to_u64()
3421 }
3422 fn to_f32 (&self) -> Option <f32> {
3423 self.0.to_f32()
3424 }
3425 fn to_f64 (&self) -> Option <f64> {
3426 self.0.to_f64()
3427 }
3428 }
3429 }
3430}
3431
3432macro_rules! impl_numcast_primitive_option {
3433 ($type:ident$(, $bound:path)*) => {
3434 impl_numcast_primitive!{$type}
3435 impl <S> num::NumCast for $type <S> where
3436 S : num::NumCast $(+ $bound)*
3437 {
3438 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3439 S::from (n).and_then ($type::new)
3440 }
3441 }
3442 }
3443}
3444
3445macro_rules! impl_numcast_primitive_map {
3446 ($type:ident) => {
3447 impl_numcast_primitive!{$type}
3448 impl <S> num::NumCast for $type <S> where S : num::NumCast {
3449 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3450 S::from (n).map ($type)
3451 }
3452 }
3453 }
3454}
3455
3456#[doc(hidden)]
3457#[macro_export]
3458macro_rules! impl_numcast_fields {
3459 ($type:ident, $($field:ident),+) => {
3460 impl <S> $type <S> {
3461 #[inline]
3462 pub fn numcast <T> (self) -> Option <$type <T>> where
3463 S : num::NumCast + PartialOrd + num::Zero,
3464 T : num::NumCast + PartialOrd + num::Zero
3465 {
3466 Some ($type {
3467 $($field: self.$field.numcast()?),+
3468 })
3469 }
3470 }
3471 }
3472}
3473
3474impl_angle!(Deg, "Degrees");
3475impl_angle!(Rad, "Radians");
3476impl_angle!(Turn, "Turns");
3477impl_angle_wrapped!(AngleWrapped, "Unsigned wrapped angle restricted to $[0, 2\\pi)$");
3478impl_angle_wrapped!(AngleWrappedSigned,
3479 "Signed wrapped angle restricted to $(-\\pi, \\pi]$");
3480
3481impl_dimension!(Point2, Vector2, NonZero2, Unit2, point2, vector2, matrix2, ortho_vec2,
3482 [x, y], [(axis_x, unit_x), (axis_y, unit_y)],
3483 [], [Point3], Matrix2, [], [Rotation2], 2, "2D");
3484impl_dimension!(Point3, Vector3, NonZero3, Unit3, point3, vector3, matrix3, ortho_vec3,
3485 [x, y, z], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z)],
3486 [Point2], [Point4], Matrix3, [Matrix2], [Rotation3], 3, "3D");
3487impl_dimension!(Point4, Vector4, NonZero4, Unit4, point4, vector4, matrix4, ortho_vec4,
3488 [x, y, z, w], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z), (axis_w, unit_w)],
3489 [Point3], [], Matrix4, [Matrix3], [], 4, "4D");
3490
3491impl_numcast_primitive_option!(Positive, PartialOrd, num::Zero);
3492impl_numcast_primitive_option!(NonNegative, PartialOrd, num::Zero);
3493impl_numcast_primitive_option!(NonZero, PartialEq, num::Zero);
3494impl_numcast_primitive_option!(Normalized, PartialOrd, num::One, num::Zero);
3495impl_numcast_primitive_option!(NormalSigned, OrderedRing);
3496impl_numcast_fields!(Angles3, yaw, pitch, roll);
3497
3498#[inline]
3502fn normalize_quaternion <S : Real> (quaternion : Quaternion <S>) -> Quaternion <S> {
3503 Quaternion::from_vec4 (*quaternion.into_vec4().normalize())
3504}
3505
3506#[cfg(test)]
3507mod tests {
3508 extern crate test;
3509
3510 use super::*;
3511 use crate::num;
3512 use approx;
3513 use rand;
3514 use rand_distr;
3515 use rand_xorshift;
3516
3517 #[expect(clippy::cast_possible_truncation)]
3518 static ORTHO_VEC_BENCH_SEED : std::sync::LazyLock <u64> = std::sync::LazyLock::new (
3519 || std::time::SystemTime::UNIX_EPOCH.elapsed().unwrap().as_nanos() as u64);
3520
3521 #[test]
3522 fn defaults() {
3523 use num::Zero;
3524
3525 assert_eq!(Positive::<f32>::default().0, 0.0);
3526 assert_eq!(NonNegative::<f32>::default().0, 0.0);
3527 assert_eq!(Normalized::<f32>::default().0, 0.0);
3528 assert_eq!(NormalSigned::<f32>::default().0, 0.0);
3529 assert_eq!(Deg::<f32>::default().0, 0.0);
3530 assert_eq!(Rad::<f32>::default().0, 0.0);
3531 assert_eq!(Turn::<f32>::default().0, 0.0);
3532 assert_eq!(
3533 Angles3::<f32>::default(),
3534 Angles3::new (AngleWrapped::zero(), AngleWrapped::zero(), AngleWrapped::zero()));
3535 assert_eq!(
3536 Pose3::<f32>::default(),
3537 Pose3 { position: Point3::origin(), angles: Angles3::default() });
3538 assert_eq!(
3539 LinearIso::<f32, Vector3 <f32>, Vector3 <f32>, Matrix3 <f32>>::default().0,
3540 Matrix3::identity());
3541 assert_eq!(LinearAuto::<f32, Vector3 <f32>>::default().0.0, Matrix3::identity());
3542 assert_eq!(Rotation2::<f32>::default().0.0.0, Matrix2::identity());
3543 assert_eq!(Rotation3::<f32>::default().0.0.0, Matrix3::identity());
3544 assert_eq!(Versor::<f32>::default().0, Quaternion::from_xyzw (0.0, 0.0, 0.0, 1.0));
3545 assert_eq!(
3546 AffineMap::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3547 AffineMap::new (Matrix3::identity(), Vector3::zero()));
3548 assert_eq!(
3549 Affinity::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3550 Affinity::new (LinearIso::new (Matrix3::identity()).unwrap(), Vector3::zero()));
3551 assert_eq!(
3552 Projectivity::<f32, Point4 <f32>, Point4 <f32>, Matrix4 <f32>>::default().0,
3553 LinearIso::new (Matrix4::identity()).unwrap());
3554 }
3555
3556 #[test]
3557 fn numcast() {
3558 let p : Positive <f32> = Positive::<f64>::noisy (0.25).numcast().unwrap();
3559 assert_eq!(p, Positive::noisy (0.25f32));
3560 let v : Vector3 <Positive <f64>> = [
3561 Positive::noisy (1.0),
3562 Positive::noisy (2.0),
3563 Positive::noisy (0.25)
3564 ].into();
3565 let _v : Vector3 <Positive <f32>> = v.numcast().unwrap();
3566 }
3567
3568 #[test]
3569 fn vector_sum() {
3570 let s = [
3571 [0.0, 1.0],
3572 [1.0, 1.0],
3573 [0.5, 0.5]
3574 ].map (Vector2::<f32>::from);
3575 let _sum = s.iter().copied().sum::<Vector2 <f32>>();
3576 }
3577
3578 #[test]
3579 fn vector_outer_product() {
3580 let v1 = vector3 (1.0, 2.0, 3.0);
3581 let v2 = vector3 (2.0, 3.0, 4.0);
3582 assert_eq!(v1.outer_product (v2).into_row_arrays(), [
3583 [2.0, 3.0, 4.0],
3584 [4.0, 6.0, 8.0],
3585 [6.0, 9.0, 12.0]
3586 ]);
3587 }
3588
3589 #[test]
3590 fn rotation3_noisy_approx() {
3591 use rand::{RngExt, SeedableRng};
3592 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3595 let up = Vector3::unit_z();
3596 for _ in 0..1000 {
3597 let forward = vector3 (
3598 rng.random_range (-1000.0f32..1000.0),
3599 rng.random_range (-1000.0f32..1000.0),
3600 0.0
3601 ).normalized();
3602 let right = forward.cross (up);
3603 let mat = Matrix3::from_col_arrays ([
3604 right.into_array(),
3605 forward.into_array(),
3606 up.into_array()
3607 ]);
3608 let _ = Rotation3::noisy_approx (mat);
3609 }
3610 }
3611
3612 #[test]
3613 fn rotation3_intrinsic_angles() {
3614 use std::f32::consts::PI;
3615 use num::Zero;
3616 use rand::{RngExt, SeedableRng};
3617 let rot = Rotation3::from_angles_intrinsic (
3619 Turn (0.25).into(), Rad::zero(), Rad::zero());
3620 approx::assert_relative_eq!(rot.mat(),
3621 Matrix3::from_col_arrays ([
3622 [ 0.0, 1.0, 0.0],
3623 [-1.0, 0.0, 0.0],
3624 [ 0.0, 0.0, 1.0]
3625 ])
3626 );
3627 assert_eq!((Turn (0.25).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3628 let rot = Rotation3::from_angles_intrinsic (
3629 Turn (0.5).into(), Rad::zero(), Rad::zero());
3630 approx::assert_relative_eq!(rot.mat(),
3631 Matrix3::from_col_arrays ([
3632 [-1.0, 0.0, 0.0],
3633 [ 0.0, -1.0, 0.0],
3634 [ 0.0, 0.0, 1.0]
3635 ])
3636 );
3637 assert_eq!((Turn (0.5).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3638 let rot = Rotation3::from_angles_intrinsic (
3640 Rad::zero(), Turn (0.25).into(), Rad::zero());
3641 approx::assert_relative_eq!(rot.mat(),
3642 Matrix3::from_col_arrays ([
3643 [1.0, 0.0, 0.0],
3644 [0.0, 0.0, 1.0],
3645 [0.0, -1.0, 0.0]
3646 ])
3647 );
3648 assert_eq!((Rad::zero(), Turn (0.25).into(), Rad::zero()), rot.intrinsic_angles());
3649 let rot = Rotation3::from_angles_intrinsic (
3651 Rad::zero(), Rad::zero(), Turn (0.25).into());
3652 approx::assert_relative_eq!(rot.mat(),
3653 Matrix3::from_col_arrays ([
3654 [0.0, 0.0, -1.0],
3655 [0.0, 1.0, 0.0],
3656 [1.0, 0.0, 0.0]
3657 ])
3658 );
3659 assert_eq!((Rad::zero(), Rad::zero(), Turn (0.25).into()), rot.intrinsic_angles());
3660 let rot = Rotation3::from_angles_intrinsic (
3662 Turn (0.25).into(), Turn (0.25).into(), Rad::zero());
3663 approx::assert_relative_eq!(rot.mat(),
3664 Matrix3::from_col_arrays ([
3665 [0.0, 1.0, 0.0],
3666 [0.0, 0.0, 1.0],
3667 [1.0, 0.0, 0.0]
3668 ])
3669 );
3670 let angles = rot.intrinsic_angles();
3671 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3672 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3673 approx::assert_relative_eq!(0.0, angles.2.0);
3674 let rot = Rotation3::from_angles_intrinsic (
3676 Turn (0.25).into(), Turn (0.25).into(), Turn (0.25).into());
3677 approx::assert_relative_eq!(rot.mat(),
3678 Matrix3::from_col_arrays ([
3679 [-1.0, 0.0, 0.0],
3680 [ 0.0, 0.0, 1.0],
3681 [ 0.0, 1.0, 0.0]
3682 ])
3683 );
3684 let angles = rot.intrinsic_angles();
3685 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3686 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3687 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.2.0);
3688
3689 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3692 let mut random_vector = || vector3 (
3693 rng.random_range (-100.0f32..100.0),
3694 rng.random_range (-100.0f32..100.0),
3695 rng.random_range (-100.0f32..100.0));
3696 for _ in 0..1000 {
3697 let rot = Rotation3::orthonormalize (
3698 random_vector(), random_vector(), random_vector()
3699 ).unwrap();
3700 let (yaw, pitch, roll) = rot.intrinsic_angles();
3701 assert!(yaw.0 < PI);
3702 assert!(yaw.0 > -PI);
3703 assert!(pitch.0 < PI);
3704 assert!(pitch.0 > -PI);
3705 assert!(roll.0 < PI);
3706 assert!(roll.0 > -PI);
3707 }
3708 }
3709
3710 #[test]
3711 fn rotation3_into_versor() {
3712 use std::f32::consts::PI;
3713 let rot = Rotation3::from_angle_x (Rad (0.01));
3714 let quat = rot.versor().0;
3715 approx::assert_relative_eq!(rot.mat(), quat.into());
3716 let rot = Rotation3::from_angle_x (Rad (-0.01));
3717 let quat = rot.versor().0;
3718 approx::assert_relative_eq!(rot.mat(), quat.into());
3719
3720 let rot = Rotation3::from_angle_y (Rad (0.01));
3721 let quat = rot.versor().0;
3722 approx::assert_relative_eq!(rot.mat(), quat.into());
3723 let rot = Rotation3::from_angle_y (Rad (-0.01));
3724 let quat = rot.versor().0;
3725 approx::assert_relative_eq!(rot.mat(), quat.into());
3726
3727 let rot = Rotation3::from_angle_z (Rad (0.01));
3728 let quat = rot.versor().0;
3729 approx::assert_relative_eq!(rot.mat(), quat.into());
3730 let rot = Rotation3::from_angle_z (Rad (-0.01));
3731 let quat = rot.versor().0;
3732 approx::assert_relative_eq!(rot.mat(), quat.into());
3733
3734 let rot = Rotation3::from_angle_x (Rad (PI / 2.0));
3735 let quat = rot.versor().0;
3736 approx::assert_relative_eq!(rot.mat(), quat.into());
3737 let rot = Rotation3::from_angle_x (Rad (-PI / 2.0));
3738 let quat = rot.versor().0;
3739 approx::assert_relative_eq!(rot.mat(), quat.into());
3740
3741 let rot = Rotation3::from_angle_y (Rad (PI / 2.0));
3742 let quat = rot.versor().0;
3743 approx::assert_relative_eq!(rot.mat(), quat.into());
3744 let rot = Rotation3::from_angle_y (Rad (-PI / 2.0));
3745 let quat = rot.versor().0;
3746 approx::assert_relative_eq!(rot.mat(), quat.into());
3747
3748 let rot = Rotation3::from_angle_z (Rad (PI / 2.0));
3749 let quat = rot.versor().0;
3750 approx::assert_relative_eq!(rot.mat(), quat.into());
3751 let rot = Rotation3::from_angle_z (Rad (-PI / 2.0));
3752 let quat = rot.versor().0;
3753 approx::assert_relative_eq!(rot.mat(), quat.into());
3754 }
3755
3756 #[bench]
3760 fn bench_orthogonal_cross_product (b : &mut test::Bencher) {
3761 use approx::AbsDiffEq;
3762 use rand::{RngExt, SeedableRng};
3763 use rand_distr::Distribution;
3764 let epsilon = f64::default_epsilon() * 256.0;
3765 let std_normal = rand_distr::StandardNormal;
3766 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3767 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3768 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3769 let x : f64 = std_normal.sample (rng);
3770 let y : f64 = std_normal.sample (rng);
3771 x / y
3772 };
3773 b.iter(||{
3774 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3775 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3776 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3777 6 => vector3 (0.0, randf (&mut rng), 0.0),
3778 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3779 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3780 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3781 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3782 _ => unreachable!()
3783 };
3784 approx::assert_abs_diff_ne!(vec, Vector3::zero(), epsilon = epsilon);
3785 let mut ortho = vec.cross (Vector3::unit_x());
3786 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) {
3787 ortho = vec.cross (Vector3::unit_y())
3788 }
3789 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3790 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3791 {
3792 println!("VEC: {vec}");
3793 println!("ORTHO: {ortho}");
3794 println!("DOT: {}", vec.dot (ortho));
3795 }
3796 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3797 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3798 });
3799 }
3800
3801 #[bench]
3805 fn bench_orthogonal_copysign (b : &mut test::Bencher) {
3806 use approx::AbsDiffEq;
3807 use rand::{RngExt, SeedableRng};
3808 use rand_distr::Distribution;
3809 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3810 let std_normal = rand_distr::StandardNormal;
3811 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3812 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3813 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3814 let x : f64 = std_normal.sample (rng);
3815 let y : f64 = std_normal.sample (rng);
3816 x / y
3817 };
3818 b.iter(||{
3819 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3820 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3821 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3822 6 => vector3 (0.0, randf (&mut rng), 0.0),
3823 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3824 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3825 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3826 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3827 _ => unreachable!()
3828 };
3829 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3830 return
3831 }
3832 let ortho = vector3 (
3833 vec.z.copysign (vec.x),
3834 vec.z.copysign (vec.y),
3835 -vec.x.copysign (vec.z) - vec.y.copysign (vec.z)
3837 );
3839 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3840 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3841 {
3842 println!("VEC: {vec}");
3843 println!("ORTHO: {ortho}");
3844 println!("DOT: {:.}", vec.dot (ortho));
3845 }
3846 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3847 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3848 });
3849 }
3850
3851 #[bench]
3855 fn bench_orthogonal_sign (b : &mut test::Bencher) {
3856 use approx::AbsDiffEq;
3857 use rand::{RngExt, SeedableRng};
3858 use rand_distr::Distribution;
3859 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3860 let std_normal = rand_distr::StandardNormal;
3861 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3862 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3863 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3864 let x : f64 = std_normal.sample (rng);
3865 let y : f64 = std_normal.sample (rng);
3866 x / y
3867 };
3868 b.iter(||{
3869 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3870 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3871 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3872 6 => vector3 (0.0, randf (&mut rng), 0.0),
3873 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3874 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3875 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3876 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3877 _ => unreachable!()
3878 };
3879 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3880 return
3881 }
3882 let sign = |n : f64| if n == 0.0 {
3883 0.0
3884 } else if n > 0.0 {
3885 1.0
3886 } else {
3887 -1.0
3888 };
3889 let signfn = |n : f64| sign ((sign (n) + 0.5) * (sign (vec.z) + 0.5));
3890 let ortho = vector3 (
3891 signfn (vec.x) * vec.z,
3892 signfn (vec.y) * vec.z,
3893 (-signfn (vec.x)).mul_add (vec.x, -signfn (vec.y) * vec.y)
3894 );
3896 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3897 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3898 {
3899 println!("VEC: {vec}");
3900 println!("ORTHO: {ortho}");
3901 println!("DOT: {:.}", vec.dot (ortho));
3902 }
3903 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3904 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3905 });
3906 }
3907}