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> From <Rad <S>> for Deg <S> {
344 fn from (radians : Rad <S>) -> Self {
345 let full_turns = radians / Rad::full_turn();
346 Deg::full_turn() * full_turns
347 }
348}
349impl <S : OrderedField> From <Turn <S>> for Deg <S> {
350 fn from (turns : Turn <S>) -> Self {
351 Deg (turns.0 * Deg::full_turn().0)
352 }
353}
354
355impl <S : Real> Angle <S> for Rad <S> {
359 fn full_turn() -> Self {
360 Rad (S::pi() * S::two())
361 }
362}
363impl <S : Real> From <Deg <S>> for Rad <S> {
364 fn from (degrees : Deg <S>) -> Self {
365 let full_turns = degrees / Deg::full_turn();
366 Rad::full_turn() * full_turns
367 }
368}
369impl <S : Real> From <Turn <S>> for Rad <S> {
370 fn from (turns : Turn <S>) -> Self {
371 Rad (turns.0 * Rad::full_turn().0)
372 }
373}
374
375impl <S : OrderedField> Angle <S> for Turn <S> {
379 fn full_turn() -> Self {
380 Turn (S::one())
381 }
382 fn half_turn() -> Self {
383 Turn (S::half())
384 }
385}
386impl <S : Real> From <Rad <S>> for Turn <S> {
387 fn from (radians : Rad <S>) -> Self {
388 Turn (radians.0 / Rad::full_turn().0)
389 }
390}
391impl <S : OrderedField> From <Deg <S>> for Turn <S> {
392 fn from (degrees : Deg <S>) -> Self {
393 Turn (degrees.0 / Deg::full_turn().0)
394 }
395}
396
397impl <S : Real> AngleWrapped <S> {
401 pub fn wrap (angle : Rad <S>) -> Self {
402 AngleWrapped (angle.wrap_unsigned())
403 }
404
405 #[must_use]
406 pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
407 AngleWrapped (f (self.0).wrap_unsigned())
408 }
409}
410
411impl <S : Real> AngleWrappedSigned <S> {
415 pub fn wrap (angle : Rad <S>) -> Self {
416 AngleWrappedSigned (angle.wrap_signed())
417 }
418
419 #[must_use]
420 pub fn map <F> (self, mut f : F) -> Self where F : FnMut (Rad <S>) -> Rad <S> {
421 AngleWrappedSigned (f (self.0).wrap_signed())
422 }
423}
424
425impl <S> Angles3 <S> {
430 pub const fn new (
431 yaw : AngleWrapped <S>,
432 pitch : AngleWrapped <S>,
433 roll : AngleWrapped <S>
434 ) -> Self {
435 Angles3 { yaw, pitch, roll }
436 }
437
438 pub fn zero() -> Self where S : Real {
439 use num::Zero;
440 Angles3::new (
441 AngleWrapped::zero(),
442 AngleWrapped::zero(),
443 AngleWrapped::zero())
444 }
445
446 pub fn wrap (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>) -> Self where S : Real {
447 let yaw = AngleWrapped::wrap (yaw);
448 let pitch = AngleWrapped::wrap (pitch);
449 let roll = AngleWrapped::wrap (roll);
450 Angles3 { yaw, pitch, roll }
451 }
452}
453
454impl <S> From <Rotation3 <S>> for Angles3 <S> where S : Real + MaybeSerDes {
455 fn from (rotation : Rotation3 <S>) -> Self {
456 let (yaw, pitch, roll) = {
457 let (yaw, pitch, roll) = rotation.intrinsic_angles();
458 ( AngleWrapped::wrap (yaw),
459 AngleWrapped::wrap (pitch),
460 AngleWrapped::wrap (roll)
461 )
462 };
463 Angles3 { yaw, pitch, roll }
464 }
465}
466
467impl Axis2 {
471 #[inline]
472 pub const fn component (self) -> usize {
473 self as usize
474 }
475}
476
477impl Axis3 {
481 #[inline]
482 pub const fn component (self) -> usize {
483 self as usize
484 }
485}
486
487impl SignedAxis3 {
491 pub fn to_vec <S : Field> (self) -> Vector3 <S> {
492 match self {
493 SignedAxis3::NegX => [-S::one(), S::zero(), S::zero()].into(),
494 SignedAxis3::PosX => [ S::one(), S::zero(), S::zero()].into(),
495 SignedAxis3::NegY => [ S::zero(), -S::one(), S::zero()].into(),
496 SignedAxis3::PosY => [ S::zero(), S::one(), S::zero()].into(),
497 SignedAxis3::NegZ => [ S::zero(), S::zero(), -S::one() ].into(),
498 SignedAxis3::PosZ => [ S::zero(), S::zero(), S::one() ].into()
499 }
500 }
501 #[inline]
502 pub fn to_unit <S> (self) -> Unit3 <S> where S : Real + std::fmt::Debug {
503 Unit3::unchecked (self.to_vec())
504 }
505}
506impl <S : Field> TryFrom <Vector3 <S>> for SignedAxis3 {
507 type Error = Vector3 <S>;
508 fn try_from (vector : Vector3 <S>) -> Result <Self, Self::Error> {
509 let axis = if vector == Vector3::unit_x() {
510 SignedAxis3::PosX
511 } else if vector == -Vector3::unit_x() {
512 SignedAxis3::NegX
513 } else if vector == Vector3::unit_y() {
514 SignedAxis3::PosY
515 } else if vector == -Vector3::unit_y() {
516 SignedAxis3::NegY
517 } else if vector == Vector3::unit_z() {
518 SignedAxis3::PosZ
519 } else if vector == -Vector3::unit_z() {
520 SignedAxis3::NegZ
521 } else {
522 return Err (vector)
523 };
524 Ok (axis)
525 }
526}
527
528impl Quadrant {
532 pub fn from_vec_strict <S> (vector : Vector2 <S>) -> Option <Quadrant> where
535 S : Ring + SignedExt
536 {
537 let sign_x = vector.x.sign();
538 let sign_y = vector.y.sign();
539 let quadrant = match (sign_x, sign_y) {
540 (Sign::Zero, _) | (_, Sign::Zero) => return None,
541 (Sign::Positive, Sign::Positive) => Quadrant::PosPos,
542 (Sign::Negative, Sign::Positive) => Quadrant::NegPos,
543 (Sign::Positive, Sign::Negative) => Quadrant::PosNeg,
544 (Sign::Negative, Sign::Negative) => Quadrant::NegNeg
545 };
546 Some (quadrant)
547 }
548 #[inline]
550 pub fn from_point_strict <S> (point : Point2 <S>) -> Option <Quadrant> where
551 S : Ring + SignedExt
552 {
553 Quadrant::from_vec_strict (point.to_vector())
554 }
555}
556impl Octant {
562 pub fn from_vec_strict <S> (vector : Vector3 <S>) -> Option <Octant> where
565 S : Ring + SignedExt
566 {
567 let sign_x = vector.x.sign();
568 let sign_y = vector.y.sign();
569 let sign_z = vector.z.sign();
570 let octant = match (sign_x, sign_y, sign_z) {
571 (Sign::Zero, _, _) |
572 ( _, Sign::Zero, _) |
573 ( _, _, Sign::Zero) => return None,
574 (Sign::Positive, Sign::Positive, Sign::Positive) => Octant::PosPosPos,
575 (Sign::Negative, Sign::Positive, Sign::Positive) => Octant::NegPosPos,
576 (Sign::Positive, Sign::Negative, Sign::Positive) => Octant::PosNegPos,
577 (Sign::Negative, Sign::Negative, Sign::Positive) => Octant::NegNegPos,
578 (Sign::Positive, Sign::Positive, Sign::Negative) => Octant::PosPosNeg,
579 (Sign::Negative, Sign::Positive, Sign::Negative) => Octant::NegPosNeg,
580 (Sign::Positive, Sign::Negative, Sign::Negative) => Octant::PosNegNeg,
581 (Sign::Negative, Sign::Negative, Sign::Negative) => Octant::NegNegNeg
582 };
583 Some (octant)
584 }
585 #[inline]
587 pub fn from_point_strict <S> (point : Point3 <S>) -> Option <Octant> where
588 S : Ring + SignedExt
589 {
590 Octant::from_vec_strict (point.to_vector())
591 }
592}
593impl <S> Positive <S> where S : PartialOrd + num::Zero {
599 pub fn new (value : S) -> Option <Self> {
610 if value > S::zero() {
611 Some (Positive (value))
612 } else {
613 None
614 }
615 }
616 pub fn noisy (value : S) -> Self {
625 assert!(value > S::zero());
626 Positive (value)
627 }
628 pub fn unchecked (value : S) -> Self {
637 debug_assert!(value > S::zero());
638 Positive (value)
639 }
640 #[must_use]
661 pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
662 Self::noisy (fun (self.0))
663 }
664}
665impl <S> Positive <S> where S : num::float::FloatCore {
666 pub fn infinity() -> Self {
668 Positive (S::infinity())
669 }
670}
671impl Positive <f32> {
672 pub const fn new_f32 (value : f32) -> Option <Self> {
674 if 0.0 < value {
675 Some (Positive (value))
676 } else {
677 None
678 }
679 }
680 pub const fn noisy_f32 (value : f32) -> Self {
682 assert!(0.0 < value);
683 Positive (value)
684 }
685 pub const fn unchecked_f32 (value : f32) -> Self {
687 debug_assert!(0.0 < value);
688 Positive (value)
689 }
690}
691impl Positive <f64> {
692 pub const fn new_f64 (value : f64) -> Option <Self> {
694 if 0.0 < value {
695 Some (Positive (value))
696 } else {
697 None
698 }
699 }
700 pub const fn noisy_f64 (value : f64) -> Self {
702 assert!(0.0 < value);
703 Positive (value)
704 }
705 pub const fn unchecked_f64 (value : f64) -> Self {
707 debug_assert!(0.0 < value);
708 Positive (value)
709 }
710}
711impl <S : Ring> num::One for Positive <S> {
712 fn one() -> Self {
713 Positive (S::one())
714 }
715}
716impl <S> std::ops::Deref for Positive <S> {
717 type Target = S;
718 fn deref (&self) -> &S {
719 &self.0
720 }
721}
722impl <S : Ring> std::ops::Mul <Positive <S>> for Positive <S> {
723 type Output = Positive <S>;
724 fn mul (self, rhs : Positive <S>) -> Self::Output {
725 Positive (self.0 * *rhs)
726 }
727}
728impl <S : Ring> std::ops::MulAssign <Positive <S>> for Positive <S> {
729 fn mul_assign (&mut self, rhs : Positive <S>) {
730 self.0 *= *rhs
731 }
732}
733impl <S : Ring> std::ops::Mul <NonNegative <S>> for Positive <S> {
734 type Output = NonNegative <S>;
735 fn mul (self, rhs : NonNegative <S>) -> Self::Output {
736 NonNegative (self.0 * *rhs)
737 }
738}
739impl <S : Ring> std::ops::MulAssign <NonNegative <S>> for Positive <S> {
740 fn mul_assign (&mut self, rhs : NonNegative <S>) {
741 self.0 *= *rhs
742 }
743}
744impl <S : Field> std::ops::Div for Positive <S> {
745 type Output = Self;
746 fn div (self, rhs : Self) -> Self::Output {
747 Positive (self.0 / rhs.0)
748 }
749}
750impl <S : Field> std::ops::DivAssign for Positive <S> {
751 fn div_assign (&mut self, rhs : Self) {
752 self.0 /= rhs.0
753 }
754}
755impl <S : Ring> std::ops::Add for Positive <S> {
756 type Output = Self;
757 fn add (self, rhs : Self) -> Self::Output {
758 Positive (self.0 + rhs.0)
759 }
760}
761impl <S : Ring> std::ops::AddAssign for Positive <S> {
762 fn add_assign (&mut self, rhs : Self) {
763 self.0 += rhs.0
764 }
765}
766impl <S : Ring> std::ops::Add <NonNegative <S>> for Positive <S> {
767 type Output = Self;
768 fn add (self, rhs : NonNegative <S>) -> Self::Output {
769 Positive (self.0 + rhs.0)
770 }
771}
772impl <S : Ring> std::ops::AddAssign <NonNegative <S>> for Positive <S> {
773 fn add_assign (&mut self, rhs : NonNegative <S>) {
774 self.0 += rhs.0
775 }
776}
777impl <S> NonNegative <S> where S : PartialOrd + num::Zero {
783 pub fn new (value : S) -> Option <Self> {
794 if value >= S::zero() {
795 Some (NonNegative (value))
796 } else {
797 None
798 }
799 }
800 pub fn noisy (value : S) -> Self {
809 assert!(S::zero() <= value);
810 NonNegative (value)
811 }
812 pub fn unchecked (value : S) -> Self {
822 debug_assert!(value >= S::zero());
823 NonNegative (value)
824 }
825 #[must_use]
843 pub fn map_noisy <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
844 Self::noisy (fun (self.0))
845 }
846}
847impl <S> NonNegative <S> where S : num::Signed {
848 pub fn abs (value : S) -> Self {
858 NonNegative (value.abs())
859 }
860 #[must_use]
870 pub fn map_abs <F> (self, mut fun : F) -> Self where F : FnMut (S) -> S {
871 Self::abs (fun (self.0))
872 }
873}
874impl <S : Sqrt> Sqrt for NonNegative <S> {
875 fn sqrt (self) -> Self {
876 NonNegative (self.0.sqrt())
877 }
878}
879impl <S : MinMax> MinMax for NonNegative <S> {
880 fn min (self, other : Self) -> Self {
881 NonNegative (S::min (self.0, other.0))
882 }
883 fn max (self, other : Self) -> Self {
884 NonNegative (S::max (self.0, other.0))
885 }
886 fn clamp (self, min : Self, max : Self) -> Self {
887 NonNegative (self.0.clamp (min.0, max.0))
888 }
889}
890impl NonNegative <f32> {
891 pub const fn new_f32 (value : f32) -> Option <Self> {
893 if 0.0 <= value {
894 Some (NonNegative (value))
895 } else {
896 None
897 }
898 }
899 pub const fn noisy_f32 (value : f32) -> Self {
901 assert!(0.0 <= value);
902 NonNegative (value)
903 }
904 pub const fn unchecked_f32 (value : f32) -> Self {
906 debug_assert!(0.0 <= value);
907 NonNegative (value)
908 }
909}
910impl NonNegative <f64> {
911 pub const fn new_f64 (value : f64) -> Option <Self> {
913 if 0.0 <= value {
914 Some (NonNegative (value))
915 } else {
916 None
917 }
918 }
919 pub const fn noisy_f64 (value : f64) -> Self {
921 assert!(0.0 <= value);
922 NonNegative (value)
923 }
924 pub const fn unchecked_f64 (value : f64) -> Self {
926 debug_assert!(0.0 <= value);
927 NonNegative (value)
928 }
929}
930impl <S : Ring> From <Positive <S>> for NonNegative <S> {
931 fn from (positive : Positive <S>) -> Self {
932 NonNegative (*positive)
933 }
934}
935impl <S : Ring> num::Zero for NonNegative <S> {
936 fn zero() -> Self {
937 NonNegative (S::zero())
938 }
939 fn is_zero (&self) -> bool {
940 self.0.is_zero()
941 }
942}
943impl <S : Field> num::One for NonNegative <S> {
944 fn one() -> Self {
945 NonNegative (S::one())
946 }
947}
948impl <S> std::ops::Deref for NonNegative <S> {
949 type Target = S;
950 fn deref (&self) -> &S {
951 &self.0
952 }
953}
954impl <S> std::ops::Mul for NonNegative <S> where S : std::ops::Mul <Output=S> {
955 type Output = Self;
956 fn mul (self, rhs : Self) -> Self::Output {
957 NonNegative (self.0 * rhs.0)
958 }
959}
960impl <S> std::ops::Mul <Positive <S>> for NonNegative <S> where
961 S : std::ops::Mul <Output=S>
962{
963 type Output = Self;
964 fn mul (self, rhs : Positive <S>) -> Self::Output {
965 NonNegative (self.0 * rhs.0)
966 }
967}
968impl <S> std::ops::Div for NonNegative <S> where S : std::ops::Div <Output=S> {
969 type Output = Self;
970 fn div (self, rhs : Self) -> Self::Output {
971 NonNegative (self.0 / rhs.0)
972 }
973}
974impl <S> std::ops::Add for NonNegative <S> where S : std::ops::Add <Output=S> {
975 type Output = Self;
976 fn add (self, rhs : Self) -> Self::Output {
977 NonNegative (self.0 + rhs.0)
978 }
979}
980impl <S> NonZero <S> where S : PartialEq + num::Zero {
986 #[inline]
997 pub fn new (value : S) -> Option <Self> {
998 if S::zero() != value {
999 Some (NonZero (value))
1000 } else {
1001 None
1002 }
1003 }
1004 #[inline]
1013 pub fn noisy (value : S) -> Self where S : std::fmt::Debug {
1014 assert_ne!(S::zero(), value);
1015 NonZero (value)
1016 }
1017 #[inline]
1026 pub fn unchecked (value : S) -> Self where S : std::fmt::Debug {
1027 debug_assert_ne!(S::zero(), value);
1028 NonZero (value)
1029 }
1030 #[inline]
1050 #[must_use]
1051 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1052 S : std::fmt::Debug,
1053 F : FnMut (S) -> S
1054 {
1055 Self::noisy (fun (self.0))
1056 }
1057}
1058impl NonZero <f32> {
1059 pub const fn new_f32 (value : f32) -> Option <Self> {
1061 if value != 0.0 {
1062 Some (NonZero (value))
1063 } else {
1064 None
1065 }
1066 }
1067 pub const fn noisy_f32 (value : f32) -> Self {
1069 assert!(value != 0.0);
1070 NonZero (value)
1071 }
1072 pub const fn unchecked_f32 (value : f32) -> Self {
1074 debug_assert!(value != 0.0);
1075 NonZero (value)
1076 }
1077}
1078impl NonZero <f64> {
1079 pub const fn new_f64 (value : f64) -> Option <Self> {
1081 if value != 0.0 {
1082 Some (NonZero (value))
1083 } else {
1084 None
1085 }
1086 }
1087 pub const fn noisy_f64 (value : f64) -> Self {
1089 assert!(value != 0.0);
1090 NonZero (value)
1091 }
1092 pub const fn unchecked_f64 (value : f64) -> Self {
1094 debug_assert!(value != 0.0);
1095 NonZero (value)
1096 }
1097}
1098impl <S : MultiplicativeMonoid> num::One for NonZero <S> {
1099 fn one() -> Self {
1100 NonZero (S::one())
1101 }
1102}
1103impl <S> std::ops::Deref for NonZero <S> {
1104 type Target = S;
1105 fn deref (&self) -> &S {
1106 &self.0
1107 }
1108}
1109impl <S : MultiplicativeMonoid> std::ops::Mul for NonZero <S> {
1110 type Output = Self;
1111 fn mul (self, rhs : Self) -> Self::Output {
1112 NonZero (self.0 * rhs.0)
1113 }
1114}
1115impl <S : PartialEq> Eq for NonZero <S> { }
1116impl <S> Normalized <S> where S : num::One + num::Zero {
1122 #[inline]
1124 pub fn zero() -> Self {
1125 Normalized (S::zero())
1126 }
1127 #[inline]
1128 pub fn is_zero (&self) -> bool {
1129 self.0.is_zero()
1130 }
1131 #[inline]
1145 pub fn new (value : S) -> Option <Self> where S : PartialOrd {
1146 if S::zero() <= value && value <= S::one() {
1147 Some (Normalized (value))
1148 } else {
1149 None
1150 }
1151 }
1152 #[inline]
1163 #[expect(clippy::same_name_method)]
1164 pub fn clamp (value : S) -> Self where S : MinMax {
1165 Normalized (value.clamp (S::zero(), S::one()))
1166 }
1167 #[inline]
1181 pub fn noisy (value : S) -> Self where S : PartialOrd {
1182 assert!(value <= S::one());
1183 assert!(S::zero() <= value);
1184 Normalized (value)
1185 }
1186 pub fn unchecked (value : S) -> Self where S : PartialOrd {
1196 debug_assert!(value <= S::one());
1197 debug_assert!(S::zero() <= value);
1198 Normalized (value)
1199 }
1200 #[inline]
1212 #[must_use]
1213 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1214 S : MinMax,
1215 F : FnMut (S) -> S
1216 {
1217 Self::clamp (fun (self.0))
1218 }
1219 #[inline]
1246 #[must_use]
1247 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1248 S : PartialOrd,
1249 F : FnMut (S) -> S
1250 {
1251 Self::noisy (fun (self.0))
1252 }
1253}
1254impl Normalized <f32> {
1255 pub const fn new_f32 (value : f32) -> Option <Self> {
1257 if 0.0 <= value && value <= 1.0 {
1258 Some (Normalized (value))
1259 } else {
1260 None
1261 }
1262 }
1263 pub const fn noisy_f32 (value : f32) -> Self {
1265 assert!(value <= 1.0);
1266 assert!(0.0 <= value);
1267 Normalized (value)
1268 }
1269 pub const fn unchecked_f32 (value : f32) -> Self {
1271 debug_assert!(value <= 1.0);
1272 debug_assert!(0.0 <= value);
1273 Normalized (value)
1274 }
1275}
1276impl Normalized <f64> {
1277 pub const fn new_f64 (value : f64) -> Option <Self> {
1279 if 0.0 <= value && value <= 1.0 {
1280 Some (Normalized (value))
1281 } else {
1282 None
1283 }
1284 }
1285 pub const fn noisy_f64 (value : f64) -> Self {
1287 assert!(value <= 1.0);
1288 assert!(0.0 <= value);
1289 Normalized (value)
1290 }
1291 pub const fn unchecked_f64 (value : f64) -> Self {
1293 debug_assert!(value <= 1.0);
1294 debug_assert!(0.0 <= value);
1295 Normalized (value)
1296 }
1297}
1298impl <S : Ring> num::One for Normalized <S> {
1299 fn one() -> Self {
1300 Normalized (S::one())
1301 }
1302}
1303impl <S> std::ops::Deref for Normalized <S> {
1304 type Target = S;
1305 fn deref (&self) -> &S {
1306 &self.0
1307 }
1308}
1309impl <S : Ring> std::ops::Mul for Normalized <S> {
1310 type Output = Self;
1311 fn mul (self, rhs : Self) -> Self::Output {
1312 Normalized (self.0 * rhs.0)
1313 }
1314}
1315impl <S : PartialEq> Eq for Normalized <S> { }
1316#[expect(clippy::derive_ord_xor_partial_ord)]
1317impl <S : PartialOrd> Ord for Normalized <S> {
1318 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1319 self.partial_cmp (other).unwrap()
1321 }
1322}
1323impl <S> NormalSigned <S> {
1329 #[inline]
1331 pub fn zero() -> Self where S : num::Zero {
1332 NormalSigned (S::zero())
1333 }
1334 #[inline]
1335 pub fn is_zero (&self) -> bool where S : num::Zero {
1336 self.0.is_zero()
1337 }
1338 #[inline]
1352 pub fn new (value : S) -> Option <Self> where S : OrderedRing {
1353 if -S::one() <= value && value <= S::one() {
1354 Some (NormalSigned (value))
1355 } else {
1356 None
1357 }
1358 }
1359 #[inline]
1370 #[expect(clippy::same_name_method)]
1371 pub fn clamp (value : S) -> Self where S : Ring + MinMax {
1372 NormalSigned (value.clamp (-S::one(), S::one()))
1373 }
1374 #[inline]
1388 pub fn noisy (value : S) -> Self where S : OrderedRing {
1389 assert!(value <= S::one());
1390 assert!(-S::one() <= value);
1391 NormalSigned (value)
1392 }
1393 #[inline]
1405 #[must_use]
1406 pub fn map_clamp <F> (self, mut fun : F) -> Self where
1407 S : Ring + MinMax,
1408 F : FnMut (S) -> S
1409 {
1410 Self::clamp (fun (self.0))
1411 }
1412 #[inline]
1439 #[must_use]
1440 pub fn map_noisy <F> (self, mut fun : F) -> Self where
1441 S : OrderedRing,
1442 F : FnMut (S) -> S
1443 {
1444 Self::noisy (fun (self.0))
1445 }
1446}
1447impl NormalSigned <f32> {
1448 pub const fn new_f32 (value : f32) -> Option <Self> {
1450 if -1.0 <= value && value <= 1.0 {
1451 Some (NormalSigned (value))
1452 } else {
1453 None
1454 }
1455 }
1456 pub const fn noisy_f32 (value : f32) -> Self {
1458 assert!(value <= 1.0);
1459 assert!(-1.0 <= value);
1460 NormalSigned (value)
1461 }
1462 pub const fn unchecked_f32 (value : f32) -> Self {
1464 debug_assert!(value <= 1.0);
1465 debug_assert!(-1.0 <= value);
1466 NormalSigned (value)
1467 }
1468}
1469impl NormalSigned <f64> {
1470 pub const fn new_f64 (value : f64) -> Option <Self> {
1472 if -1.0 <= value && value <= 1.0 {
1473 Some (NormalSigned (value))
1474 } else {
1475 None
1476 }
1477 }
1478 pub const fn noisy_f64 (value : f64) -> Self {
1480 assert!(value <= 1.0);
1481 assert!(-1.0 <= value);
1482 NormalSigned (value)
1483 }
1484 pub const fn unchecked_f64 (value : f64) -> Self {
1486 debug_assert!(value <= 1.0);
1487 debug_assert!(-1.0 <= value);
1488 NormalSigned (value)
1489 }
1490}
1491impl <S : OrderedField> From <Normalized <S>> for NormalSigned <S> {
1492 fn from (Normalized (value) : Normalized <S>) -> Self {
1493 debug_assert!(S::zero() <= value);
1494 debug_assert!(value <= S::one());
1495 NormalSigned (value)
1496 }
1497}
1498impl <S : Field> num::One for NormalSigned <S> {
1499 fn one() -> Self {
1500 NormalSigned (S::one())
1501 }
1502}
1503impl <S> std::ops::Deref for NormalSigned <S> {
1504 type Target = S;
1505 fn deref (&self) -> &S {
1506 &self.0
1507 }
1508}
1509impl <S : Field> std::ops::Mul for NormalSigned <S> {
1510 type Output = Self;
1511 fn mul (self, rhs : Self) -> Self::Output {
1512 NormalSigned (self.0 * rhs.0)
1513 }
1514}
1515impl <S : Field> Eq for NormalSigned <S> { }
1516#[expect(clippy::derive_ord_xor_partial_ord)]
1517impl <S : OrderedField> Ord for NormalSigned <S> {
1518 fn cmp (&self, other : &Self) -> std::cmp::Ordering {
1519 self.partial_cmp (other).unwrap()
1521 }
1522}
1523impl <S : Field> std::ops::Neg for NormalSigned <S> {
1524 type Output = Self;
1525 fn neg (self) -> Self {
1526 NormalSigned (-self.0)
1527 }
1528}
1529impl <S : Ring> Vector2Ext <S> for Vector2 <S> {
1535 fn exterior_product (self, rhs : Vector2 <S>) -> S {
1537 self.x * rhs.y - self.y * rhs.x
1538 }
1539}
1540fn ortho_vec2 <S> (vector : Vector2 <S>) -> Vector2 <S> {
1541 vector.yx()
1542}
1543
1544fn ortho_vec3 <S> (vector : Vector3 <S>) -> Vector3 <S> where
1548 S : Ring + approx::AbsDiffEq <Epsilon = S>
1549{
1550 let mut ortho = vector.cross (Vector3::unit_x());
1554 if approx::abs_diff_eq!(ortho, Vector3::zero()) {
1555 ortho = vector.cross (Vector3::unit_y())
1556 }
1557 ortho
1558}
1559
1560fn ortho_vec4 <S> (_vector : Vector4 <S>) -> Vector4 <S> {
1564 unimplemented!("TODO")
1565}
1566
1567impl <S> std::ops::Deref for Unit <S> {
1571 type Target = S;
1572 fn deref (&self) -> &S {
1573 &self.0
1574 }
1575}
1576
1577impl <S> Rotation2 <S> where S : Ring + MaybeSerDes {
1581 pub fn new (mat : Matrix2 <S>) -> Option <Self> {
1586 let transform = LinearAuto (LinearIso::new (mat)?);
1587 if !transform.is_rotation() {
1588 None
1589 } else {
1590 Some (Rotation2 (transform))
1591 }
1592 }
1593 pub fn from_angle (angle : Rad <S>) -> Self where S : num::real::Real {
1595 Rotation2 (LinearAuto (LinearIso (Matrix2::rotation_z (angle.0), PhantomData)))
1596 }
1597}
1598impl <S> From <Rad <S>> for Rotation2 <S> where
1599 S : Ring + num::real::Real + MaybeSerDes
1600{
1601 fn from (angle : Rad <S>) -> Self {
1602 Rotation2::from_angle (angle)
1603 }
1604}
1605
1606impl <S> Rotation3 <S> where S : Ring + MaybeSerDes {
1610 pub fn new (mat : Matrix3 <S>) -> Option <Self> {
1615 if mat * mat.transposed() != Matrix3::identity() || mat.determinant() != S::one() {
1616 None
1617 } else {
1618 Some (Rotation3 (LinearAuto (LinearIso (mat, PhantomData))))
1619 }
1620 }
1621 pub fn from_angle_x (angle : Rad <S>) -> Self where S : num::real::Real {
1625 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_x (angle.0), PhantomData)))
1626 }
1627 pub fn from_angle_y (angle : Rad <S>) -> Self where S : num::real::Real {
1631 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_y (angle.0), PhantomData)))
1632 }
1633 pub fn from_angle_z (angle : Rad <S>) -> Self where S : num::real::Real {
1637 Rotation3 (LinearAuto (LinearIso (Matrix3::rotation_z (angle.0), PhantomData)))
1638 }
1639 pub fn from_angles_intrinsic (yaw : Rad <S>, pitch : Rad <S>, roll : Rad <S>)
1645 -> Self where S : num::real::Real
1646 {
1647 let rotation1 = Rotation3::from_angle_z (yaw);
1648 let rotation2 = Rotation3::from_axis_angle (rotation1.cols.x, pitch) * rotation1;
1649 Rotation3::from_axis_angle (rotation2.cols.y, roll) * rotation2
1650 }
1651 pub fn from_axis_angle (axis : Vector3 <S>, angle : Rad <S>) -> Self where
1653 S : num::real::Real
1654 {
1655 Rotation3 (LinearAuto (LinearIso (
1656 Matrix3::rotation_3d (angle.0, axis), PhantomData)))
1657 }
1658 pub fn orthonormalize (v1 : Vector3 <S>, v2 : Vector3 <S>, v3 : Vector3 <S>)
1661 -> Option <Self> where S : OrderedField + Sqrt
1662 {
1663 if Matrix3::from_col_arrays ([v1, v2, v3].map (Vector3::into_array)).determinant()
1664 == S::zero()
1665 {
1666 None
1667 } else {
1668 let project = |u : Vector3 <S>, v : Vector3 <S>| u * (u.dot (v) / *u.self_dot());
1669 let u1 = v1;
1670 let u2 = v2 - project (u1, v2);
1671 let u3 = v3 - project (u1, v3) - project (u2, v3);
1672 Some (Rotation3 (LinearAuto (LinearIso (Matrix3::from_col_arrays ([
1673 u1.normalize().into_array(),
1674 u2.normalize().into_array(),
1675 u3.normalize().into_array()
1676 ]), PhantomData))))
1677 }
1678 }
1679
1680 pub fn look_at (point : Point3 <S>) -> Self where
1684 S : num::Float + num::FloatConst + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
1685 {
1686 if point == Point::origin() {
1687 Self::identity()
1688 } else {
1689 use num::Zero;
1690 let forward = point.0.normalized();
1691 let right = {
1692 let projected = forward.with_z (S::zero());
1693 if !projected.is_zero() {
1694 Self::from_angle_z (-Rad (S::FRAC_PI_2())) * projected.normalized()
1695 } else {
1696 Vector3::unit_x()
1697 }
1698 };
1699 let up = right.cross (forward);
1700 let mat =
1701 Matrix3::from_col_arrays ([right, forward, up].map (Vector3::into_array));
1702 Self::noisy_approx (mat)
1703 }
1704 }
1705
1706 pub fn intrinsic_angles (&self) -> (Rad <S>, Rad <S>, Rad <S>) where S : Real {
1711 let cols = &self.cols;
1712 let yaw = Rad (S::atan2 (-cols.y.x, cols.y.y));
1713 let pitch = Rad (S::atan2 (cols.y.z, S::sqrt (S::one() - cols.y.z * cols.y.z)));
1714 let roll = Rad (S::atan2 (-cols.x.z, cols.z.z));
1715 (yaw, pitch, roll)
1716 }
1717
1718 pub fn versor (self) -> Versor <S> where
1722 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
1723 {
1724 let diagonal = self.diagonal();
1725 let t = diagonal.sum();
1726 let m =
1727 MinMax::max (MinMax::max (MinMax::max (diagonal.x, diagonal.y), diagonal.z), t);
1728 let qmax = S::half() * Sqrt::sqrt (S::one() - t + S::two() * m);
1729 let qmax4_recip = (S::four() * qmax).recip();
1730 let [qx, qy, qz, qw] : [S; 4];
1731 let cols = self.cols;
1732 if m == diagonal.x {
1733 qx = qmax;
1734 qy = qmax4_recip * (cols.x.y + cols.y.x);
1735 qz = qmax4_recip * (cols.x.z + cols.z.x);
1736 qw = -qmax4_recip * (cols.z.y - cols.y.z);
1737 } else if m == diagonal.y {
1738 qx = qmax4_recip * (cols.x.y + cols.y.x);
1739 qy = qmax;
1740 qz = qmax4_recip * (cols.y.z + cols.z.y);
1741 qw = -qmax4_recip * (cols.x.z - cols.z.x);
1742 } else if m == diagonal.z {
1743 qx = qmax4_recip * (cols.x.z + cols.z.x);
1744 qy = qmax4_recip * (cols.y.z + cols.z.y);
1745 qz = qmax;
1746 qw = -qmax4_recip * (cols.y.x - cols.x.y);
1747 } else {
1748 qx = -qmax4_recip * (cols.z.y - cols.y.z);
1749 qy = -qmax4_recip * (cols.x.z - cols.z.x);
1750 qz = -qmax4_recip * (cols.y.x - cols.x.y);
1751 qw = qmax;
1752 }
1753 let quat = Quaternion::from_xyzw (qx, qy, qz, qw);
1754 Versor::unchecked_approx (quat)
1755 }
1756}
1757impl <S> From <Angles3 <S>> for Rotation3 <S> where
1758 S : Real + num::real::Real + MaybeSerDes
1759{
1760 fn from (angles : Angles3 <S>) -> Self {
1761 Rotation3::from_angles_intrinsic (
1762 angles.yaw.angle(), angles.pitch.angle(), angles.roll.angle())
1763 }
1764}
1765
1766impl <S : Real> Versor <S> {
1770 pub fn normalize (quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
1774 assert_ne!(quaternion, Quaternion::zero());
1775 Versor (normalize_quaternion (quaternion))
1776 }
1777 pub fn noisy (unit_quaternion : Quaternion <S>) -> Self where
1781 S : num::real::Real + std::fmt::Debug
1782 {
1783 assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
1784 Versor (unit_quaternion)
1785 }
1786 pub fn noisy_approx (unit_quaternion : Quaternion <S>) -> Self where
1790 S : num::real::Real + approx::RelativeEq <Epsilon=S>
1791 {
1792 assert!(unit_quaternion.into_vec4().is_normalized());
1793 Versor (unit_quaternion)
1794 }
1795 pub fn unchecked (unit_quaternion : Quaternion <S>) -> Self where S : std::fmt::Debug {
1799 debug_assert_eq!(unit_quaternion, normalize_quaternion (unit_quaternion));
1800 Versor (unit_quaternion)
1801 }
1802 pub fn unchecked_approx (unit_quaternion : Quaternion <S>) -> Self where
1806 S : num::real::Real + approx::RelativeEq <Epsilon=S>
1807 {
1808 debug_assert!(unit_quaternion.into_vec4().is_normalized());
1809 Versor (unit_quaternion)
1810 }
1811 pub fn from_scaled_axis (scaled_axis : Vector3 <S>) -> Self where
1814 S : std::fmt::Debug + num::Float
1815 {
1816 if scaled_axis == Vector3::zero() {
1817 let versor = Quaternion::rotation_3d (S::zero(), scaled_axis);
1818 debug_assert_eq!(versor.magnitude(), S::one());
1819 Versor (versor)
1820 } else {
1821 let angle = scaled_axis.magnitude();
1822 debug_assert!(S::zero() < angle);
1823 let axis = scaled_axis / angle;
1824 Versor (Quaternion::rotation_3d (angle, axis))
1825 }
1826 }
1827}
1828impl <S> Default for Versor <S> where S : num::One + num::Zero {
1829 fn default() -> Self {
1830 Versor (Quaternion::default())
1831 }
1832}
1833impl <S> std::ops::Mul <Vector3 <S>> for Versor <S> where S : Real + num::Float {
1834 type Output = Vector3 <S>;
1835 fn mul (self, rhs : Vector3 <S>) -> Self::Output {
1836 self.0 * rhs
1837 }
1838}
1839impl <S> From <Rotation3 <S>> for Versor <S> where
1840 S : Real + MaybeSerDes + num::real::Real + approx::RelativeEq <Epsilon=S>
1841{
1842 fn from (rot : Rotation3 <S>) -> Self {
1843 rot.versor()
1844 }
1845}
1846impl <S> std::ops::Deref for Versor <S> {
1847 type Target = Quaternion <S>;
1848 fn deref (&self) -> &Quaternion <S> {
1849 &self.0
1850 }
1851}
1852impl <S, V, W, M> LinearIso <S, V, W, M> where
1858 M : LinearMap <S, V, W> + Copy,
1859 V : Module <S>,
1860 W : Module <S>,
1861 S : Ring
1862{
1863 pub fn mat (self) -> M {
1864 *self
1865 }
1866 pub fn is_invertible (linear_map : M) -> bool {
1868 linear_map.determinant() != S::zero()
1869 }
1870 pub fn is_invertible_approx (linear_map : M, epsilon : Option <S>) -> bool where
1872 S : approx::AbsDiffEq <Epsilon=S>
1873 {
1874 let epsilon = epsilon.unwrap_or_else (||{
1875 let four = S::one() + S::one() + S::one() + S::one();
1876 S::default_epsilon() * four
1877 });
1878 approx::abs_diff_ne!(linear_map.determinant(), S::zero(), epsilon=epsilon)
1879 }
1880 pub fn new (linear_map : M) -> Option <Self> {
1882 if Self::is_invertible (linear_map) {
1883 Some (LinearIso (linear_map, PhantomData))
1884 } else {
1885 None
1886 }
1887 }
1888 pub fn new_approx (linear_map : M, epsilon : Option <S>) -> Option <Self> where
1890 S : approx::RelativeEq <Epsilon=S>
1891 {
1892 if Self::is_invertible_approx (linear_map, epsilon) {
1893 Some (LinearIso (linear_map, PhantomData))
1894 } else {
1895 None
1896 }
1897 }
1898}
1899impl <S, V, W, M> std::ops::Deref for LinearIso <S, V, W, M> where
1900 M : LinearMap <S, V, W>,
1901 V : Module <S>,
1902 W : Module <S>,
1903 S : Ring
1904{
1905 type Target = M;
1906 fn deref (&self) -> &Self::Target {
1907 &self.0
1908 }
1909}
1910impl <S, V> num::One for LinearIso <S, V, V, V::LinearEndo> where
1911 V : Module <S>,
1912 S : Ring
1913{
1914 fn one () -> Self {
1915 LinearIso (V::LinearEndo::one(), PhantomData)
1916 }
1917}
1918impl <S, V, W, M> std::ops::Mul <V> for LinearIso <S, V, W, M> where
1919 M : LinearMap <S, V, W>,
1920 V : Module <S>,
1921 W : Module <S>,
1922 S : Ring
1923{
1924 type Output = W;
1925 fn mul (self, rhs : V) -> Self::Output {
1926 self.0 * rhs
1927 }
1928}
1929impl <S, V> std::ops::Mul for LinearIso <S, V, V, V::LinearEndo> where
1930 V : Module <S>,
1931 S : Ring
1932{
1933 type Output = Self;
1934 fn mul (self, rhs : Self) -> Self::Output {
1935 LinearIso (self.0 * rhs.0, PhantomData)
1936 }
1937}
1938impl <S, V> std::ops::MulAssign for LinearIso <S, V, V, V::LinearEndo> where
1939 V : Module <S>,
1940 S : Ring
1941{
1942 fn mul_assign (&mut self, rhs : Self) {
1943 self.0 *= rhs.0
1944 }
1945}
1946
1947impl <S, V> LinearAuto <S, V> where
1951 V : Module <S>,
1952 V::LinearEndo : MaybeSerDes,
1953 S : Ring
1954{
1955 pub fn mat (self) -> V::LinearEndo {
1956 **self
1957 }
1958 pub fn is_orthonormal (&self) -> bool {
1963 use num::One;
1964 let determinant = self.0.0.determinant();
1965 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
1966 (determinant == S::one() || determinant == -S::one())
1967 }
1968 pub fn is_orthonormal_approx (&self) -> bool where
1974 S : approx::RelativeEq <Epsilon=S>,
1975 V::LinearEndo : approx::RelativeEq <Epsilon=S>
1976 {
1977 use num::One;
1978 let four = S::one() + S::one() + S::one() + S::one();
1979 let epsilon = S::default_epsilon() * four;
1980 let max_relative = S::default_max_relative() * four;
1981 let determinant = self.0.0.determinant();
1982 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
1983 max_relative=max_relative, epsilon=epsilon) &&
1984 ( approx::relative_eq!(determinant, S::one(), max_relative=max_relative) ||
1985 approx::relative_eq!(determinant, -S::one(), max_relative=max_relative) )
1986 }
1987 pub fn is_rotation (&self) -> bool {
1991 use num::One;
1992 let determinant = self.0.0.determinant();
1993 self.0.0 * self.0.0.transpose() == V::LinearEndo::one() &&
1994 determinant == S::one()
1995 }
1996 pub fn is_rotation_approx (&self) -> bool where
2001 S : approx::RelativeEq <Epsilon=S>,
2002 V::LinearEndo : approx::RelativeEq <Epsilon=S>
2003 {
2004 use num::One;
2005 let four = S::one() + S::one() + S::one() + S::one();
2006 let epsilon = S::default_epsilon() * four;
2007 let max_relative = S::default_max_relative() * four;
2008 let determinant = self.0.0.determinant();
2009 approx::relative_eq!(self.0.0 * self.0.0.transpose(), V::LinearEndo::one(),
2010 max_relative=max_relative, epsilon=epsilon) &&
2011 approx::relative_eq!(determinant, S::one(), max_relative=max_relative)
2012 }
2013}
2014impl <S, V> std::ops::Mul <V> for LinearAuto <S, V> where
2015 V : Module <S>,
2016 V::LinearEndo : MaybeSerDes,
2017 S : Ring
2018{
2019 type Output = V;
2020 fn mul (self, rhs : V) -> Self::Output {
2021 self.0 * rhs
2022 }
2023}
2024impl <S, V> std::ops::Mul for LinearAuto <S, V> where
2025 V : Module <S>,
2026 V::LinearEndo : MaybeSerDes,
2027 S : Ring
2028{
2029 type Output = Self;
2030 fn mul (self, rhs : Self) -> Self::Output {
2031 LinearAuto (self.0 * rhs.0)
2032 }
2033}
2034impl <S, V> std::ops::MulAssign <Self> for LinearAuto <S, V> where
2035 V : Module <S>,
2036 V::LinearEndo : MaybeSerDes,
2037 S : Ring
2038{
2039 fn mul_assign (&mut self, rhs : Self) {
2040 self.0 *= rhs.0
2041 }
2042}
2043impl <S, V> num::One for LinearAuto <S, V> where
2044 V : Module <S>,
2045 V::LinearEndo : MaybeSerDes,
2046 S : Ring
2047{
2048 fn one() -> Self {
2049 LinearAuto (LinearIso::one())
2050 }
2051}
2052
2053impl <S, A, B, M> AffineMap <S, A, B, M> where
2057 A : AffineSpace <S>,
2058 B : AffineSpace <S>,
2059 M : LinearMap <S, A::Translation, B::Translation>,
2060 S : Field
2061{
2062 pub const fn new (linear_map : M, translation : B::Translation) -> Self {
2063 AffineMap { linear_map, translation, _phantom: PhantomData }
2064 }
2065 pub fn map (self, point : A) -> B {
2066 B::from_vector (self.linear_map * point.to_vector() + self.translation)
2067 }
2068 pub fn transform (self, translation : A::Translation) -> B::Translation {
2069 self.linear_map * translation
2070 }
2071}
2072
2073impl <S, A, B, M> Affinity <S, A, B, M> where
2077 M : LinearMap <S, A::Translation, B::Translation>,
2078 A : AffineSpace <S>,
2079 B : AffineSpace <S>,
2080 S : Field
2081{
2082 pub const fn new (
2083 linear_iso : LinearIso <S, A::Translation, B::Translation, M>,
2084 translation : B::Translation
2085 ) -> Self {
2086 Affinity { linear_iso, translation }
2087 }
2088 pub fn mat (self) -> M {
2089 *self.linear_iso
2090 }
2091 pub fn map (self, point : A) -> B {
2092 B::from_vector (self.linear_iso * point.to_vector() + self.translation)
2093 }
2094 pub fn transform (self, translation : A::Translation) -> B::Translation {
2095 self.linear_iso * translation
2096 }
2097}
2098
2099impl <S, P, Q, M> Projectivity <S, P, Q, M> where
2103 M : LinearMap <S, P::Translation, Q::Translation>,
2104 P : ProjectiveSpace <S>,
2105 Q : ProjectiveSpace <S>,
2106 S : Field
2107{
2108 pub const fn new (linear_iso : LinearIso <S, P::Translation, Q::Translation, M>)
2109 -> Self
2110 {
2111 Projectivity (linear_iso)
2112 }
2113 pub fn mat (self) -> M {
2114 **self
2115 }
2116}
2117impl <S, P, Q, M> std::ops::Deref for Projectivity <S, P, Q, M> where
2118 M : LinearMap <S, P::Translation, Q::Translation>,
2119 P : ProjectiveSpace <S>,
2120 Q : ProjectiveSpace <S>,
2121 S : Field
2122{
2123 type Target = LinearIso <S, P::Translation, Q::Translation, M>;
2124 fn deref (&self) -> &Self::Target {
2125 &self.0
2126 }
2127}
2128impl <S, P, Q, M> std::ops::Mul <P> for Projectivity <S, P, Q, M> where
2129 M : LinearMap <S, P::Translation, Q::Translation>,
2130 P : ProjectiveSpace <S>,
2131 Q : ProjectiveSpace <S>,
2132 S : Field
2133{
2134 type Output = Q;
2135 fn mul (self, rhs : P) -> Q {
2136 Q::from_vector (self.0 * rhs.to_vector())
2137 }
2138}
2139
2140pub macro nonzero {
2141 ($x:expr) => {
2142 $crate::NonZero::new ($x)
2143 },
2144 ($x:expr, $y:expr) => {
2145 $crate::NonZero2::new (Vector2::new ($x, $y))
2146 },
2147 ($x:expr, $y:expr, $z:expr) => {
2148 $crate::NonZero3::new (Vector3::new ($x, $y, $z))
2149 },
2150 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2151 $crate::NonZero4::new (Vector4::new ($x, $y, $z, $w))
2152 }
2153}
2154
2155pub macro point {
2156 ($x:expr, $y:expr) => {
2157 $crate::point2 ($x, $y)
2158 },
2159 ($x:expr, $y:expr, $z:expr) => {
2160 $crate::point3 ($x, $y, $z)
2161 },
2162 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2163 $crate::point4 ($x, $y, $z, $w)
2164 }
2165}
2166
2167pub macro vector {
2168 ($x:expr, $y:expr) => {
2169 $crate::vector2 ($x, $y)
2170 },
2171 ($x:expr, $y:expr, $z:expr) => {
2172 $crate::vector3 ($x, $y, $z)
2173 },
2174 ($x:expr, $y:expr, $z:expr, $w:expr) => {
2175 $crate::vector4 ($x, $y, $z, $w)
2176 }
2177}
2178
2179pub macro matrix {
2180 ($v00:expr, $v01:expr; $v10:expr, $v11:expr$(;)?) => {
2181 $crate::Matrix2::new ($v00, $v01, $v10, $v11)
2182 },
2183 ( $v00:expr, $v01:expr, $v02:expr;
2184 $v10:expr, $v11:expr, $v12:expr;
2185 $v20:expr, $v21:expr, $v22:expr$(;)?
2186 ) => {
2187 $crate::Matrix3::new ($v00, $v01, $v02, $v10, $v11, $v12, $v20, $v21, $v22)
2188 },
2189 ( $v00:expr, $v01:expr, $v02:expr, $v03:expr;
2190 $v10:expr, $v11:expr, $v12:expr, $v13:expr;
2191 $v20:expr, $v21:expr, $v22:expr, $v23:expr;
2192 $v30:expr, $v31:expr, $v32:expr, $v33:expr$(;)?
2193 ) => {
2194 $crate::Matrix4::new (
2195 $v00, $v01, $v02, $v03,
2196 $v10, $v11, $v12, $v13,
2197 $v20, $v21, $v22, $v23,
2198 $v30, $v31, $v32, $v33)
2199 }
2200}
2201
2202macro_rules! impl_angle {
2203 ( $angle:ident, $docname:expr, $mk_rad:expr) => {
2204 #[doc = $docname]
2205 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2206 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2208 pub struct $angle <S> (pub S);
2209 impl_numcast_primitive_map!($angle);
2210 impl <S : Ring> std::iter::Sum for $angle <S> {
2211 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2212 use num::Zero;
2213 iter.fold ($angle::zero(), |acc, item| acc + item)
2214 }
2215 }
2216 impl <S : Ring> std::ops::Neg for $angle <S> {
2217 type Output = Self;
2218 fn neg (self) -> Self {
2219 $angle (-self.0)
2220 }
2221 }
2222 impl <S : Ring> std::ops::Add for $angle <S> {
2223 type Output = Self;
2224 fn add (self, other : Self) -> Self::Output {
2225 $angle (self.0 + other.0)
2226 }
2227 }
2228 impl <S : Ring> std::ops::AddAssign for $angle <S> {
2229 fn add_assign (&mut self, other : Self) {
2230 self.0 += other.0
2231 }
2232 }
2233 impl <S : Ring> std::ops::Sub for $angle <S> {
2234 type Output = Self;
2235 fn sub (self, other : Self) -> Self::Output {
2236 $angle (self.0 - other.0)
2237 }
2238 }
2239 impl <S : Ring> std::ops::SubAssign for $angle <S> {
2240 fn sub_assign (&mut self, other : Self) {
2241 self.0 -= other.0
2242 }
2243 }
2244 impl <S : Ring> std::ops::Mul <S> for $angle <S> {
2245 type Output = Self;
2246 fn mul (self, scalar : S) -> Self::Output {
2247 $angle (scalar * self.0)
2248 }
2249 }
2250 impl <S : Ring> std::ops::MulAssign <S> for $angle <S> {
2251 fn mul_assign (&mut self, scalar : S) {
2252 self.0 *= scalar
2253 }
2254 }
2255 impl <S : Field> std::ops::Div for $angle <S> {
2256 type Output = S;
2257 fn div (self, other : Self) -> Self::Output {
2258 self.0 / other.0
2259 }
2260 }
2261 impl <S : Field> std::ops::Div <S> for $angle <S> {
2262 type Output = Self;
2263 fn div (self, scalar : S) -> Self::Output {
2264 $angle (self.0 / scalar)
2265 }
2266 }
2267 impl <S : Field> std::ops::DivAssign <S> for $angle <S> {
2268 fn div_assign (&mut self, scalar : S) {
2269 self.0 /= scalar
2270 }
2271 }
2272 impl <S : OrderedField> std::ops::Rem for $angle <S> {
2273 type Output = Self;
2274 fn rem (self, other : Self) -> Self::Output {
2275 $angle (self.0 % other.0)
2276 }
2277 }
2278 impl <S : Ring> num::Zero for $angle <S> {
2279 fn zero() -> Self {
2280 $angle (S::zero())
2281 }
2282 fn is_zero (&self) -> bool {
2283 self.0.is_zero()
2284 }
2285 }
2286 impl <S : Ring> From <S> for $angle <S> {
2287 fn from (s : S) -> Self {
2288 $angle (s)
2289 }
2290 }
2291 impl <S : Real> Trig <S> for $angle <S> {
2292 fn sin (self) -> S {
2293 $mk_rad (self).sin()
2294 }
2295 fn sin_cos (self) -> (S, S) {
2296 $mk_rad (self).sin_cos()
2297 }
2298 fn cos (self) -> S {
2299 $mk_rad (self).cos()
2300 }
2301 fn tan (self) -> S {
2302 $mk_rad (self).tan()
2303 }
2304 fn asin (self) -> S {
2305 $mk_rad (self).asin()
2306 }
2307 fn acos (self) -> S {
2308 $mk_rad (self).acos()
2309 }
2310 fn atan (self) -> S {
2311 $mk_rad (self).atan()
2312 }
2313 fn atan2 (self, other : Self) -> S {
2314 $mk_rad (self).atan2 ($mk_rad (other))
2315 }
2316 }
2317 }
2318}
2319
2320macro_rules! impl_angle_wrapped {
2321 ( $angle:ident, $comment:expr ) => {
2322 #[doc = $comment]
2323 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2324 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, PartialOrd)]
2325 pub struct $angle <S> (Rad <S>);
2326
2327 impl_numcast!($angle);
2328 impl <S : Copy> $angle <S> {
2329 pub const fn angle (&self) -> Rad <S> {
2330 self.0
2331 }
2332 }
2333 impl <S : Real> std::iter::Sum for $angle <S> {
2334 fn sum <I> (iter : I) -> Self where I : Iterator <Item = Self> {
2335 use num::Zero;
2336 iter.fold ($angle::zero(), |acc, item| acc + item)
2337 }
2338 }
2339 impl <S : Real> std::ops::Neg for $angle <S> {
2340 type Output = Self;
2341 fn neg (self) -> Self {
2342 self.map (Rad::neg)
2343 }
2344 }
2345 impl <S : Real> std::ops::Add for $angle <S> {
2346 type Output = Self;
2347 fn add (self, other : Self) -> Self::Output {
2348 self.map (|angle| angle + other.0)
2349 }
2350 }
2351 impl <S : Real> std::ops::AddAssign for $angle <S> {
2352 fn add_assign (&mut self, other : Self) {
2353 *self = *self + other
2354 }
2355 }
2356 impl <S : Real> std::ops::Add <Rad <S>> for $angle <S> {
2357 type Output = Self;
2358 fn add (self, angle : Rad <S>) -> Self::Output {
2359 self.map (|a| a + angle)
2360 }
2361 }
2362 impl <S : Real> std::ops::AddAssign <Rad <S>> for $angle <S> {
2363 fn add_assign (&mut self, angle : Rad <S>) {
2364 *self = *self + angle
2365 }
2366 }
2367 impl <S : Real> std::ops::Sub for $angle <S> {
2368 type Output = Self;
2369 fn sub (self, other : Self) -> Self::Output {
2370 self.map (|angle| angle - other.0)
2371 }
2372 }
2373 impl <S : Real> std::ops::SubAssign for $angle <S> {
2374 fn sub_assign (&mut self, other : Self) {
2375 *self = *self - other
2376 }
2377 }
2378 impl <S : Real> std::ops::Sub <Rad <S>> for $angle <S> {
2379 type Output = Self;
2380 fn sub (self, angle : Rad <S>) -> Self::Output {
2381 self.map (|a| a - angle)
2382 }
2383 }
2384 impl <S : Real> std::ops::SubAssign <Rad <S>> for $angle <S> {
2385 fn sub_assign (&mut self, angle : Rad <S>) {
2386 *self = *self - angle
2387 }
2388 }
2389 impl <S : Real> std::ops::Mul <S> for $angle <S> {
2390 type Output = Self;
2391 fn mul (self, scalar : S) -> Self::Output {
2392 self.map (|angle| angle * scalar)
2393 }
2394 }
2395 impl <S : Real> std::ops::MulAssign <S> for $angle <S> {
2396 fn mul_assign (&mut self, scalar : S) {
2397 *self = *self * scalar
2398 }
2399 }
2400 impl <S : Real> std::ops::Div for $angle <S> {
2401 type Output = S;
2402 fn div (self, other : Self) -> Self::Output {
2403 self.0 / other.0
2404 }
2405 }
2406 impl <S : Real> std::ops::Div <S> for $angle <S> {
2407 type Output = Self;
2408 fn div (self, scalar : S) -> Self::Output {
2409 self.map (|angle| angle / scalar)
2410 }
2411 }
2412 impl <S : Real> std::ops::DivAssign <S> for $angle <S> {
2413 fn div_assign (&mut self, scalar : S) {
2414 *self = *self / scalar
2415 }
2416 }
2417 impl <S : Real> std::ops::Rem for $angle <S> {
2418 type Output = Self;
2419 fn rem (self, other : Self) -> Self::Output {
2420 self.map (|angle| angle % other.0)
2421 }
2422 }
2423 impl <S : Real> num::Zero for $angle <S> {
2424 fn zero() -> Self {
2425 $angle (Rad::zero())
2426 }
2427 fn is_zero (&self) -> bool {
2428 self.0.is_zero()
2429 }
2430 }
2431 }
2432}
2433
2434macro_rules! impl_dimension {
2435 ( $point:ident, $vector:ident, $nonzero:ident, $unit:ident,
2436 $point_fun:ident, $vector_fun:ident, $matrix_fun:ident, $ortho_fun:ident,
2437 [$($component:ident),+],
2438 [$(($axis_method:ident, $unit_method:ident)),+], [$($patch:ident)?],
2439 [$($projective:ident)?],
2440 $matrix:ident, [$($submatrix:ident)?], [$($rotation:ident)?],
2441 $ndims:expr, $dimension:expr
2442 ) => {
2443 #[doc = $dimension]
2444 #[doc = "position"]
2445 #[derive(Clone, Copy, Debug, Default, Eq, PartialEq, Display, From, Neg)]
2446 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2447 #[display("{}", _0)]
2448 #[repr(C)]
2449 pub struct $point <S> (pub $vector <S>);
2450
2451 #[doc = $dimension]
2452 #[doc = "non-zero vector"]
2453 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2454 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2455 #[display("{}", _0)]
2456 #[repr(C)]
2457 pub struct $nonzero <S> ($vector <S>);
2458
2459 #[doc = $dimension]
2460 #[doc = "unit vector"]
2461 #[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
2462 #[derive(Clone, Copy, Debug, Eq, PartialEq, Display)]
2463 #[display("{}", _0)]
2464 #[repr(C)]
2465 pub struct $unit <S> ($vector <S>);
2466
2467 #[doc = "Construct a"]
2471 #[doc = $dimension]
2472 #[doc = "point"]
2473 pub const fn $point_fun <S> ($($component : S),+) -> $point <S> {
2474 $point::new ($($component),+)
2475 }
2476 impl_numcast!($point);
2477 impl <S> $point <S> {
2478 pub const fn new ($($component : S),+) -> Self {
2479 $point ($vector::new ($($component),+))
2480 }
2481 $(
2482 pub const fn $component (&self) -> S where S : Copy {
2483 self.0.$component
2484 }
2485 )+
2486 pub fn distance (self, other : $point <S>) -> NonNegative <S> where
2487 S : OrderedField + Sqrt
2488 {
2489 MetricSpace::distance (self.0, other.0)
2490 }
2491 pub fn distance_squared (self, other : $point <S>) -> NonNegative <S> where
2492 S : OrderedField
2493 {
2494 MetricSpace::distance_squared (self.0, other.0)
2495 }
2496 }
2497 $(
2499 impl <S : Field> ProjectiveSpace <S> for $projective <S> {
2500 type Patch = $point <S>;
2501 }
2502 )?
2503 impl <S : Field> AffineSpace <S> for $point <S> {
2504 type Translation = $vector <S>;
2505 }
2506 impl <S : Ring> Point <$vector <S>> for $point <S> {
2507 fn to_vector (self) -> $vector <S> {
2508 self.0
2509 }
2510 fn from_vector (vector : $vector <S>) -> Self {
2511 $point (vector)
2512 }
2513 }
2514 impl <S> From <[S; $ndims]> for $point <S> {
2515 fn from (array : [S; $ndims]) -> Self {
2516 $point (array.into())
2517 }
2518 }
2519 $(
2520 impl <S> From <($patch <S>, S)> for $point <S> {
2521 fn from ((patch, scale) : ($patch <S>, S)) -> Self {
2522 $point ((patch.0, scale).into())
2523 }
2524 }
2525 )?
2526 impl <S> std::cmp::PartialOrd for $point <S> where S : PartialOrd {
2527 fn partial_cmp (&self, other : &Self) -> Option <std::cmp::Ordering> {
2528 self.0.as_slice().partial_cmp (&other.0.as_slice())
2529 }
2530 }
2531 impl <S> std::ops::Add <$vector <S>> for $point <S> where S : AdditiveGroup {
2532 type Output = Self;
2533 fn add (self, displacement : $vector <S>) -> Self::Output {
2534 $point (self.0 + displacement)
2535 }
2536 }
2537 impl <S> std::ops::Add <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2538 type Output = Self;
2539 fn add (self, displacement : &$vector <S>) -> Self::Output {
2540 $point (self.0 + *displacement)
2541 }
2542 }
2543 impl <S> std::ops::Add <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2544 type Output = $point <S>;
2545 fn add (self, displacement : $vector <S>) -> Self::Output {
2546 $point (self.0 + displacement)
2547 }
2548 }
2549 impl <S> std::ops::Add <&$vector <S>> for &$point <S> where
2550 S : AdditiveGroup + Copy
2551 {
2552 type Output = $point <S>;
2553 fn add (self, displacement : &$vector <S>) -> Self::Output {
2554 $point (self.0 + *displacement)
2555 }
2556 }
2557 impl <S> std::ops::AddAssign <$vector <S>> for $point <S> where S : AdditiveGroup {
2558 fn add_assign (&mut self, displacement : $vector <S>) {
2559 self.0 += displacement
2560 }
2561 }
2562 impl <S> std::ops::AddAssign <&$vector <S>> for $point <S> where
2563 S : AdditiveGroup + Copy
2564 {
2565 fn add_assign (&mut self, displacement : &$vector <S>) {
2566 self.0 += *displacement
2567 }
2568 }
2569 impl <S> std::ops::Sub <$vector <S>> for $point <S> where S : AdditiveGroup {
2570 type Output = Self;
2571 fn sub (self, displacement : $vector <S>) -> Self::Output {
2572 $point (self.0 - displacement)
2573 }
2574 }
2575 impl <S> std::ops::Sub <&$vector <S>> for $point <S> where S : AdditiveGroup + Copy {
2576 type Output = Self;
2577 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2578 $point (self.0 - *displacement)
2579 }
2580 }
2581 impl <S> std::ops::Sub <$point <S>> for $point <S> where S : AdditiveGroup {
2582 type Output = $vector <S>;
2583 fn sub (self, other : Self) -> Self::Output {
2584 self.0 - other.0
2585 }
2586 }
2587 impl <S> std::ops::Sub <&$point <S>> for $point <S> where S : AdditiveGroup + Copy {
2588 type Output = $vector <S>;
2589 fn sub (self, other : &Self) -> Self::Output {
2590 self.0 - other.0
2591 }
2592 }
2593 impl <S> std::ops::Sub <$vector <S>> for &$point <S> where S : AdditiveGroup + Copy {
2594 type Output = $point <S>;
2595 fn sub (self, displacement : $vector <S>) -> Self::Output {
2596 $point (self.0 - displacement)
2597 }
2598 }
2599 impl <S> std::ops::Sub <&$vector <S>> for &$point <S> where
2600 S : AdditiveGroup + Copy
2601 {
2602 type Output = $point <S>;
2603 fn sub (self, displacement : &$vector <S>) -> Self::Output {
2604 $point (self.0 - *displacement)
2605 }
2606 }
2607 impl <S> std::ops::Sub <$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2608 type Output = $vector <S>;
2609 fn sub (self, other : $point <S>) -> Self::Output {
2610 self.0 - other.0
2611 }
2612 }
2613 impl <S> std::ops::Sub <&$point <S>> for &$point <S> where S : AdditiveGroup + Copy {
2614 type Output = $vector <S>;
2615 fn sub (self, other : &$point <S>) -> Self::Output {
2616 self.0 - other.0
2617 }
2618 }
2619 impl <S> std::ops::SubAssign <$vector <S>> for $point <S> where
2620 S : AdditiveGroup
2621 {
2622 fn sub_assign (&mut self, displacement : $vector <S>) {
2623 self.0 -= displacement
2624 }
2625 }
2626 impl <S> std::ops::SubAssign <&$vector <S>> for $point <S> where
2627 S : AdditiveGroup + Copy
2628 {
2629 fn sub_assign (&mut self, displacement : &$vector <S>) {
2630 self.0 -= *displacement
2631 }
2632 }
2633 impl <S> approx::AbsDiffEq for $point <S> where
2634 S : approx::AbsDiffEq,
2635 S::Epsilon : Copy
2636 {
2637 type Epsilon = S::Epsilon;
2638 fn default_epsilon() -> Self::Epsilon {
2639 S::default_epsilon()
2640 }
2641 #[inline]
2642 fn abs_diff_eq (&self, other : &Self, epsilon : Self::Epsilon) -> bool {
2643 self.0.abs_diff_eq (&other.0, epsilon)
2644 }
2645 }
2646 impl <S> approx::RelativeEq for $point <S> where
2647 S : approx::RelativeEq,
2648 S::Epsilon : Copy
2649 {
2650 fn default_max_relative() -> Self::Epsilon {
2651 S::default_max_relative()
2652 }
2653 #[inline]
2654 fn relative_eq (&self,
2655 other : &Self, epsilon : Self::Epsilon, max_relative : Self::Epsilon
2656 ) -> bool {
2657 self.0.relative_eq (&other.0, epsilon, max_relative)
2658 }
2659 }
2660 impl <S> approx::UlpsEq for $point <S> where
2661 S : approx::UlpsEq,
2662 S::Epsilon : Copy
2663 {
2664 fn default_max_ulps() -> u32 {
2665 S::default_max_ulps()
2666 }
2667 #[inline]
2668 fn ulps_eq (&self, other : &Self, epsilon : Self::Epsilon, max_ulps : u32)
2669 -> bool
2670 {
2671 self.0.ulps_eq (&other.0, epsilon, max_ulps)
2672 }
2673 }
2674 #[doc = "Construct a"]
2678 #[doc = $dimension]
2679 #[doc = "vector"]
2680 pub const fn $vector_fun <S> ($($component : S),+) -> $vector <S> {
2681 $vector::new ($($component),+)
2682 }
2683 impl <S> NormedVectorSpace <S> for $vector <S> where S : OrderedField {
2684 type Unit = $unit <S>;
2685 fn norm_squared (self) -> NonNegative <S> {
2686 self.self_dot()
2687 }
2688 fn norm_max (self) -> NonNegative <S> where S : SignedExt {
2689 NonNegative::unchecked (self.map (|x| x.abs()).reduce_partial_max())
2690 }
2691 fn normalize (self) -> Self::Unit where S : Sqrt {
2692 $unit (self / *self.norm())
2693 }
2694 }
2695 impl <S> InnerProductSpace <S> for $vector <S> where S : Field {
2696 fn outer_product (self, rhs : Self) -> $matrix <S> {
2697 let mut cols : $vector <$vector <S>> = [$vector::zero(); $ndims].into();
2698 for col in 0..$ndims {
2699 for row in 0..$ndims {
2700 cols[col][row] = self[row] * rhs[col];
2701 }
2702 }
2703 $matrix { cols }
2704 }
2705 fn orthogonal (self) -> Self where S : approx::AbsDiffEq <Epsilon = S> {
2706 $ortho_fun (self)
2707 }
2708 }
2709 impl <S : Ring> Dot <S> for $vector <S> {
2710 fn dot (self, other : Self) -> S {
2711 self.dot (other)
2712 }
2713 }
2714 impl <S : Field> VectorSpace <S> for $vector <S> {
2715 type NonZero = $nonzero <S>;
2716 fn map <F> (self, f : F) -> Self where F : FnMut (S) -> S {
2717 self.map (f)
2718 }
2719 }
2720 impl <S> Module <S> for $vector <S> where S : Ring + num::MulAdd {
2721 type LinearEndo = $matrix <S>;
2722 }
2723 impl <S> GroupAction <Addition, $point <S>> for $vector <S>
2724 where S : AdditiveGroup { }
2725 impl <S> MonoidAction <Addition, $point <S>> for $vector <S>
2726 where S : AdditiveGroup { }
2727 impl <S> SemigroupAction <Addition, $point <S>> for $vector <S>
2728 where S : AdditiveGroup
2729 {
2730 fn action (self, point : $point <S>) -> $point <S> {
2731 point + self
2732 }
2733 }
2734 impl <S> std::ops::Mul <NonNegative <S>> for $vector <S> where S : Ring {
2735 type Output = Self;
2736 fn mul (self, rhs : NonNegative <S>) -> Self::Output {
2737 self * *rhs
2738 }
2739 }
2740 impl <S> std::ops::Mul <Positive <S>> for $vector <S> where S : Ring {
2741 type Output = Self;
2742 fn mul (self, rhs : Positive <S>) -> Self::Output {
2743 self * *rhs
2744 }
2745 }
2746 impl <S> std::ops::Mul <NonZero <S>> for $vector <S> where S : Ring {
2747 type Output = Self;
2748 fn mul (self, rhs : NonZero <S>) -> Self::Output {
2749 self * *rhs
2750 }
2751 }
2752 impl <S> std::ops::Mul <Normalized <S>> for $vector <S> where S : Ring {
2753 type Output = Self;
2754 fn mul (self, rhs : Normalized <S>) -> Self::Output {
2755 self * *rhs
2756 }
2757 }
2758 impl <S> std::ops::Mul <NormalSigned <S>> for $vector <S> where S : Ring {
2759 type Output = Self;
2760 fn mul (self, rhs : NormalSigned <S>) -> Self::Output {
2761 self * *rhs
2762 }
2763 }
2764 impl <S> std::ops::Mul <Unit <S>> for $vector <S> where S : Ring {
2765 type Output = Self;
2766 fn mul (self, rhs : Unit <S>) -> Self::Output {
2767 self * *rhs
2768 }
2769 }
2770 impl <S> std::ops::Mul <LinearAuto <S, Self>> for $vector <S> where
2771 S : Ring + MaybeSerDes
2772 {
2773 type Output = Self;
2774 fn mul (self, rhs : LinearAuto <S, Self>) -> Self::Output {
2775 self * rhs.0
2776 }
2777 }
2778 impl <S> std::ops::Mul <LinearIso <S, Self, Self, $matrix <S>>> for $vector <S> where
2779 S : Ring + MaybeSerDes
2780 {
2781 type Output = Self;
2782 fn mul (self, rhs : LinearIso <S, Self, Self, $matrix <S>>) -> Self::Output {
2783 self * rhs.0
2784 }
2785 }
2786 impl <S> num::Inv for LinearAuto <S, $vector <S>> where
2787 S : Ring + num::real::Real + MaybeSerDes
2788 {
2789 type Output = Self;
2790 fn inv (self) -> Self::Output {
2791 LinearAuto (self.0.inv())
2792 }
2793 }
2794 impl <S> std::ops::Div for LinearAuto <S, $vector <S>> where
2795 S : Ring + num::Float + MaybeSerDes
2796 {
2797 type Output = Self;
2798 fn div (self, rhs : Self) -> Self::Output {
2799 LinearAuto (self.0 / rhs.0)
2800 }
2801 }
2802 impl <S> std::ops::DivAssign <Self> for LinearAuto <S, $vector <S>> where
2803 S : Ring + num::Float + MaybeSerDes
2804 {
2805 fn div_assign (&mut self, rhs : Self) {
2806 self.0 /= rhs.0
2807 }
2808 }
2809 impl <S> From <$unit <S>> for $vector <S> where S : Copy {
2810 fn from (unit : $unit <S>) -> Self {
2811 *unit
2812 }
2813 }
2814 #[doc = "Construct a"]
2818 #[doc = $dimension]
2819 #[doc = "matrix"]
2820 pub const fn $matrix_fun <S> ($($component : $vector <S>),+) -> $matrix <S> {
2821 $matrix {
2822 cols: $vector_fun ($($component),+)
2823 }
2824 }
2825 impl <S, V, W> num::Inv for LinearIso <S, V, W, $matrix <S>> where
2826 V : Module <S>,
2827 W : Module <S>,
2828 S : Ring + num::real::Real
2829 {
2830 type Output = LinearIso <S, W, V, $matrix <S>>;
2831 fn inv (self) -> Self::Output {
2832 let mat4 = Matrix4::from (self.0);
2834 LinearIso (mat4.inverted().into(), PhantomData::default())
2835 }
2836 }
2837 impl <S, V> std::ops::Div for LinearIso <S, V, V, $matrix <S>> where
2838 V : Module <S, LinearEndo=$matrix <S>>,
2839 S : Ring + num::real::Real
2840 {
2841 type Output = Self;
2842 #[expect(clippy::suspicious_arithmetic_impl)]
2843 fn div (self, rhs : Self) -> Self::Output {
2844 use num::Inv;
2845 self * rhs.inv()
2846 }
2847 }
2848 impl <S, V> std::ops::DivAssign for LinearIso <S, V, V, $matrix <S>> where
2849 V : Module <S, LinearEndo=$matrix <S>>,
2850 S : Ring + num::real::Real
2851 {
2852 #[expect(clippy::suspicious_op_assign_impl)]
2853 fn div_assign (&mut self, rhs : Self) {
2854 use num::Inv;
2855 *self *= rhs.inv()
2856 }
2857 }
2858 impl <S> std::ops::Mul <NonNegative <S>> for $matrix <S> where S : Ring {
2859 type Output = Self;
2860 fn mul (self, rhs : NonNegative <S>) -> Self::Output {
2861 self * *rhs
2862 }
2863 }
2864 impl <S> std::ops::Mul <Positive <S>> for $matrix <S> where S : Ring {
2865 type Output = Self;
2866 fn mul (self, rhs : Positive <S>) -> Self::Output {
2867 self * *rhs
2868 }
2869 }
2870 impl <S> std::ops::Mul <NonZero <S>> for $matrix <S> where S : Ring {
2871 type Output = Self;
2872 fn mul (self, rhs : NonZero <S>) -> Self::Output {
2873 self * *rhs
2874 }
2875 }
2876 impl <S> std::ops::Mul <Normalized <S>> for $matrix <S> where S : Ring {
2877 type Output = Self;
2878 fn mul (self, rhs : Normalized <S>) -> Self::Output {
2879 self * *rhs
2880 }
2881 }
2882 impl <S> std::ops::Mul <NormalSigned <S>> for $matrix <S> where S : Ring {
2883 type Output = Self;
2884 fn mul (self, rhs : NormalSigned <S>) -> Self::Output {
2885 self * *rhs
2886 }
2887 }
2888 impl <S> std::ops::Mul <Unit <S>> for $matrix <S> where S : Ring {
2889 type Output = Self;
2890 fn mul (self, rhs : Unit <S>) -> Self::Output {
2891 self * *rhs
2892 }
2893 }
2894 $(
2895 impl <S : Copy> Matrix <S> for $matrix <S> {
2896 type Rows = $vector <$vector <S>>;
2897 type Submatrix = $submatrix <S>;
2898 fn rows (self) -> $vector <$vector <S>> {
2899 self.into_row_arrays().map ($vector::from).into()
2900 }
2901 fn submatrix (self, i : usize, j : usize) -> $submatrix <S> {
2902 let a : [S; ($ndims-1) * ($ndims-1)] = std::array::from_fn (|index|{
2903 let i = (i + 1 + index / ($ndims-1)) % $ndims;
2904 let j = (j + 1 + index % ($ndims-1)) % $ndims;
2905 self.cols[i][j]
2906 });
2907 $submatrix::from_col_array (a)
2908 }
2909 fn fill_zeros (submatrix : $submatrix <S>) -> Self where S : num::Zero{
2911 let cols = $vector::from (
2912 std::array::from_fn (|i| if i < $ndims-1 {
2913 $vector::from (submatrix.cols[i])
2914 } else {
2915 $vector::zero()
2916 })
2917 );
2918 $matrix { cols }
2919 }
2920 }
2921 )?
2922 impl <S : Ring> LinearMap <S, $vector <S>, $vector <S>> for $matrix <S> {
2923 fn determinant (self) -> S {
2924 self.determinant()
2925 }
2926 fn transpose (self) -> Self {
2927 self.transposed()
2928 }
2929 }
2930 $(
2934 impl <S> $rotation <S> where S : Ring + MaybeSerDes {
2935 pub fn identity() -> Self {
2937 use num::One;
2938 Self::one()
2939 }
2940 pub fn new_approx (mat : $matrix <S>) -> Option <Self> where
2946 S : approx::RelativeEq <Epsilon=S>
2947 {
2948 let four = S::one() + S::one() + S::one() + S::one();
2949 let thirtytwo = four * four * (S::one() + S::one());
2950 let epsilon = S::default_epsilon() * thirtytwo;
2951 let max_relative = S::default_max_relative() * four;
2952 if approx::relative_ne!(mat * mat.transposed(), $matrix::identity(),
2953 max_relative=max_relative, epsilon=epsilon
2954 ) || approx::relative_ne!(mat.determinant(), S::one(), max_relative=max_relative) {
2955 None
2956 } else {
2957 Some ($rotation (LinearAuto (LinearIso (mat, PhantomData))))
2958 }
2959 }
2960 pub fn noisy (mat : $matrix <S>) -> Self where S : std::fmt::Debug {
2964 assert_eq!(mat * mat.transposed(), $matrix::identity());
2965 assert_eq!(mat.determinant(), S::one());
2966 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2967 }
2968 pub fn noisy_approx (mat : $matrix <S>) -> Self where
2973 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
2974 {
2975 let four = S::one() + S::one() + S::one() + S::one();
2976 let epsilon = S::default_epsilon() * four;
2977 let max_relative = S::default_max_relative() * four;
2978 approx::assert_relative_eq!(mat * mat.transposed(), $matrix::identity(),
2979 epsilon=epsilon, max_relative=max_relative);
2980 approx::assert_relative_eq!(mat.determinant(), S::one(), max_relative=max_relative);
2981 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2982 }
2983 pub fn unchecked (mat : $matrix <S>) -> Self where S : std::fmt::Debug {
2987 debug_assert!(mat * mat.transposed() == $matrix::identity());
2988 debug_assert!(mat.determinant() == S::one());
2989 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
2990 }
2991 pub fn unchecked_approx (mat : $matrix <S>) -> Self where
2996 S : approx::RelativeEq <Epsilon=S> + std::fmt::Debug
2997 {
2998 if cfg!(debug_assertions) {
2999 let four = S::one() + S::one() + S::one() + S::one();
3000 let epsilon = S::default_epsilon() * four;
3001 let max_relative = S::default_max_relative() * four;
3002 approx::assert_relative_eq!(mat * mat.transposed(), $matrix::identity(),
3003 max_relative=max_relative, epsilon=epsilon);
3004 approx::assert_relative_eq!(mat.determinant(), S::one(),
3005 max_relative=max_relative);
3006 }
3007 $rotation (LinearAuto (LinearIso (mat, PhantomData)))
3008 }
3009 pub const fn mat (self) -> $matrix <S> {
3011 self.0.0.0
3012 }
3013 pub fn rotate (self, point : $point <S>) -> $point <S> {
3015 $point::from (self * point.0)
3016 }
3017 pub fn rotate_around (self, point : $point <S>, center : $point <S>)
3019 -> $point <S>
3020 {
3021 self.rotate (point - center.0) + center.0
3022 }
3023 }
3024 impl <S> std::ops::Deref for $rotation <S> where S : Ring + MaybeSerDes {
3025 type Target = LinearAuto <S, $vector <S>>;
3026 fn deref (&self) -> &Self::Target {
3027 &self.0
3028 }
3029 }
3030 impl <S> num::Inv for $rotation <S> where S : Ring + MaybeSerDes {
3031 type Output = Self;
3032 fn inv (self) -> Self::Output {
3033 $rotation (LinearAuto (LinearIso (self.0.0.0.transpose(), PhantomData)))
3034 }
3035 }
3036 impl <S> std::ops::Mul <$vector <S>> for $rotation <S> where
3037 S : Ring + MaybeSerDes
3038 {
3039 type Output = $vector <S>;
3040 fn mul (self, rhs : $vector <S>) -> Self::Output {
3041 self.0 * rhs
3042 }
3043 }
3044 impl <S> std::ops::Mul <$nonzero <S>> for $rotation <S> where
3045 S : Ring + MaybeSerDes
3046 {
3047 type Output = $nonzero <S>;
3048 fn mul (self, rhs : $nonzero <S>) -> Self::Output {
3049 $nonzero (self.0 * rhs.0)
3050 }
3051 }
3052 impl <S> std::ops::Mul <$unit <S>> for $rotation <S> where S : Ring + MaybeSerDes {
3053 type Output = $unit <S>;
3054 fn mul (self, rhs : $unit <S>) -> Self::Output {
3055 $unit (self.0 * rhs.0)
3056 }
3057 }
3058 impl <S> std::ops::Mul <$rotation <S>> for $vector <S> where S : Ring + MaybeSerDes {
3059 type Output = $vector <S>;
3060 fn mul (self, rhs : $rotation <S>) -> Self::Output {
3061 self * rhs.0
3062 }
3063 }
3064 impl <S> std::ops::Mul for $rotation <S> where S : Ring + MaybeSerDes {
3065 type Output = Self;
3066 fn mul (self, rhs : Self) -> Self::Output {
3067 $rotation (self.0 * rhs.0)
3068 }
3069 }
3070 impl <S> std::ops::MulAssign <Self> for $rotation <S> where S : Ring + MaybeSerDes {
3071 fn mul_assign (&mut self, rhs : Self) {
3072 self.0 *= rhs.0
3073 }
3074 }
3075 impl <S> std::ops::Div for $rotation <S> where S : Ring + MaybeSerDes {
3076 type Output = Self;
3077 #[expect(clippy::suspicious_arithmetic_impl)]
3078 fn div (self, rhs : Self) -> Self::Output {
3079 use num::Inv;
3080 self * rhs.inv()
3081 }
3082 }
3083 impl <S> std::ops::DivAssign <Self> for $rotation <S> where S : Ring + MaybeSerDes {
3084 #[expect(clippy::suspicious_op_assign_impl)]
3085 fn div_assign (&mut self, rhs : Self) {
3086 use num::Inv;
3087 *self *= rhs.inv()
3088 }
3089 }
3090 impl <S> num::One for $rotation <S> where S : Ring + MaybeSerDes + num::Zero + num::One {
3091 fn one() -> Self {
3092 $rotation (LinearAuto (LinearIso ($matrix::identity(), PhantomData)))
3093 }
3094 }
3095 )?
3096
3097 impl_numcast!($nonzero);
3101 impl <S> $nonzero <S> {
3102 pub fn new (vector : $vector <S>) -> Option <Self> where S : Ring {
3112 use num::Zero;
3113 if !vector.is_zero() {
3114 Some ($nonzero (vector))
3115 } else {
3116 None
3117 }
3118 }
3119 pub fn noisy (vector : $vector <S>) -> Self where S : Ring {
3128 use num::Zero;
3129 assert!(!vector.is_zero());
3130 $nonzero (vector)
3131 }
3132 #[inline]
3136 pub fn unchecked (vector : $vector <S>) -> Self where S : Ring + std::fmt::Debug {
3137 debug_assert_ne!(vector, $vector::zero());
3138 $nonzero (vector)
3139 }
3140 #[must_use]
3152 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3153 where S : Ring
3154 {
3155 Self::noisy (fun (self.0))
3156 }
3157 #[must_use]
3158 pub fn map_unchecked (self, fun : fn ($vector <S>) -> $vector <S>) -> Self
3159 where S : Ring + std::fmt::Debug
3160 {
3161 Self::unchecked (fun (self.0))
3162 }
3163 }
3164 impl <S> std::ops::Deref for $nonzero <S> {
3165 type Target = $vector <S>;
3166 fn deref (&self) -> &$vector <S> {
3167 &self.0
3168 }
3169 }
3170 impl <S : Ring> std::ops::Neg for $nonzero <S> {
3171 type Output = Self;
3172 fn neg (self) -> Self {
3173 $nonzero (-self.0)
3174 }
3175 }
3176 impl <S> From <$unit <S>> for $nonzero <S> where S : Ring + std::fmt::Debug {
3177 fn from (unit : $unit <S>) -> Self {
3178 $nonzero::unchecked (*unit)
3179 }
3180 }
3181
3182 impl_numcast!($unit);
3186 impl <S> $unit <S> {
3187 $(
3193 #[inline]
3194 pub fn $axis_method() -> Self where S : num::One + num::Zero {
3195 $unit ($vector::$unit_method())
3196 }
3197 )+
3198 pub fn new (vector : $vector <S>) -> Option <Self> where S : OrderedField + Sqrt {
3202 if vector == *vector.normalize() {
3203 Some ($unit (vector))
3204 } else {
3205 None
3206 }
3207 }
3208 pub fn new_approx (vector : $vector <S>) -> Option <Self> where
3215 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3216 {
3217 if vector.is_normalized() {
3218 Some ($unit (vector))
3219 } else {
3220 None
3221 }
3222 }
3223 pub fn normalize (vector : $vector <S>) -> Self where S : OrderedField + Sqrt {
3244 use num::Zero;
3245 assert!(!vector.is_zero());
3246 vector.normalize()
3247 }
3248 pub fn normalize_approx (vector : $vector <S>) -> Self where
3252 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3253 {
3254 use num::Zero;
3255 assert!(!vector.is_zero());
3256 let vector = if vector.is_normalized() {
3257 vector
3258 } else {
3259 vector.normalized()
3260 };
3261 $unit (vector)
3262 }
3263 pub fn noisy (vector : $vector <S>) -> Self where
3272 S : OrderedField + Sqrt + std::fmt::Debug
3273 {
3274 assert_eq!(vector, *vector.normalize());
3275 $unit (vector)
3276 }
3277 pub fn noisy_approx (vector : $vector <S>) -> Self where
3286 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3287 {
3288 assert!(vector.is_normalized());
3289 $unit (vector)
3290 }
3291 #[inline]
3295 pub fn unchecked (vector : $vector <S>) -> Self where
3296 S : OrderedField + Sqrt + std::fmt::Debug
3297 {
3298 debug_assert_eq!(vector, *vector.normalize());
3299 $unit (vector)
3300 }
3301 #[inline]
3311 pub fn unchecked_approx (vector : $vector <S>) -> Self where
3312 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3313 {
3314 debug_assert!(vector.is_normalized());
3315 $unit (vector)
3316 }
3317 pub fn random_unit <R : rand::RngExt> (rng : &mut R) -> Self where
3319 S : OrderedField + Sqrt + rand::distr::uniform::SampleUniform
3320 {
3321 let vector = $vector {
3322 $($component: rng.random_range (-S::one()..S::one())),+
3323 };
3324 vector.normalize()
3325 }
3326 pub fn invert (mut self) -> Self where S : Ring {
3329 self.0 = -self.0;
3330 self
3331 }
3332 #[must_use]
3344 pub fn map_noisy (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3345 S : OrderedField + Sqrt + std::fmt::Debug
3346 {
3347 Self::noisy (fun (self.0))
3348 }
3349 #[must_use]
3361 pub fn map_noisy_approx (self, fun : fn ($vector <S>) -> $vector <S>) -> Self where
3362 S : num::real::Real + approx::RelativeEq <Epsilon=S>
3363 {
3364 Self::noisy_approx (fun (self.0))
3365 }
3366 }
3367 impl <S> std::ops::Deref for $unit <S> {
3368 type Target = $vector <S>;
3369 fn deref (&self) -> &$vector <S> {
3370 &self.0
3371 }
3372 }
3373 impl <S : Ring> std::ops::Neg for $unit <S> {
3374 type Output = Self;
3375 fn neg (self) -> Self {
3376 $unit (-self.0)
3377 }
3378 }
3379 }
3381}
3382
3383macro_rules! impl_numcast {
3384 ($type:ident) => {
3385 impl <S> $type <S> {
3386 #[inline]
3387 pub fn numcast <T> (self) -> Option <$type <T>> where
3388 S : num::NumCast,
3389 T : num::NumCast
3390 {
3391 self.0.numcast().map ($type)
3392 }
3393 }
3394 }
3395}
3396
3397macro_rules! impl_numcast_primitive {
3398 ($type:ident) => {
3399 impl <S> $type <S> {
3400 #[inline]
3401 pub fn numcast <T> (self) -> Option <$type <T>> where
3402 S : num::NumCast,
3403 T : num::NumCast
3404 {
3405 T::from (self.0).map ($type)
3406 }
3407 }
3408 impl <S> num::ToPrimitive for $type <S> where S : num::ToPrimitive {
3409 fn to_isize (&self) -> Option <isize> {
3410 self.0.to_isize()
3411 }
3412 fn to_i8 (&self) -> Option <i8> {
3413 self.0.to_i8()
3414 }
3415 fn to_i16 (&self) -> Option <i16> {
3416 self.0.to_i16()
3417 }
3418 fn to_i32 (&self) -> Option <i32> {
3419 self.0.to_i32()
3420 }
3421 fn to_i64 (&self) -> Option <i64> {
3422 self.0.to_i64()
3423 }
3424 fn to_usize (&self) -> Option <usize> {
3425 self.0.to_usize()
3426 }
3427 fn to_u8 (&self) -> Option <u8> {
3428 self.0.to_u8()
3429 }
3430 fn to_u16 (&self) -> Option <u16> {
3431 self.0.to_u16()
3432 }
3433 fn to_u32 (&self) -> Option <u32> {
3434 self.0.to_u32()
3435 }
3436 fn to_u64 (&self) -> Option <u64> {
3437 self.0.to_u64()
3438 }
3439 fn to_f32 (&self) -> Option <f32> {
3440 self.0.to_f32()
3441 }
3442 fn to_f64 (&self) -> Option <f64> {
3443 self.0.to_f64()
3444 }
3445 }
3446 }
3447}
3448
3449macro_rules! impl_numcast_primitive_option {
3450 ($type:ident$(, $bound:path)*) => {
3451 impl_numcast_primitive!{$type}
3452 impl <S> num::NumCast for $type <S> where
3453 S : num::NumCast $(+ $bound)*
3454 {
3455 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3456 S::from (n).and_then ($type::new)
3457 }
3458 }
3459 }
3460}
3461
3462macro_rules! impl_numcast_primitive_map {
3463 ($type:ident) => {
3464 impl_numcast_primitive!{$type}
3465 impl <S> num::NumCast for $type <S> where S : num::NumCast {
3466 fn from <T : num::ToPrimitive> (n : T) -> Option <$type <S>> {
3467 S::from (n).map ($type)
3468 }
3469 }
3470 }
3471}
3472
3473#[doc(hidden)]
3474#[macro_export]
3475macro_rules! impl_numcast_fields {
3476 ($type:ident, $($field:ident),+) => {
3477 impl <S> $type <S> {
3478 #[inline]
3479 pub fn numcast <T> (self) -> Option <$type <T>> where
3480 S : num::NumCast + PartialOrd + num::Zero,
3481 T : num::NumCast + PartialOrd + num::Zero
3482 {
3483 Some ($type {
3484 $($field: self.$field.numcast()?),+
3485 })
3486 }
3487 }
3488 }
3489}
3490
3491impl_angle!(Deg, "Degrees", Rad::from);
3492impl_angle!(Rad, "Radians", |x : Rad <_>| x.0);
3493impl_angle!(Turn, "Turns", Rad::from);
3494impl_angle_wrapped!(AngleWrapped, "Unsigned wrapped angle restricted to $[0, 2\\pi)$");
3495impl_angle_wrapped!(AngleWrappedSigned,
3496 "Signed wrapped angle restricted to $(-\\pi, \\pi]$");
3497
3498impl_dimension!(Point2, Vector2, NonZero2, Unit2, point2, vector2, matrix2, ortho_vec2,
3499 [x, y], [(axis_x, unit_x), (axis_y, unit_y)],
3500 [], [Point3], Matrix2, [], [Rotation2], 2, "2D");
3501impl_dimension!(Point3, Vector3, NonZero3, Unit3, point3, vector3, matrix3, ortho_vec3,
3502 [x, y, z], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z)],
3503 [Point2], [Point4], Matrix3, [Matrix2], [Rotation3], 3, "3D");
3504impl_dimension!(Point4, Vector4, NonZero4, Unit4, point4, vector4, matrix4, ortho_vec4,
3505 [x, y, z, w], [(axis_x, unit_x), (axis_y, unit_y), (axis_z, unit_z), (axis_w, unit_w)],
3506 [Point3], [], Matrix4, [Matrix3], [], 4, "4D");
3507
3508impl_numcast_primitive_option!(Positive, PartialOrd, num::Zero);
3509impl_numcast_primitive_option!(NonNegative, PartialOrd, num::Zero);
3510impl_numcast_primitive_option!(NonZero, PartialEq, num::Zero);
3511impl_numcast_primitive_option!(Normalized, PartialOrd, num::One, num::Zero);
3512impl_numcast_primitive_option!(NormalSigned, OrderedRing);
3513impl_numcast_fields!(Angles3, yaw, pitch, roll);
3514
3515#[inline]
3519fn normalize_quaternion <S : Real> (quaternion : Quaternion <S>) -> Quaternion <S> {
3520 Quaternion::from_vec4 (*quaternion.into_vec4().normalize())
3521}
3522
3523#[cfg(test)]
3524mod tests {
3525 extern crate test;
3526
3527 use super::*;
3528 use crate::num;
3529 use approx;
3530 use rand;
3531 use rand_distr;
3532 use rand_xorshift;
3533
3534 #[expect(clippy::cast_possible_truncation)]
3535 static ORTHO_VEC_BENCH_SEED : std::sync::LazyLock <u64> = std::sync::LazyLock::new (
3536 || std::time::SystemTime::UNIX_EPOCH.elapsed().unwrap().as_nanos() as u64);
3537
3538 #[test]
3539 fn defaults() {
3540 use num::Zero;
3541
3542 assert_eq!(Positive::<f32>::default().0, 0.0);
3543 assert_eq!(NonNegative::<f32>::default().0, 0.0);
3544 assert_eq!(Normalized::<f32>::default().0, 0.0);
3545 assert_eq!(NormalSigned::<f32>::default().0, 0.0);
3546 assert_eq!(Deg::<f32>::default().0, 0.0);
3547 assert_eq!(Rad::<f32>::default().0, 0.0);
3548 assert_eq!(Turn::<f32>::default().0, 0.0);
3549 assert_eq!(
3550 Angles3::<f32>::default(),
3551 Angles3::new (AngleWrapped::zero(), AngleWrapped::zero(), AngleWrapped::zero()));
3552 assert_eq!(
3553 Pose3::<f32>::default(),
3554 Pose3 { position: Point3::origin(), angles: Angles3::default() });
3555 assert_eq!(
3556 LinearIso::<f32, Vector3 <f32>, Vector3 <f32>, Matrix3 <f32>>::default().0,
3557 Matrix3::identity());
3558 assert_eq!(LinearAuto::<f32, Vector3 <f32>>::default().0.0, Matrix3::identity());
3559 assert_eq!(Rotation2::<f32>::default().0.0.0, Matrix2::identity());
3560 assert_eq!(Rotation3::<f32>::default().0.0.0, Matrix3::identity());
3561 assert_eq!(Versor::<f32>::default().0, Quaternion::from_xyzw (0.0, 0.0, 0.0, 1.0));
3562 assert_eq!(
3563 AffineMap::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3564 AffineMap::new (Matrix3::identity(), Vector3::zero()));
3565 assert_eq!(
3566 Affinity::<f32, Point3 <f32>, Point3 <f32>, Matrix3 <f32>>::default(),
3567 Affinity::new (LinearIso::new (Matrix3::identity()).unwrap(), Vector3::zero()));
3568 assert_eq!(
3569 Projectivity::<f32, Point4 <f32>, Point4 <f32>, Matrix4 <f32>>::default().0,
3570 LinearIso::new (Matrix4::identity()).unwrap());
3571 }
3572
3573 #[test]
3574 fn numcast() {
3575 let p : Positive <f32> = Positive::<f64>::noisy (0.25).numcast().unwrap();
3576 assert_eq!(p, Positive::noisy (0.25f32));
3577 let v : Vector3 <Positive <f64>> = [
3578 Positive::noisy (1.0),
3579 Positive::noisy (2.0),
3580 Positive::noisy (0.25)
3581 ].into();
3582 let _v : Vector3 <Positive <f32>> = v.numcast().unwrap();
3583 }
3584
3585 #[test]
3586 fn trig() {
3587 let a = Turn (0.25);
3588 let b = Deg (90.0);
3589 let c = Rad (std::f32::consts::FRAC_PI_2);
3590 assert_eq!(a.sin(), 1.0);
3591 assert_eq!(b.sin(), 1.0);
3592 assert_eq!(c.sin(), 1.0);
3593 }
3594
3595 #[test]
3596 fn vector_sum() {
3597 let s = [
3598 [0.0, 1.0],
3599 [1.0, 1.0],
3600 [0.5, 0.5]
3601 ].map (Vector2::<f32>::from);
3602 let _sum = s.iter().copied().sum::<Vector2 <f32>>();
3603 }
3604
3605 #[test]
3606 fn vector_outer_product() {
3607 let v1 = vector3 (1.0, 2.0, 3.0);
3608 let v2 = vector3 (2.0, 3.0, 4.0);
3609 assert_eq!(v1.outer_product (v2).into_row_arrays(), [
3610 [2.0, 3.0, 4.0],
3611 [4.0, 6.0, 8.0],
3612 [6.0, 9.0, 12.0]
3613 ]);
3614 }
3615
3616 #[test]
3617 fn rotation3_noisy_approx() {
3618 use rand::{RngExt, SeedableRng};
3619 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3622 let up = Vector3::unit_z();
3623 for _ in 0..1000 {
3624 let forward = vector3 (
3625 rng.random_range (-1000.0f32..1000.0),
3626 rng.random_range (-1000.0f32..1000.0),
3627 0.0
3628 ).normalized();
3629 let right = forward.cross (up);
3630 let mat = Matrix3::from_col_arrays ([
3631 right.into_array(),
3632 forward.into_array(),
3633 up.into_array()
3634 ]);
3635 let _ = Rotation3::noisy_approx (mat);
3636 }
3637 }
3638
3639 #[test]
3640 fn rotation3_intrinsic_angles() {
3641 use std::f32::consts::PI;
3642 use num::Zero;
3643 use rand::{RngExt, SeedableRng};
3644 let rot = Rotation3::from_angles_intrinsic (
3646 Turn (0.25).into(), Rad::zero(), Rad::zero());
3647 approx::assert_relative_eq!(rot.mat(),
3648 Matrix3::from_col_arrays ([
3649 [ 0.0, 1.0, 0.0],
3650 [-1.0, 0.0, 0.0],
3651 [ 0.0, 0.0, 1.0]
3652 ])
3653 );
3654 assert_eq!((Turn (0.25).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3655 let rot = Rotation3::from_angles_intrinsic (
3656 Turn (0.5).into(), Rad::zero(), Rad::zero());
3657 approx::assert_relative_eq!(rot.mat(),
3658 Matrix3::from_col_arrays ([
3659 [-1.0, 0.0, 0.0],
3660 [ 0.0, -1.0, 0.0],
3661 [ 0.0, 0.0, 1.0]
3662 ])
3663 );
3664 assert_eq!((Turn (0.5).into(), Rad::zero(), Rad::zero()), rot.intrinsic_angles());
3665 let rot = Rotation3::from_angles_intrinsic (
3667 Rad::zero(), Turn (0.25).into(), Rad::zero());
3668 approx::assert_relative_eq!(rot.mat(),
3669 Matrix3::from_col_arrays ([
3670 [1.0, 0.0, 0.0],
3671 [0.0, 0.0, 1.0],
3672 [0.0, -1.0, 0.0]
3673 ])
3674 );
3675 assert_eq!((Rad::zero(), Turn (0.25).into(), Rad::zero()), rot.intrinsic_angles());
3676 let rot = Rotation3::from_angles_intrinsic (
3678 Rad::zero(), Rad::zero(), Turn (0.25).into());
3679 approx::assert_relative_eq!(rot.mat(),
3680 Matrix3::from_col_arrays ([
3681 [0.0, 0.0, -1.0],
3682 [0.0, 1.0, 0.0],
3683 [1.0, 0.0, 0.0]
3684 ])
3685 );
3686 assert_eq!((Rad::zero(), Rad::zero(), Turn (0.25).into()), rot.intrinsic_angles());
3687 let rot = Rotation3::from_angles_intrinsic (
3689 Turn (0.25).into(), Turn (0.25).into(), Rad::zero());
3690 approx::assert_relative_eq!(rot.mat(),
3691 Matrix3::from_col_arrays ([
3692 [0.0, 1.0, 0.0],
3693 [0.0, 0.0, 1.0],
3694 [1.0, 0.0, 0.0]
3695 ])
3696 );
3697 let angles = rot.intrinsic_angles();
3698 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3699 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3700 approx::assert_relative_eq!(0.0, angles.2.0);
3701 let rot = Rotation3::from_angles_intrinsic (
3703 Turn (0.25).into(), Turn (0.25).into(), Turn (0.25).into());
3704 approx::assert_relative_eq!(rot.mat(),
3705 Matrix3::from_col_arrays ([
3706 [-1.0, 0.0, 0.0],
3707 [ 0.0, 0.0, 1.0],
3708 [ 0.0, 1.0, 0.0]
3709 ])
3710 );
3711 let angles = rot.intrinsic_angles();
3712 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.0.0);
3713 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.1.0);
3714 approx::assert_relative_eq!(Rad::from (Turn (0.25)).0, angles.2.0);
3715
3716 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (0);
3719 let mut random_vector = || vector3 (
3720 rng.random_range (-100.0f32..100.0),
3721 rng.random_range (-100.0f32..100.0),
3722 rng.random_range (-100.0f32..100.0));
3723 for _ in 0..1000 {
3724 let rot = Rotation3::orthonormalize (
3725 random_vector(), random_vector(), random_vector()
3726 ).unwrap();
3727 let (yaw, pitch, roll) = rot.intrinsic_angles();
3728 assert!(yaw.0 < PI);
3729 assert!(yaw.0 > -PI);
3730 assert!(pitch.0 < PI);
3731 assert!(pitch.0 > -PI);
3732 assert!(roll.0 < PI);
3733 assert!(roll.0 > -PI);
3734 }
3735 }
3736
3737 #[test]
3738 fn rotation3_into_versor() {
3739 use std::f32::consts::PI;
3740 let rot = Rotation3::from_angle_x (Rad (0.01));
3741 let quat = rot.versor().0;
3742 approx::assert_relative_eq!(rot.mat(), quat.into());
3743 let rot = Rotation3::from_angle_x (Rad (-0.01));
3744 let quat = rot.versor().0;
3745 approx::assert_relative_eq!(rot.mat(), quat.into());
3746
3747 let rot = Rotation3::from_angle_y (Rad (0.01));
3748 let quat = rot.versor().0;
3749 approx::assert_relative_eq!(rot.mat(), quat.into());
3750 let rot = Rotation3::from_angle_y (Rad (-0.01));
3751 let quat = rot.versor().0;
3752 approx::assert_relative_eq!(rot.mat(), quat.into());
3753
3754 let rot = Rotation3::from_angle_z (Rad (0.01));
3755 let quat = rot.versor().0;
3756 approx::assert_relative_eq!(rot.mat(), quat.into());
3757 let rot = Rotation3::from_angle_z (Rad (-0.01));
3758 let quat = rot.versor().0;
3759 approx::assert_relative_eq!(rot.mat(), quat.into());
3760
3761 let rot = Rotation3::from_angle_x (Rad (PI / 2.0));
3762 let quat = rot.versor().0;
3763 approx::assert_relative_eq!(rot.mat(), quat.into());
3764 let rot = Rotation3::from_angle_x (Rad (-PI / 2.0));
3765 let quat = rot.versor().0;
3766 approx::assert_relative_eq!(rot.mat(), quat.into());
3767
3768 let rot = Rotation3::from_angle_y (Rad (PI / 2.0));
3769 let quat = rot.versor().0;
3770 approx::assert_relative_eq!(rot.mat(), quat.into());
3771 let rot = Rotation3::from_angle_y (Rad (-PI / 2.0));
3772 let quat = rot.versor().0;
3773 approx::assert_relative_eq!(rot.mat(), quat.into());
3774
3775 let rot = Rotation3::from_angle_z (Rad (PI / 2.0));
3776 let quat = rot.versor().0;
3777 approx::assert_relative_eq!(rot.mat(), quat.into());
3778 let rot = Rotation3::from_angle_z (Rad (-PI / 2.0));
3779 let quat = rot.versor().0;
3780 approx::assert_relative_eq!(rot.mat(), quat.into());
3781 }
3782
3783 #[bench]
3787 fn bench_orthogonal_cross_product (b : &mut test::Bencher) {
3788 use approx::AbsDiffEq;
3789 use rand::{RngExt, SeedableRng};
3790 use rand_distr::Distribution;
3791 let epsilon = f64::default_epsilon() * 256.0;
3792 let std_normal = rand_distr::StandardNormal;
3793 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3794 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3795 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3796 let x : f64 = std_normal.sample (rng);
3797 let y : f64 = std_normal.sample (rng);
3798 x / y
3799 };
3800 b.iter(||{
3801 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3802 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3803 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3804 6 => vector3 (0.0, randf (&mut rng), 0.0),
3805 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3806 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3807 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3808 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3809 _ => unreachable!()
3810 };
3811 approx::assert_abs_diff_ne!(vec, Vector3::zero(), epsilon = epsilon);
3812 let mut ortho = vec.cross (Vector3::unit_x());
3813 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) {
3814 ortho = vec.cross (Vector3::unit_y())
3815 }
3816 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3817 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3818 {
3819 println!("VEC: {vec}");
3820 println!("ORTHO: {ortho}");
3821 println!("DOT: {}", vec.dot (ortho));
3822 }
3823 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3824 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3825 });
3826 }
3827
3828 #[bench]
3832 fn bench_orthogonal_copysign (b : &mut test::Bencher) {
3833 use approx::AbsDiffEq;
3834 use rand::{RngExt, SeedableRng};
3835 use rand_distr::Distribution;
3836 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3837 let std_normal = rand_distr::StandardNormal;
3838 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3839 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3840 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3841 let x : f64 = std_normal.sample (rng);
3842 let y : f64 = std_normal.sample (rng);
3843 x / y
3844 };
3845 b.iter(||{
3846 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3847 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3848 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3849 6 => vector3 (0.0, randf (&mut rng), 0.0),
3850 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3851 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3852 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3853 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3854 _ => unreachable!()
3855 };
3856 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3857 return
3858 }
3859 let ortho = vector3 (
3860 vec.z.copysign (vec.x),
3861 vec.z.copysign (vec.y),
3862 -vec.x.copysign (vec.z) - vec.y.copysign (vec.z)
3864 );
3866 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3867 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3868 {
3869 println!("VEC: {vec}");
3870 println!("ORTHO: {ortho}");
3871 println!("DOT: {:.}", vec.dot (ortho));
3872 }
3873 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3874 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3875 });
3876 }
3877
3878 #[bench]
3882 fn bench_orthogonal_sign (b : &mut test::Bencher) {
3883 use approx::AbsDiffEq;
3884 use rand::{RngExt, SeedableRng};
3885 use rand_distr::Distribution;
3886 let epsilon = f64::default_epsilon() * 2.0.powi (32);
3887 let std_normal = rand_distr::StandardNormal;
3888 println!("ORTHO_VEC_BENCH_SEED: {}", *ORTHO_VEC_BENCH_SEED);
3889 let mut rng = rand_xorshift::XorShiftRng::seed_from_u64 (*ORTHO_VEC_BENCH_SEED);
3890 let randf = |rng : &mut rand_xorshift::XorShiftRng| {
3891 let x : f64 = std_normal.sample (rng);
3892 let y : f64 = std_normal.sample (rng);
3893 x / y
3894 };
3895 b.iter(||{
3896 let vec : Vector3 <f64> = match rng.random_range (0..=10) {
3897 0..=4 => vector3 (randf (&mut rng), randf (&mut rng), randf (&mut rng)),
3898 5 => vector3 (randf (&mut rng), 0.0, 0.0),
3899 6 => vector3 (0.0, randf (&mut rng), 0.0),
3900 7 => vector3 (0.0, 0.0, randf (&mut rng)),
3901 8 => vector3 (0.0, randf (&mut rng), randf (&mut rng)),
3902 9 => vector3 (randf (&mut rng), 0.0, randf (&mut rng)),
3903 10 => vector3 (randf (&mut rng), randf (&mut rng), 0.0),
3904 _ => unreachable!()
3905 };
3906 if approx::abs_diff_eq!(vec, Vector3::zero(), epsilon = epsilon) {
3907 return
3908 }
3909 let sign = |n : f64| if n == 0.0 {
3910 0.0
3911 } else if n > 0.0 {
3912 1.0
3913 } else {
3914 -1.0
3915 };
3916 let signfn = |n : f64| sign ((sign (n) + 0.5) * (sign (vec.z) + 0.5));
3917 let ortho = vector3 (
3918 signfn (vec.x) * vec.z,
3919 signfn (vec.y) * vec.z,
3920 (-signfn (vec.x)).mul_add (vec.x, -signfn (vec.y) * vec.y)
3921 );
3923 if approx::abs_diff_eq!(ortho, Vector3::zero(), epsilon = epsilon) ||
3924 approx::abs_diff_ne!(vec.dot (ortho), 0.0, epsilon = epsilon)
3925 {
3926 println!("VEC: {vec}");
3927 println!("ORTHO: {ortho}");
3928 println!("DOT: {:.}", vec.dot (ortho));
3929 }
3930 approx::assert_abs_diff_ne!(ortho, Vector3::zero(), epsilon = epsilon);
3931 approx::assert_abs_diff_eq!(vec.dot (ortho), 0.0, epsilon = epsilon);
3932 });
3933 }
3934}