peridot_math/
linarg.rs

1//! PeridotExtendedMathematics: Vector/Matrix
2
3use crate::numtraits::{One, Zero};
4use crate::Real;
5use std::mem::transmute;
6use std::ops::*;
7
8/// 2-dimensional vector
9#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
10#[repr(C)]
11pub struct Vector2<T>(pub T, pub T);
12/// 3-dimensional vector
13#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
14#[repr(C)]
15pub struct Vector3<T>(pub T, pub T, pub T);
16/// 4-dimensional vector
17#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
18#[repr(C)]
19pub struct Vector4<T>(pub T, pub T, pub T, pub T);
20/// Arbitrary rotating
21#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
22#[repr(C)]
23pub struct Quaternion<T>(pub T, pub T, pub T, pub T);
24
25/// Type alias for vec2 of f32
26pub type Vector2F32 = Vector2<f32>;
27/// Type alias for vec3 of f32
28pub type Vector3F32 = Vector3<f32>;
29/// Type alias for vec4 of f32
30pub type Vector4F32 = Vector4<f32>;
31/// Type alias for qvec of f32
32pub type QuaternionF32 = Quaternion<f32>;
33
34/// 2x2 matrix
35#[derive(Debug, Clone, PartialEq, Eq, Hash)]
36#[repr(C)]
37pub struct Matrix2<T>(pub [T; 2], pub [T; 2]);
38/// 3x3 matrix
39#[derive(Debug, Clone, PartialEq, Eq, Hash)]
40#[repr(C)]
41pub struct Matrix3<T>(pub [T; 3], pub [T; 3], pub [T; 3]);
42/// 4x4 matrix
43#[derive(Debug, Clone, PartialEq, Eq, Hash)]
44#[repr(C)]
45pub struct Matrix4<T>(pub [T; 4], pub [T; 4], pub [T; 4], pub [T; 4]);
46/// 2x3 matrix
47#[derive(Debug, Clone, PartialEq, Eq, Hash)]
48#[repr(C)]
49pub struct Matrix2x3<T>(pub [T; 3], pub [T; 3]);
50/// 3x4 matrix
51#[derive(Debug, Clone, PartialEq, Eq, Hash)]
52#[repr(C)]
53pub struct Matrix3x4<T>(pub [T; 4], pub [T; 4], pub [T; 4]);
54
55/// Type alias for mat2 of f32
56pub type Matrix2F32 = Matrix2<f32>;
57/// Type alias for mat3 of f32
58pub type Matrix3F32 = Matrix3<f32>;
59/// Type alias for mat4 of f32
60pub type Matrix4F32 = Matrix4<f32>;
61/// Type alias for mat23 of f32
62pub type Matrix2x3F32 = Matrix2x3<f32>;
63/// Type alias for mat34 of f32
64pub type Matrix3x4F32 = Matrix3x4<f32>;
65
66impl<T> Vector2<T> {
67    pub const fn x(&self) -> &T {
68        &self.0
69    }
70
71    pub const fn y(&self) -> &T {
72        &self.1
73    }
74}
75impl<T> Vector3<T> {
76    pub const fn x(&self) -> &T {
77        &self.0
78    }
79
80    pub const fn y(&self) -> &T {
81        &self.1
82    }
83
84    pub const fn z(&self) -> &T {
85        &self.2
86    }
87}
88impl<T> Vector4<T> {
89    pub const fn x(&self) -> &T {
90        &self.0
91    }
92
93    pub const fn y(&self) -> &T {
94        &self.1
95    }
96
97    pub const fn z(&self) -> &T {
98        &self.2
99    }
100
101    pub const fn w(&self) -> &T {
102        &self.3
103    }
104}
105
106// Identities of Vectors //
107impl<T: Zero> Zero for Vector2<T> {
108    const ZERO: Self = Vector2(T::ZERO, T::ZERO);
109}
110impl<T: One> One for Vector2<T> {
111    const ONE: Self = Vector2(T::ONE, T::ONE);
112}
113impl<T: Zero> Zero for Vector3<T> {
114    const ZERO: Self = Vector3(T::ZERO, T::ZERO, T::ZERO);
115}
116impl<T: One> One for Vector3<T> {
117    const ONE: Self = Vector3(T::ONE, T::ONE, T::ONE);
118}
119impl<T: Zero> Zero for Vector4<T> {
120    const ZERO: Self = Vector4(T::ZERO, T::ZERO, T::ZERO, T::ZERO);
121}
122impl<T: One> One for Vector4<T> {
123    const ONE: Self = Vector4(T::ONE, T::ONE, T::ONE, T::ONE);
124}
125
126// Per-Element Identities of Vectors //
127impl<T: Zero + One> Vector2<T> {
128    pub fn left() -> Self
129    where
130        Self: Neg<Output = Self>,
131    {
132        -Self::right()
133    }
134
135    pub fn down() -> Self
136    where
137        Self: Neg<Output = Self>,
138    {
139        -Self::up()
140    }
141
142    pub const fn right() -> Self {
143        Self(T::ONE, T::ZERO)
144    }
145
146    pub const fn up() -> Self {
147        Self(T::ZERO, T::ONE)
148    }
149}
150
151impl<T: Zero + One> Vector3<T> {
152    pub fn left() -> Self
153    where
154        Self: Neg<Output = Self>,
155    {
156        -Self::right()
157    }
158
159    pub const fn right() -> Self {
160        Self(T::ONE, T::ZERO, T::ZERO)
161    }
162
163    pub fn down() -> Self
164    where
165        Self: Neg<Output = Self>,
166    {
167        -Self::up()
168    }
169
170    pub const fn up() -> Self {
171        Self(T::ZERO, T::ONE, T::ZERO)
172    }
173
174    pub fn back() -> Self
175    where
176        Self: Neg<Output = Self>,
177    {
178        -Self::forward()
179    }
180
181    pub const fn forward() -> Self {
182        Self(T::ZERO, T::ZERO, T::ONE)
183    }
184}
185
186// Extending/Shrinking Vector Types //
187/// Vector4(x, y, z, w) -> Vector3(x / w, y / w, z / w)
188/// panic occured when w == 0
189impl<T: Div<T> + Copy> From<Vector4<T>> for Vector3<<T as Div>::Output> {
190    fn from(Vector4(x, y, z, w): Vector4<T>) -> Self {
191        Vector3(x / w, y / w, z / w)
192    }
193}
194
195impl<T> Vector2<T> {
196    pub fn with_z(self, z: T) -> Vector3<T> {
197        Vector3(self.0, self.1, z)
198    }
199}
200impl<T> Vector3<T> {
201    pub fn with_w(self, w: T) -> Vector4<T> {
202        Vector4(self.0, self.1, self.2, w)
203    }
204}
205
206// Safe Transmuting as Slice //
207impl<T> AsRef<[T; 2]> for Vector2<T> {
208    fn as_ref(&self) -> &[T; 2] {
209        unsafe { transmute(self) }
210    }
211}
212impl<T> AsRef<[T; 3]> for Vector3<T> {
213    fn as_ref(&self) -> &[T; 3] {
214        unsafe { transmute(self) }
215    }
216}
217impl<T> AsRef<[T; 4]> for Vector4<T> {
218    fn as_ref(&self) -> &[T; 4] {
219        unsafe { transmute(self) }
220    }
221}
222impl<T> AsRef<[T; 4]> for Quaternion<T> {
223    fn as_ref(&self) -> &[T; 4] {
224        unsafe { transmute(self) }
225    }
226}
227
228// Identities(for Multiplication) //
229impl<T: Zero + One> One for Matrix2<T> {
230    const ONE: Self = Matrix2([T::ONE, T::ZERO], [T::ZERO, T::ONE]);
231}
232impl<T: Zero + One> One for Matrix3<T> {
233    const ONE: Self = Matrix3(
234        [T::ONE, T::ZERO, T::ZERO],
235        [T::ZERO, T::ONE, T::ZERO],
236        [T::ZERO, T::ZERO, T::ONE],
237    );
238}
239impl<T: Zero + One> One for Matrix4<T> {
240    const ONE: Self = Matrix4(
241        [T::ONE, T::ZERO, T::ZERO, T::ZERO],
242        [T::ZERO, T::ONE, T::ZERO, T::ZERO],
243        [T::ZERO, T::ZERO, T::ONE, T::ZERO],
244        [T::ZERO, T::ZERO, T::ZERO, T::ONE],
245    );
246}
247impl<T: Zero + One> One for Matrix2x3<T> {
248    const ONE: Self = Matrix2x3([T::ONE, T::ZERO, T::ZERO], [T::ZERO, T::ONE, T::ZERO]);
249}
250impl<T: Zero + One> One for Matrix3x4<T> {
251    const ONE: Self = Matrix3x4(
252        [T::ONE, T::ZERO, T::ZERO, T::ZERO],
253        [T::ZERO, T::ONE, T::ZERO, T::ZERO],
254        [T::ZERO, T::ZERO, T::ONE, T::ZERO],
255    );
256}
257
258// Extending Matrix Dimensions //
259impl<T: Zero + One> From<Matrix2<T>> for Matrix3<T> {
260    fn from(Matrix2([a, b], [c, d]): Matrix2<T>) -> Self {
261        Matrix3([a, b, T::ZERO], [c, d, T::ZERO], [T::ZERO, T::ZERO, T::ONE])
262    }
263}
264impl<T: Zero + One> From<Matrix3<T>> for Matrix4<T> {
265    fn from(Matrix3([a, b, c], [d, e, f], [g, h, i]): Matrix3<T>) -> Self {
266        Matrix4(
267            [a, b, c, T::ZERO],
268            [d, e, f, T::ZERO],
269            [g, h, i, T::ZERO],
270            [T::ZERO, T::ZERO, T::ZERO, T::ONE],
271        )
272    }
273}
274
275// Matrix and Matrix Multiplication //
276impl<T: Copy> Mul for Matrix2<T>
277where
278    T: Mul<T>,
279    <T as Mul>::Output: Add,
280{
281    type Output = Matrix2<<<T as Mul>::Output as Add>::Output>;
282    fn mul(self, other: Matrix2<T>) -> Self::Output {
283        let dp =
284            |src: &[T; 2], colindex: usize| src[0] * other.0[colindex] + src[1] * other.1[colindex];
285
286        Matrix2(
287            [dp(&self.0, 0), dp(&self.0, 1)],
288            [dp(&self.1, 0), dp(&self.1, 1)],
289        )
290    }
291}
292impl<T: Copy> Mul for Matrix3<T>
293where
294    T: Mul<T>,
295    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
296{
297    type Output = Matrix3<<T as Mul>::Output>;
298
299    fn mul(self, other: Matrix3<T>) -> Self::Output {
300        let dp = |src: &[T; 3], colindex: usize| {
301            src[0] * other.0[colindex] + src[1] * other.1[colindex] + src[2] * other.2[colindex]
302        };
303
304        Matrix3(
305            [dp(&self.0, 0), dp(&self.0, 1), dp(&self.0, 2)],
306            [dp(&self.1, 0), dp(&self.1, 1), dp(&self.1, 2)],
307            [dp(&self.2, 0), dp(&self.2, 1), dp(&self.2, 2)],
308        )
309    }
310}
311impl<T: Copy> Mul for Matrix4<T>
312where
313    T: Mul<T>,
314    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
315{
316    type Output = Matrix4<<T as Mul>::Output>;
317
318    fn mul(self, other: Matrix4<T>) -> Self::Output {
319        let dp = |src: &[T; 4], colindex: usize| {
320            src[0] * other.0[colindex]
321                + src[1] * other.1[colindex]
322                + src[2] * other.2[colindex]
323                + src[3] * other.3[colindex]
324        };
325
326        Matrix4(
327            [
328                dp(&self.0, 0),
329                dp(&self.0, 1),
330                dp(&self.0, 2),
331                dp(&self.0, 3),
332            ],
333            [
334                dp(&self.1, 0),
335                dp(&self.1, 1),
336                dp(&self.1, 2),
337                dp(&self.1, 3),
338            ],
339            [
340                dp(&self.2, 0),
341                dp(&self.2, 1),
342                dp(&self.2, 2),
343                dp(&self.2, 3),
344            ],
345            [
346                dp(&self.3, 0),
347                dp(&self.3, 1),
348                dp(&self.3, 2),
349                dp(&self.3, 3),
350            ],
351        )
352    }
353}
354
355// Scaling, Rotating //
356impl<T> Matrix2<T> {
357    pub fn scale(Vector2(x, y): Vector2<T>) -> Self
358    where
359        T: Zero,
360    {
361        Matrix2([x, T::ZERO], [T::ZERO, y])
362    }
363
364    pub fn rotate(rad: T) -> Self
365    where
366        T: Real + Copy + Neg<Output = T>,
367    {
368        let (s, c) = rad.sin_cos();
369        Matrix2([c, -s], [s, c])
370    }
371}
372
373// Scaling/Translating //
374impl<T> Matrix3<T> {
375    #[inline(always)]
376    pub fn scale(Vector3(x, y, z): Vector3<T>) -> Self
377    where
378        T: Zero,
379    {
380        Matrix3(
381            [x, T::ZERO, T::ZERO],
382            [T::ZERO, y, T::ZERO],
383            [T::ZERO, T::ZERO, z],
384        )
385    }
386
387    #[inline(always)]
388    pub fn translation(Vector2(x, y): Vector2<T>) -> Self
389    where
390        T: One + Zero,
391    {
392        Matrix3(
393            [T::ONE, T::ZERO, x],
394            [T::ZERO, T::ONE, y],
395            [T::ZERO, T::ZERO, T::ONE],
396        )
397    }
398}
399impl<T> Matrix4<T> {
400    #[inline(always)]
401    pub fn scale(Vector4(x, y, z, w): Vector4<T>) -> Self
402    where
403        T: Zero,
404    {
405        Matrix4(
406            [x, T::ZERO, T::ZERO, T::ZERO],
407            [T::ZERO, y, T::ZERO, T::ZERO],
408            [T::ZERO, T::ZERO, z, T::ZERO],
409            [T::ZERO, T::ZERO, T::ZERO, w],
410        )
411    }
412
413    #[inline(always)]
414    pub fn translation(Vector3(x, y, z): Vector3<T>) -> Self
415    where
416        T: One + Zero,
417    {
418        Matrix4(
419            [T::ONE, T::ZERO, T::ZERO, x],
420            [T::ZERO, T::ONE, T::ZERO, y],
421            [T::ZERO, T::ZERO, T::ONE, z],
422            [T::ZERO, T::ZERO, T::ZERO, T::ONE],
423        )
424    }
425}
426
427// TRS(Translate-Rotate-Scale) constructor //
428impl<T> Matrix4<T> {
429    pub fn trs(translate: Vector3<T>, rotate: Quaternion<T>, scale: Vector3<T>) -> Self
430    where
431        T: One + Zero + Mul<T, Output = T> + Sub<T, Output = T> + Add<T, Output = T> + Copy,
432    {
433        Matrix4::translation(translate)
434            * Matrix4::from(rotate)
435            * Matrix4::scale(scale.with_w(T::ONE))
436    }
437}
438
439fn dotproduct2<T: Mul + Copy>(a: &[T; 2], b: &[T; 2]) -> <T as Mul>::Output
440where
441    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
442{
443    a[0] * b[0] + a[1] * b[1]
444}
445fn dotproduct3<T: Mul + Copy>(a: &[T; 3], b: &[T; 3]) -> <T as Mul>::Output
446where
447    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
448{
449    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
450}
451fn dotproduct4<T: Mul + Copy>(a: &[T; 4], b: &[T; 4]) -> <T as Mul>::Output
452where
453    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
454{
455    a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]
456}
457
458// Point Translation by Multiplication //
459impl<T: Mul + Copy> Mul<Vector2<T>> for Matrix2<T>
460where
461    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
462{
463    type Output = Vector2<<T as Mul>::Output>;
464    fn mul(self, v: Vector2<T>) -> Self::Output {
465        Vector2(
466            dotproduct2(v.as_ref(), &self.0),
467            dotproduct2(v.as_ref(), &self.1),
468        )
469    }
470}
471impl<T: Mul + Copy> Mul<Vector3<T>> for Matrix3<T>
472where
473    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
474{
475    type Output = Vector3<<T as Mul>::Output>;
476    fn mul(self, v: Vector3<T>) -> Self::Output {
477        Vector3(
478            dotproduct3(v.as_ref(), &self.0),
479            dotproduct3(v.as_ref(), &self.1),
480            dotproduct3(v.as_ref(), &self.2),
481        )
482    }
483}
484impl<T: Mul + Copy> Mul<Vector4<T>> for Matrix4<T>
485where
486    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
487{
488    type Output = Vector4<<T as Mul>::Output>;
489    fn mul(self, v: Vector4<T>) -> Self::Output {
490        Vector4(
491            dotproduct4(v.as_ref(), &self.0),
492            dotproduct4(v.as_ref(), &self.1),
493            dotproduct4(v.as_ref(), &self.2),
494            dotproduct4(v.as_ref(), &self.3),
495        )
496    }
497}
498impl<T: Mul + One + Copy> Mul<Vector2<T>> for Matrix2x3<T>
499where
500    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
501{
502    type Output = Vector2<<T as Mul>::Output>;
503    fn mul(self, v: Vector2<T>) -> Self::Output {
504        let v3 = v.with_z(T::ONE);
505        Vector2(
506            dotproduct3(v3.as_ref(), &self.0),
507            dotproduct3(v3.as_ref(), &self.1),
508        )
509    }
510}
511impl<T: Mul + One + Copy> Mul<Vector3<T>> for Matrix3x4<T>
512where
513    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
514{
515    type Output = Vector3<<T as Mul>::Output>;
516    fn mul(self, v: Vector3<T>) -> Self::Output {
517        let v4 = v.with_w(T::ONE);
518        Vector3(
519            dotproduct4(v4.as_ref(), &self.0),
520            dotproduct4(v4.as_ref(), &self.1),
521            dotproduct4(v4.as_ref(), &self.2),
522        )
523    }
524}
525// shortcuts //
526impl<T: Mul + One + Copy> Mul<Vector2<T>> for Matrix3<T>
527where
528    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
529{
530    type Output = <Matrix3<T> as Mul<Vector3<T>>>::Output;
531    fn mul(self, v: Vector2<T>) -> Self::Output {
532        self * v.with_z(T::ONE)
533    }
534}
535impl<T: Mul + One + Copy> Mul<Vector2<T>> for Matrix4<T>
536where
537    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
538{
539    type Output = <Matrix4<T> as Mul<Vector4<T>>>::Output;
540    fn mul(self, v: Vector2<T>) -> Self::Output {
541        self * v.with_z(T::ONE)
542    }
543}
544impl<T: Mul + One + Copy> Mul<Vector3<T>> for Matrix4<T>
545where
546    <T as Mul>::Output: Add<Output = <T as Mul>::Output>,
547{
548    type Output = <Matrix4<T> as Mul<Vector4<T>>>::Output;
549    fn mul(self, v: Vector3<T>) -> Self::Output {
550        self * v.with_w(T::ONE)
551    }
552}
553
554// Length Function and Normalization //
555impl<T> Vector2<T>
556where
557    T: Real + Copy + Mul<T, Output = T> + Add<T, Output = T>,
558{
559    #[inline]
560    pub fn len(&self) -> T {
561        self.len2().sqrt()
562    }
563
564    #[inline]
565    pub fn normalize(&self) -> Self
566    where
567        T: Div<T, Output = T>,
568    {
569        let l0 = self.len();
570        Vector2(self.0 / l0, self.1 / l0)
571    }
572}
573impl<T> Vector3<T>
574where
575    T: Real + Copy + Mul<T, Output = T> + Add<T, Output = T>,
576{
577    #[inline]
578    pub fn len(&self) -> T {
579        self.len2().sqrt()
580    }
581
582    #[inline]
583    pub fn normalize(&self) -> Self
584    where
585        T: Div<T, Output = T>,
586    {
587        let l0 = self.len();
588        Vector3(self.0 / l0, self.1 / l0, self.2 / l0)
589    }
590}
591impl<T> Vector4<T>
592where
593    T: Real + Copy + Mul<T, Output = T> + Add<T, Output = T>,
594{
595    #[inline]
596    pub fn len(&self) -> T {
597        self.len2().sqrt()
598    }
599
600    #[inline]
601    pub fn normalize(&self) -> Self
602    where
603        T: Div<T, Output = T>,
604    {
605        let l0 = self.len();
606        Vector4(self.0 / l0, self.1 / l0, self.2 / l0, self.3 / l0)
607    }
608}
609
610// Aspect Rate
611impl<T> Vector2<T>
612where
613    T: Copy + Div<T, Output = T> + PartialEq + Zero,
614{
615    #[inline(always)]
616    pub fn aspect_wh(&self) -> T {
617        if self.1 == T::ZERO {
618            T::ZERO
619        } else {
620            self.0 / self.1
621        }
622    }
623
624    #[inline(always)]
625    pub fn aspect_hw(&self) -> T {
626        if self.0 == T::ZERO {
627            T::ZERO
628        } else {
629            self.1 / self.0
630        }
631    }
632}
633
634/// identity
635impl<T: One + Zero> One for Quaternion<T> {
636    const ONE: Self = Quaternion(T::ZERO, T::ZERO, T::ZERO, T::ONE);
637}
638
639impl<T> Quaternion<T> {
640    /// Creates new quaternion from rotation axis and angle in radian.
641    pub fn new(rad: T, axis: Vector3<T>) -> Self
642    where
643        T: Div<T, Output = T> + Real + One + Copy + Add<T, Output = T> + Mul<T, Output = T>,
644    {
645        let (s, c) = (rad / (T::ONE + T::ONE)).sin_cos();
646        let axis = axis.normalize();
647
648        Self(axis.0 * s, axis.1 * s, axis.2 * s, c)
649    }
650
651    /// Calculates the lerp-ed quaternion between 2 quaternions by `t`.
652    pub fn lerp(&self, other: &Self, t: T) -> Self
653    where
654        T: Real
655            + Copy
656            + Mul<T, Output = T>
657            + Add<T, Output = T>
658            + Div<T, Output = T>
659            + Sub<T, Output = T>
660            + One,
661    {
662        let omg = self.angle(other);
663        let (fa, fb) = (
664            (omg * (T::ONE - t)).sin() / omg.sin(),
665            (omg * t).sin() / omg.sin(),
666        );
667
668        Self(
669            fa * self.0 + fb * other.0,
670            fa * self.1 + fb * other.1,
671            fa * self.2 + fb * other.2,
672            fa * self.3 + fb * other.3,
673        )
674    }
675
676    /// Calculates a difference of angle between 2 quaternions
677    #[inline(always)]
678    pub fn angle(&self, other: &Self) -> T
679    where
680        T: Real + Copy + Mul<T, Output = T> + Add<T, Output = T>,
681    {
682        dotproduct4(self.as_ref(), other.as_ref()).acos()
683    }
684
685    /// Calculates a normalized quaternion
686    #[inline(always)]
687    pub fn normalize(self) -> Self
688    where
689        T: Copy + Div<T, Output = T> + Mul<T, Output = T> + Add<T, Output = T> + Real,
690    {
691        let d = (self.0 * self.0 + self.1 * self.1 + self.2 * self.2 + self.3 * self.3).sqrt();
692
693        Self(self.0 / d, self.1 / d, self.2 / d, self.3 / d)
694    }
695}
696
697impl<T> Mul for Quaternion<T>
698where
699    T: Mul<T, Output = T> + Add<T, Output = T> + Sub<T, Output = T> + Copy,
700{
701    type Output = Quaternion<T>;
702
703    fn mul(self, Quaternion(x2, y2, z2, w2): Quaternion<T>) -> Self::Output {
704        let Quaternion(x1, y1, z1, w1) = self;
705
706        let x0 = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2;
707        let y0 = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2;
708        let z0 = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2;
709        let w0 = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2;
710
711        Quaternion(x0, y0, z0, w0)
712    }
713}
714
715// q * neg q = id
716impl<T: Neg<Output = T>> Neg for Quaternion<T> {
717    type Output = Quaternion<T>;
718    fn neg(self) -> Self {
719        Quaternion(-self.0, -self.1, -self.2, self.3)
720    }
721}
722
723impl<T> From<Quaternion<T>> for Matrix3<T>
724where
725    T: One + Mul<T, Output = T> + Sub<T, Output = T> + Add<T, Output = T> + Copy,
726{
727    fn from(Quaternion(x, y, z, w): Quaternion<T>) -> Self {
728        let two = T::ONE + T::ONE;
729        let m11 = T::ONE - two * y * y - two * z * z;
730        let m22 = T::ONE - two * x * x - two * z * z;
731        let m33 = T::ONE - two * x * x - two * y * y;
732        let m12 = two * (x * y + w * z);
733        let m13 = two * (x * z - w * y);
734        let m21 = two * (x * y - w * z);
735        let m23 = two * (y * z + w * x);
736        let m31 = two * (x * z + w * y);
737        let m32 = two * (y * z - w * x);
738
739        Matrix3([m11, m12, m13], [m21, m22, m23], [m31, m32, m33])
740    }
741}
742impl<T> From<Quaternion<T>> for Matrix4<T>
743where
744    T: One + Zero + Mul<T, Output = T> + Sub<T, Output = T> + Add<T, Output = T> + Copy,
745{
746    fn from(Quaternion(x, y, z, w): Quaternion<T>) -> Self {
747        let two = T::ONE + T::ONE;
748        let m11 = T::ONE - two * y * y - two * z * z;
749        let m22 = T::ONE - two * x * x - two * z * z;
750        let m33 = T::ONE - two * x * x - two * y * y;
751        let m12 = two * (x * y + w * z);
752        let m13 = two * (x * z - w * y);
753        let m21 = two * (x * y - w * z);
754        let m23 = two * (y * z + w * x);
755        let m31 = two * (x * z + w * y);
756        let m32 = two * (y * z - w * x);
757
758        Matrix4(
759            [m11, m12, m13, T::ZERO],
760            [m21, m22, m23, T::ZERO],
761            [m31, m32, m33, T::ZERO],
762            [T::ZERO, T::ZERO, T::ZERO, T::ONE],
763        )
764    }
765}
766
767// Per-element Operations //
768macro_rules! VariadicElementOps
769{
770    (for $e: ident ($($n: tt),*)) =>
771    {
772        impl<T: Add> Add for $e<T>
773        {
774            type Output = $e<<T as Add>::Output>;
775            fn add(self, other: Self) -> Self::Output { $e($(self.$n + other.$n),*) }
776        }
777        impl<T: AddAssign> AddAssign for $e<T>
778        {
779            fn add_assign(&mut self, other: Self) { $(self.$n += other.$n);* }
780        }
781
782        impl<T: Sub> Sub for $e<T>
783        {
784            type Output = $e<<T as Sub>::Output>;
785            fn sub(self, other: Self) -> Self::Output { $e($(self.$n - other.$n),*) }
786        }
787        impl<T: SubAssign> SubAssign for $e<T>
788        {
789            fn sub_assign(&mut self, other: Self) { $(self.$n -= other.$n);* }
790        }
791
792        impl<T: Mul + Copy> Mul<T> for $e<T>
793        {
794            type Output = $e<<T as Mul>::Output>;
795            fn mul(self, other: T) -> Self::Output { $e($(self.$n * other),*) }
796        }
797        impl<T: MulAssign + Copy> MulAssign<T> for $e<T>
798        {
799            fn mul_assign(&mut self, other: T) { $(self.$n *= other);* }
800        }
801
802        impl<T: Neg> Neg for $e<T>
803        {
804            type Output = $e<<T as Neg>::Output>;
805            fn neg(self) -> Self::Output { $e($(-self.$n),*) }
806        }
807
808        impl<T: Mul<Output = T> + Add<Output = T> + Copy> $e<T>
809        {
810            /// Calculates an inner product between 2 vectors
811            pub fn dot(self, other: Self) -> T
812            {
813                CTSummation!($(self.$n * other.$n),*)
814            }
815        }
816        impl<T: Mul<Output = T> + Add<Output = T> + Copy> $e<T>
817        {
818            /// Calculates a squared length of a vector
819            pub fn len2(self) -> T
820            {
821                CTSummation!($(self.$n * self.$n),*)
822            }
823        }
824
825        impl<T> $e<T> {
826            /// Calculates minimum value of each element
827            pub fn min(self, other: Self) -> Self where T: crate::numtraits::Min<Output = T> {
828                $e($(self.$n.min(other.$n)),*)
829            }
830            /// Calculates maximum value of each element
831            pub fn max(self, other: Self) -> Self where T: crate::numtraits::Max<Output = T> {
832                $e($(self.$n.max(other.$n)),*)
833            }
834        }
835    }
836}
837macro_rules! CTSummation
838{
839    ($e: expr, $($e2: expr),+) => { $e $(.add($e2))* }
840}
841VariadicElementOps!(for Vector2 (0, 1));
842VariadicElementOps!(for Vector3 (0, 1, 2));
843VariadicElementOps!(for Vector4 (0, 1, 2, 3));
844
845impl<T> Vector2<T>
846where
847    T: Mul<Output = T> + Sub<Output = T> + Copy,
848{
849    /// Cross product between 2d vectors
850    pub fn cross(&self, other: &Self) -> T {
851        self.0 * other.1 - self.1 * other.0
852    }
853}
854impl<T> Vector3<T>
855where
856    T: Mul<T, Output = T> + Sub<T, Output = T> + Copy,
857{
858    /// Cross product between 3d vectors
859    pub fn cross(&self, other: &Self) -> Self {
860        let x = self.1 * other.2 - self.2 * other.1;
861        let y = self.2 * other.0 - self.0 * other.2;
862        let z = self.0 * other.1 - self.1 * other.0;
863
864        Vector3(x, y, z)
865    }
866}
867
868// External Interops //
869#[cfg(feature = "bedrock-interop")]
870mod bedrock_interop;
871#[cfg(feature = "euclid-interop")]
872mod euclid_interop;
873
874#[cfg(test)]
875mod tests {
876    use super::*;
877
878    #[test]
879    fn vector_dimension_transform() {
880        assert_eq!(Vector2(2, 3).with_z(1), Vector3(2, 3, 1));
881        assert_eq!(
882            Vector3(2.5, 3.0, 4.1).with_w(1.0),
883            Vector4(2.5, 3.0, 4.1, 1.0)
884        );
885        assert_eq!(Vector3::from(Vector4(4, 6, 8, 2)), Vector3(2, 3, 4));
886    }
887    #[test]
888    fn matrix_multiplication_identity() {
889        assert_eq!(Matrix3::ONE * Vector3(1, 2, 3), Vector3(1, 2, 3));
890        assert_eq!(
891            Matrix2::ONE * Matrix2::scale(Vector2(2, 3)),
892            Matrix2::scale(Vector2(2, 3))
893        );
894        assert_eq!(
895            Matrix3::ONE * Matrix3::scale(Vector3(2, 3, 4)),
896            Matrix3::scale(Vector3(2, 3, 4))
897        );
898        assert_eq!(
899            Matrix4::ONE * Matrix4::scale(Vector4(2, 3, 4, 5)),
900            Matrix4::scale(Vector4(2, 3, 4, 5))
901        );
902    }
903    #[test]
904    fn matrix_transforming() {
905        assert_eq!(
906            Matrix3([1, 0, 2], [0, 1, 3], [0, 0, 1]) * Vector2(0, 0),
907            Vector3(2, 3, 1)
908        );
909        assert_eq!(
910            Matrix4([1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 4], [0, 0, 0, 1]) * Vector3(1, 2, 3),
911            Vector4(3, 5, 7, 1)
912        );
913        assert_eq!(Matrix2::scale(Vector2(2, 3)) * Vector2(1, 1), Vector2(2, 3));
914    }
915    #[test]
916    fn matrix_extension() {
917        assert_eq!(
918            Matrix3::from(Matrix2([0, 1], [1, 0])),
919            Matrix3([0, 1, 0], [1, 0, 0], [0, 0, 1])
920        );
921        assert_eq!(
922            Matrix4::from(Matrix3::from(Matrix2([0, 1], [2, 3]))),
923            Matrix4([0, 1, 0, 0], [2, 3, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1])
924        );
925    }
926    #[test]
927    #[allow(clippy::no_effect)]
928    fn vector_ops() {
929        assert_eq!(Vector3(0, 1, 2) + Vector3(3, 4, 5), Vector3(3, 5, 7));
930        assert_eq!(Vector3(6, 7, 8) - Vector3(3, 4, 5), Vector3(3, 3, 3));
931        assert_eq!(Vector3(0, 2, 4) * 3, Vector3(0, 6, 12));
932        assert_eq!(-Vector3(1, 2, -1), Vector3(-1, -2, 1));
933        assert_eq!(
934            Vector3(2, 3, 4).dot(Vector3(5, 6, 7)),
935            2 * 5 + 3 * 6 + 4 * 7
936        );
937        assert_eq!(Vector2(0, 1).dot(Vector2(1, 0)), 0);
938        #[allow(clippy::identity_op)]
939        {
940            assert_eq!(Vector3(1, 2, 3).len2(), 1 * 1 + 2 * 2 + 3 * 3);
941        }
942        assert_eq!(Vector2(10, 3).cross(&Vector2(4, 30)), 10 * 30 - 3 * 4);
943        assert_eq!(
944            Vector3(2, 3, 4).cross(&Vector3(6, 7, 8)),
945            Vector3(3 * 8 - 4 * 7, 4 * 6 - 2 * 8, 2 * 7 - 6 * 3)
946        );
947    }
948    #[test]
949    fn inv_quaternion() {
950        let q = Quaternion(0.0f32, 1.0, 0.0, 3.0).normalize();
951        let Quaternion(a, b, c, d) = q * -q;
952        // approximate
953        assert!(a.abs() <= f32::EPSILON);
954        assert!(b.abs() <= f32::EPSILON);
955        assert!(c.abs() <= f32::EPSILON);
956        assert!((1.0 - d).abs() <= f32::EPSILON);
957    }
958    #[test]
959    fn vector_ops_assign() {
960        let mut v1 = Vector3(0, 1, 2);
961        v1 += Vector3(1, -3, 2);
962        assert_eq!(v1, Vector3(0, 1, 2) + Vector3(1, -3, 2));
963        v1 -= Vector3(1, -3, 2);
964        assert_eq!(v1, Vector3(0, 1, 2) + Vector3(1, -3, 2) - Vector3(1, -3, 2));
965        v1 *= 4;
966        assert_eq!(
967            v1,
968            (Vector3(0, 1, 2) + Vector3(1, -3, 2) - Vector3(1, -3, 2)) * 4
969        );
970    }
971}
peridot_math/linarg.rs

peridot_math/
linarg.rs
