hexga_math/geometry/
matrix.rs

1use crate::*;
2
3/// 1x1 matrix
4pub type Matrix1<T> = SquareMatrix<T,1>;
5/// 2x2 matrix
6pub type Matrix2<T> = SquareMatrix<T,2>;
7/// 3x3 matrix
8pub type Matrix3<T> = SquareMatrix<T,3>;
9/// 4x4 matrix
10pub type Matrix4<T> = SquareMatrix<T,4>;
11
12pub type Mat<const ROW : usize, const COL : usize> = Matrix<float,ROW,COL>;
13/// 1x1 matrix
14pub type Mat1 = Mat<1,1>;
15/// 2x2 matrix
16pub type Mat2 = Mat<2,2>;
17/// 3x3 matrix
18pub type Mat3 = Mat<3,3>;
19/// 4x4 matrix
20pub type Mat4 = Mat<4,4>;
21
22pub type MatP<const COL : usize> = SquareMatrix<int,COL>;
23/// 1x1 matrix
24pub type Mat1P = MatP<1>;
25/// 2x2 matrix
26pub type Mat2P = MatP<2>;
27/// 3x3 matrix
28pub type Mat3P = MatP<3>;
29/// 4x4 matrix
30pub type Mat4P = MatP<4>;
31
32/// NxN matrix
33pub type SquareMatrix<T, const N : usize> = Matrix<T, N, N>;
34
35/// ROW rows, COL columns.
36/// 
37/// Can be indexed `matrix[row][col]`
38#[repr(C)]
39#[derive(PartialEq, Eq, Clone, Copy)]
40pub struct Matrix<T, const ROW : usize, const COL : usize>
41{
42    pub columns : Vector<Vector<T, ROW>,COL>,
43}
44
45impl<T, const ROW : usize, const COL : usize> Deref for Matrix<T, ROW, COL>
46{
47    type Target=Vector<Vector<T, ROW>,COL>;
48    fn deref(&self) -> &Self::Target { &self.columns }
49}
50
51impl<T, const ROW : usize, const COL : usize> DerefMut for Matrix<T, ROW, COL>
52{
53    fn deref_mut(&mut self) -> &mut Self::Target { &mut self.columns }
54}
55
56impl<T, const ROW : usize, const COL : usize>  Matrix<T, ROW, COL>
57{
58    fn _fmt(&self, f: &mut Formatter<'_>, d : impl Fn(&T, &mut Formatter<'_>) -> DResult) -> DResult
59    {
60        for c in 0..COL
61        {
62            for r in 0..ROW
63            {
64                d(&self[c][r], f)?;
65                write!(f, " ")?;
66            }
67            writeln!(f)?;
68        }
69        Ok(())
70    }
71}
72
73use std::fmt::*;
74impl<T, const ROW : usize, const COL : usize> Display for Matrix<T, ROW, COL> where T : Display { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
75impl<T, const ROW : usize, const COL : usize> Debug for Matrix<T, ROW, COL> where T : Debug { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
76impl<T, const ROW : usize, const COL : usize> Octal for Matrix<T, ROW, COL> where T : Octal { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
77impl<T, const ROW : usize, const COL : usize> Binary for Matrix<T, ROW, COL> where T : Binary { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
78impl<T, const ROW : usize, const COL : usize> LowerHex for Matrix<T, ROW, COL> where T : LowerHex { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
79impl<T, const ROW : usize, const COL : usize> UpperHex for Matrix<T, ROW, COL> where T : UpperHex { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
80impl<T, const ROW : usize, const COL : usize> LowerExp for Matrix<T, ROW, COL> where T : LowerExp { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
81impl<T, const ROW : usize, const COL : usize> UpperExp for Matrix<T, ROW, COL> where T : UpperExp { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
82impl<T, const ROW : usize, const COL : usize> Pointer for Matrix<T, ROW, COL> where T : Pointer { fn fmt(&self, f: &mut Formatter<'_>) -> DResult  { self._fmt(f, T::fmt) } }
83
84impl<T, const ROW : usize, const COL : usize> Default for Matrix<T,ROW,COL> where Vector<Vector<T, ROW>,COL> : Default
85{
86    fn default() -> Self {
87        Self { columns: ___() }
88    }
89}
90
91impl<T, const ROW : usize> Matrix<T,ROW,1>
92{
93    /// From a `Mx1` vector
94    pub const fn from_vector(vector : Vector<T, ROW>) -> Self 
95    {
96        Self::from_col(Vector::from_array([vector]))
97    }
98}
99
100impl<T, const ROW : usize> From<Vector<T, ROW>> for Matrix<T,ROW,1>
101{
102    fn from(value: Vector<T, ROW>) -> Self {
103        Self::from_vector(value)
104    }
105}
106impl<T, const ROW : usize> From<Matrix<T,ROW,1>> for Vector<T, ROW> where Vector<T,ROW> : Default
107{
108    fn from(mut value: Matrix<T,ROW,1>) -> Self 
109    {
110        std::mem::take(&mut value.columns[0])
111    }
112}
113
114impl<T, const ROW : usize, const COL : usize> Matrix<T,ROW,COL>
115{
116    pub fn from_fn<F>(mut columns : F) -> Self where F : FnMut(Point2) -> T, Self : Default
117    {
118        Self::from_col_array(std::array::from_fn(|column| std::array::from_fn(|row| columns(point2(column as _,row as _)))))
119    }
120
121    pub fn from_fn_col_and_row<F>(mut fn_columns_then_rows : F) -> Self where F : FnMut(usize, usize) -> T, Self : Default
122    {
123        Self::from_col_array(std::array::from_fn(|column| std::array::from_fn(|row| fn_columns_then_rows(column,row))))
124    }
125
126    pub fn from_fn_row_and_col<F>(mut fn_columns_then_rows : F) -> Self where F : FnMut(usize, usize) -> T, Self : Default
127    {
128        Self::from_col_array(std::array::from_fn(|column| std::array::from_fn(|row| fn_columns_then_rows(row,column))))
129    }
130
131    pub const fn from_col(columns : Vector<Vector<T, ROW>,COL>) -> Self { Self { columns }}
132    pub fn from_col_array(columns : [[T;ROW];COL]) -> Self 
133    { 
134        Self 
135        { 
136            columns :  Vector::from_array(columns.map(|array| Vector::from_array(array)))
137        }
138    }
139
140    pub fn from_row(rows : Vector<Vector<T, COL>,ROW>) -> Self where Vector::<Vector<T, ROW>, COL> : Default, T : Copy
141    {
142        Matrix::from_col(rows).transpose()
143    }
144    pub fn from_row_array(rows : [[T;COL];ROW]) -> Self where Vector::<Vector<T, ROW>, COL> : Default, T : Copy
145    {
146        Self::from_row(Vector::from_array(rows.map(|array| Vector::from_array(array))))
147    }
148
149    pub fn col(&self) -> Vector<Vector<T, ROW>,COL> where Vector<Vector<T, ROW>,COL> : Copy { self.columns }
150    pub fn row(&self) -> Vector<Vector<T, COL>,ROW> where Vector::<Vector<T, COL>, ROW> : Default, T : Copy { self.transpose().columns }
151    
152    /// Transpose the matrix
153    /// 
154    /// ```rust
155    /// use hexga_math::*;
156    /// 
157    /// assert_eq!(
158    ///     Mat2::from_col(vector2(vec2(1., 2.), vec2(3., 4.))).transpose(), 
159    ///     Mat2::from_col(vector2(vec2(1., 3.), vec2(2., 4.)))
160    /// );
161    /// 
162    /// assert_eq!(
163    ///     Mat::<3,2>::from_col(vector2(vec3(1., 2., 3.), vec3(4., 5., 6.))).transpose(),
164    ///     Mat::<2,3>::from_col(vector3(vec2(1., 4.), vec2(2., 5.), vec2(3., 6.)))
165    /// );
166    /// ```
167    pub fn transpose(&self) -> Matrix<T,COL,ROW> where Vector::<Vector<T, COL>, ROW> : Default, T : Copy
168    {
169        let mut transposed_columns = Vector::<Vector<T, COL>, ROW>::default();
170
171        for i in 0..COL 
172        {
173            for j in 0..ROW
174            {
175                transposed_columns[j][i] = self[i][j];
176            }
177        }
178        Matrix::from_col(transposed_columns)
179    }
180}
181
182// 2D
183impl<T> SquareMatrix<T,2> 
184    where 
185    Self : Copy, 
186    SquareMatrix<T, 2> : Mul<Vector<T, 2>, Output = Vector<T, 2>>,
187    Vector<T, 2> : Into< Vector<T, 1>>
188{
189    pub fn transform_relative(self, relative : Vector<T, 1>) -> Vector<T, 1> where T : Zero
190    {
191        self.transform(relative.with_y(T::ZERO)).into()
192    }
193
194    pub fn transform_position(self, position : Vector<T, 1>) -> Vector<T, 1> where T : One
195    {
196        self.transform(position.with_y(T::ONE)).into()
197    }
198}
199
200// 3D
201impl<T> SquareMatrix<T,3> 
202    where 
203    Self : Copy, 
204    SquareMatrix<T, 3> : Mul<Vector<T, 3>, Output = Vector<T, 3>>,
205    Vector<T, 3> : Into< Vector<T, 2>>
206{
207    pub fn transform_relative(self, relative : Vector<T, 2>) -> Vector<T, 2> where T : Zero
208    {
209        self.transform(relative.with_z(T::ZERO)).into()
210    }
211
212    pub fn transform_position(self, position : Vector<T, 2>) -> Vector<T, 2> where T : One
213    {
214        self.transform(position.with_z(T::ONE)).into()
215    }
216}
217
218// 4D
219impl<T> SquareMatrix<T,4> 
220    where 
221    Self : Copy, 
222    SquareMatrix<T, 4> : Mul<Vector<T, 4>, Output = Vector<T, 4>>,
223    Vector<T, 4> : Into< Vector<T, 3>>
224{
225    pub fn transform_relative(self, relative : Vector<T, 3>) -> Vector<T, 3> where T : Zero
226    {
227        self.transform(relative.with_w(T::ZERO)).into()
228    }
229
230    pub fn transform_position(self, position : Vector<T, 3>) -> Vector<T, 3> where T : One
231    {
232        self.transform(position.with_w(T::ONE)).into()
233    }
234}
235
236
237impl<T, const N : usize> SquareMatrix<T,N> 
238    where
239    Self : Copy, 
240    SquareMatrix<T, N>: Mul<Vector<T, N>, Output = Vector<T, N>>
241{
242    /* 
243    /// The last value/component will be replaced by 0 because it is relative/a vector in math terms.
244    pub fn transform_relative(self, relative : Vector<T, N>) -> Vector<T, N> where T : Zero
245    {
246        self.transform(relative.with(N-1, T::ZERO))
247    }
248
249    /// The last value/component will be replaced by 1 because it is fixed/point in math terms.
250    pub fn transform_fixed(self, fixed : Vector<T, N>) -> Vector<T, N> where T : Zero
251    {
252        self.transform(fixed.with(N-1, T::ZERO))
253    }
254    */
255    /// The last value/component is 0 if it is a relative/vector/delta, 1 if it is fixed/point/position in math terms. 
256    pub fn transform(self, value : Vector<T, N>) -> Vector<T, N>
257    {
258        self * value
259    }
260}
261
262
263
264
265impl<T, const ROW : usize, const COL : usize> Zero for Matrix<T,ROW,COL> where Vector<Vector<T,ROW>,COL> : Zero
266{
267    const ZERO : Self = Self::from_col(Vector::<Vector::<T, ROW>, COL>::ZERO);
268}
269
270impl<T, const N : usize> SquareMatrix<T,N> where T : One + Zero + Copy
271{
272    /// Same as [One]
273    pub const IDENTITY : Self = Self::ONE;
274}
275
276impl<T, const N : usize> One for SquareMatrix<T,N> where T : One + Zero + Copy
277{
278    const ONE : Self =
279    {
280        let mut col = [[T::ZERO; N]; N];
281        let mut i = 0;
282        while i < N
283        {
284            col[i][i] = T::ONE;
285            i+=1;
286        }
287        Self::from_col(unsafe { std::mem::transmute_copy(&col) })
288    };
289}
290
291impl<T, const N : usize> MinusOne for SquareMatrix<T,N> where T : MinusOne + Zero + Copy
292{
293    const MINUS_ONE : Self =
294    {
295        let mut col = [[T::ZERO; N]; N];
296        let mut i = 0;
297        while i < N
298        {
299            col[i][i] = T::MINUS_ONE;
300            i+=1;
301        }
302        Self::from_col(unsafe { std::mem::transmute_copy(&col) })
303    };
304}
305
306impl<T, const N : usize> Half for SquareMatrix<T,N> where T : Half + Zero + Copy
307{
308    const HALF : Self =
309    {
310        let mut col = [[T::ZERO; N]; N];
311        let mut i = 0;
312        while i < N
313        {
314            col[i][i] = T::HALF;
315            i+=1;
316        }
317        Self::from_col(unsafe { std::mem::transmute_copy(&col) })
318    };
319}
320
321impl<T, const ROW : usize, const COL : usize> MinValue for Matrix<T,ROW,COL> where T : MinValue + Copy
322{
323    const MIN : Self = Self::from_col(Vector::from_array([Vector::from_array([T::MIN; ROW]); COL]));
324}
325impl<T, const ROW : usize, const COL : usize> MaxValue for Matrix<T,ROW,COL> where T : MaxValue + Copy
326{
327    const MAX : Self = Self::from_col(Vector::from_array([Vector::from_array([T::MAX; ROW]); COL]));
328}
329impl<T, const ROW : usize, const COL : usize> NaNValue for Matrix<T,ROW,COL> where T : NaNValue + Copy
330{
331    const NAN : Self = Self::from_col(Vector::from_array([Vector::from_array([T::NAN; ROW]); COL]));
332}
333
334
335impl<T, const ROW : usize, const COL : usize> Add<Self> for Matrix<T,ROW,COL> 
336    where 
337    Vector<Vector<T, ROW>, COL> : Add<Vector<Vector<T, ROW>, COL>,Output = Vector<Vector<T, ROW>, COL>>
338{
339    type Output = Self;
340    
341    fn add(self, rhs: Self) -> Self::Output 
342    {
343        Self::from_col(self.columns.add(rhs.columns))
344    }
345}
346
347impl<T, const ROW : usize, const COL : usize> AddAssign<Self> for Matrix<T,ROW,COL> 
348    where 
349    Self : Add<Self,Output = Self> + Copy
350{
351    fn add_assign(&mut self, rhs: Self) {
352        *self = (*self).add(rhs);
353    }
354}
355
356
357impl<T, const ROW : usize, const COL : usize> Sub<Self> for Matrix<T,ROW,COL> 
358    where 
359    Vector<Vector<T, ROW>, COL> : Sub<Vector<Vector<T, ROW>, COL>,Output = Vector<Vector<T, ROW>, COL>>
360{
361    type Output = Self;
362    
363    fn sub(self, rhs: Self) -> Self::Output 
364    {
365        Self::from_col(self.columns.sub(rhs.columns))
366    }
367}
368
369impl<T, const ROW : usize, const COL : usize> SubAssign<Self> for Matrix<T,ROW,COL> 
370    where 
371    Self : Sub<Self,Output = Self> + Copy
372{
373    fn sub_assign(&mut self, rhs: Self) {
374        *self = (*self).sub(rhs);
375    }
376}
377
378
379impl<T, const ROW : usize, const COL : usize> Sum for Matrix<T,ROW,COL> where Self : Zero + Add<Self,Output = Self>
380{
381    fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
382        iter.fold(Self::ZERO, Self::add)
383    }
384}
385
386impl<T, const COL : usize> Product for SquareMatrix<T,COL> where Self : One + Mul<Self,Output = Self>
387{
388    fn product<I: Iterator<Item = Self>>(iter: I) -> Self {
389        iter.fold(Self::ONE, Self::mul)
390    }
391}
392
393/// ```
394/// use hexga_math::*;
395/// 
396/// let m1  = Mat2P::from_col((point2(7, 6), point2(5, 3)).into());
397/// let m2  = Mat2P::from_col((point2(2, 5), point2(1, 1)).into());
398/// let m3  = Mat2P::from_col((point2(39, 27), point2(12, 9)).into());
399/// 
400/// assert_eq!(m1 * m2, m3);
401/// 
402/// 
403/// let m1 = Matrix::<i32,2,3>::from_col(vector3(vector2(1, 4), vector2(2, 5), vector2(3, 6)));
404/// let m2 = Matrix::<i32,3,2>::from_col(vector2(vector3(7, 9, 11), vector3(8, 10, 12)));
405/// let m3 = Matrix::<i32,2,2>::from_col(vector2(vector2(1*7+2*9+3*11, 4*7+5*9+6*11), vector2(1*8+2*10+3*12, 4*8+5*10+6*12)));
406/// 
407/// assert_eq!(m1 * m2, m3);
408/// ```
409impl<T, const ROW : usize, const COL : usize, const COL2 : usize> Mul<Matrix<T,COL,COL2>> for Matrix<T,ROW,COL>
410    where
411    T : NumberArithmetic,
412    Matrix<T,ROW,COL2> : Default
413{
414    type Output = Matrix<T, ROW, COL2>;
415    
416    fn mul(self, rhs: Matrix<T,COL,COL2>) -> Self::Output 
417    {
418        let mut result = Matrix::___();
419
420        for colum in 0..COL2 
421        {
422            for row in 0..ROW 
423            {
424                let mut sum = T::ZERO;
425                for k in 0..COL 
426                {
427                    sum += self[k][row] * rhs[colum][k];
428                }
429                result[colum][row] = sum;
430            }
431        }
432        result
433    }
434}
435
436
437impl<T, const ROW : usize, const COL : usize> Mul<Vector<T,COL>> for Matrix<T,ROW,COL>
438    where 
439    T : NumberArithmetic,
440    //Matrix<T,ROW,COL> : Mul<Matrix<T,ROW,1>>,
441    Self : Mul<Matrix<T,COL,1>, Output = Matrix<T,ROW,1>>,
442    Vector<T,COL> : From<Matrix::<T,ROW,1>>
443{
444    type Output = Vector<T,COL>;
445    
446    fn mul(self, rhs: Vector<T,COL>) -> Self::Output 
447    {
448        let m : Matrix::<T,COL,1> = rhs.into();
449        let r = self * m;
450        r.into()
451    }
452}
453
454impl<T, const COL : usize> MulAssign<Self> for SquareMatrix<T,COL> 
455    where 
456    Self : Mul<Self,Output = Self> + Copy
457{
458    fn mul_assign(&mut self, rhs: Self) {
459        *self = *self * rhs;
460    }
461}
462
463impl<T, const ROW : usize, const COL : usize> Mul<T> for Matrix<T,ROW,COL> where T : Copy + Mul<T>
464{
465    type Output = Matrix<T::Output,ROW,COL>;
466    
467    fn mul(self, rhs: T) -> Self::Output 
468    {
469        Matrix::from_col(self.columns.map(|v| v.map(|i| i.mul(rhs))))
470    }
471}
472
473impl<T, const ROW : usize, const COL : usize> MulAssign<T> for Matrix<T,ROW,COL> where T : Copy + Mul<T,Output = T>
474{
475    fn mul_assign(&mut self, rhs: T) {
476        *self = (*self).mul(rhs);
477    }
478}
479
480impl<T, const ROW : usize, const COL : usize> Div<T> for Matrix<T,ROW,COL> where T : Copy + Div<T>
481{
482    type Output = Matrix<T::Output,ROW,COL>;
483    
484    fn div(self, rhs: T) -> Self::Output 
485    {
486        Matrix::from_col(self.columns.map(|v| v.map(|i| i.div(rhs))))
487    }
488}
489
490impl<T, const ROW : usize, const COL : usize> DivAssign<T> for Matrix<T,ROW,COL> where T : Copy + Div<T,Output = T>
491{
492    fn div_assign(&mut self, rhs: T) {
493        *self = (*self).div(rhs);
494    }
495}
496
497
498
499impl<T, const N: usize> SquareMatrix<T, N>
500where
501    T: FloatingNumber,
502{
503    /// Inverse the matrix, or return `None` if the matrix is singular.
504    /// 
505    /// ```
506    /// use hexga_math::*;
507    /// 
508    /// assert_eq!(Mat2::IDENTITY.inverse(), Some(Mat2::IDENTITY));
509    /// 
510    /// // Non inversible matrix
511    /// let mut m2 = Mat2::IDENTITY;
512    /// m2.set_y(vec2(0., 0.));
513    /// assert_eq!(m2.inverse(), None);
514    /// 
515    /// let m2 = Mat2::from_col(vector2(vec2(1., 3.), vec2(2., 4.)));
516    /// let m2_inv = m2.inverse().unwrap();
517    /// 
518    /// assert_eq!(m2 * m2_inv, Mat2::IDENTITY);
519    /// ```
520    pub fn inverse(mut self) -> Option<Self> 
521    {
522        let mut augmented = Self::IDENTITY;
523
524        for i in 0..N 
525        {
526            if self[i][i] == T::ZERO { return None; }
527
528            let pivot = self[i][i];
529
530            for j in 0..N 
531            {
532                self[i][j] /= pivot;
533                augmented[i][j] /=  pivot;
534            }
535
536            for k in (i + 1)..N 
537            {
538                let factor = self[k][i];
539                for j in 0..N 
540                {
541                    self[k][j] = self[k][j] - factor * self[i][j];
542                    let a = augmented[i][j];
543                    augmented[k][j] -= factor * a;
544                }
545            }
546        }
547
548        for i in (0..N).rev() 
549        {
550            for k in 0..i 
551            {
552                let factor = self[k][i];
553                for j in 0..N 
554                {
555                    let a = self[i][j];
556                    self[k][j] -= factor * a;
557                    let a = augmented[i][j];
558                    augmented[k][j] -= factor * a;
559                }
560            }
561        }
562
563        Some(augmented)
564    }
565}
566
567
568// Todo : impl det in a generic way once const generic will be here.
569// Do a match on N, and hardcode the case for N in 0..=4
570
571// There is no generic way to compute the determinant of a matrix, because it required calculaing the submatrix, but at the moment const generic operation are not possible 
572// I also don't want any heap allocation when computing the determinant
573impl<T> SquareMatrix<T, 0> where T: NumberArithmetic + One,
574{
575    pub fn det(&self) -> T 
576    {
577        T::ONE
578    }
579}
580
581impl<T> SquareMatrix<T, 1> where T: NumberArithmetic + One,
582{
583    pub fn det(&self) -> T 
584    {
585        self[0][0]
586    }
587}
588
589impl<T> SquareMatrix<T, 2> where T: NumberArithmetic + One,
590{
591    pub fn det(&self) -> T 
592    {
593        self[0][0] * self[1][1] - self[0][1] * self[1][0]
594    }
595}
596
597impl<T> SquareMatrix<T, 3> where T: NumberArithmetic + One,
598{
599    pub fn det(&self) -> T 
600    {
601          self[0][0] * (self[1][1] * self[2][2] - self[1][2] * self[2][1])
602        - self[0][1] * (self[1][0] * self[2][2] - self[1][2] * self[2][0])
603        + self[0][2] * (self[1][0] * self[2][1] - self[1][1] * self[2][0])
604    }
605}
606
607impl<T> SquareMatrix<T, 4> where T: NumberArithmetic + One,
608{
609    pub fn det(&self) -> T 
610    {
611        let a = self[2][2] * self[3][3] - self[2][3] * self[3][2];
612        let b = self[2][1] * self[3][3] - self[2][3] * self[3][1];
613        let c = self[2][1] * self[3][2] - self[2][2] * self[3][1];
614        let d = self[2][0] * self[3][3] - self[2][3] * self[3][0];
615        let e = self[2][0] * self[3][2] - self[2][2] * self[3][0];
616        let f = self[2][0] * self[3][1] - self[2][1] * self[3][0];
617
618        self[0][0] * 
619        (
620              self[1][1] * a
621            - self[1][2] * b
622            + self[1][3] * c
623        )
624
625        - self[0][1] * 
626        (
627            self[1][0] * a
628          - self[1][2] * d
629          + self[1][3] * e
630        )
631
632        + self[0][2] * 
633        (
634            self[1][0] * b
635          - self[1][1] * d
636          + self[1][3] * f
637        )
638        
639        - self[0][3] * 
640        (
641            self[1][0] * c
642          - self[1][1] * e
643          + self[1][2] * f
644        )
645    }
646}
647
648
649macro_rules! matrix_have_x {
650    ($dim : expr) => 
651    {
652        impl<T> HaveX<Vector<T,$dim>> for SquareMatrix<T,$dim>
653        {
654            fn iter_x<'a>(&'a self) -> impl Iterator<Item=&'a Vector<T,$dim>> where Vector<T,$dim>: 'a {
655                self.columns.as_array().as_slice()[0..=Self::X_INDEX].iter()
656            }
657        
658            fn iter_x_mut<'a>(&'a mut self) -> impl Iterator<Item=&'a mut Vector<T,$dim>> where Vector<T,$dim>: 'a {
659                self.columns.as_array_mut().as_mut_slice()[0..=Self::X_INDEX].iter_mut()
660            }
661        }
662    };
663}
664
665matrix_have_x!(1);
666matrix_have_x!(2);
667matrix_have_x!(3);
668matrix_have_x!(4);
669
670macro_rules! matrix_have_y {
671    ($dim : expr) => 
672    {
673        impl<T> HaveY<Vector<T,$dim>> for SquareMatrix<T,$dim>
674        {
675            fn iter_xy<'a>(&'a self) -> impl Iterator<Item=&'a Vector<T,$dim>> where Vector<T,$dim>: 'a {
676                self.columns.as_array().as_slice()[0..=Self::Y_INDEX].iter()
677            }
678        
679            fn iter_xy_mut<'a>(&'a mut self) -> impl Iterator<Item=&'a mut Vector<T,$dim>> where Vector<T,$dim>: 'a {
680                self.columns.as_array_mut().as_mut_slice()[0..=Self::Y_INDEX].iter_mut()
681            }
682        }
683    };
684}
685
686matrix_have_y!(2);
687matrix_have_y!(3);
688matrix_have_y!(4);
689
690macro_rules! matrix_have_z {
691    ($dim : expr) => 
692    {
693        impl<T> HaveZ<Vector<T,$dim>> for SquareMatrix<T,$dim>
694        {
695            fn iter_xyz<'a>(&'a self) -> impl Iterator<Item=&'a Vector<T,$dim>> where Vector<T,$dim>: 'a {
696                self.columns.as_array().as_slice()[0..=Self::Z_INDEX].iter()
697            }
698        
699            fn iter_xyz_mut<'a>(&'a mut self) -> impl Iterator<Item=&'a mut Vector<T,$dim>> where Vector<T,$dim>: 'a {
700                self.columns.as_array_mut().as_mut_slice()[0..=Self::Z_INDEX].iter_mut()
701            }
702        }
703    };
704}
705
706matrix_have_z!(3);
707matrix_have_z!(4);
708
709macro_rules! matrix_have_w {
710    ($dim : expr) => 
711    {
712        impl<T> HaveW<Vector<T,$dim>> for SquareMatrix<T,$dim>
713        {
714            fn iter_xyzw<'a>(&'a self) -> impl Iterator<Item=&'a Vector<T,$dim>> where Vector<T,$dim>: 'a {
715                self.columns.as_array().as_slice()[0..=Self::W_INDEX].iter()
716            }
717        
718            fn iter_xyzw_mut<'a>(&'a mut self) -> impl Iterator<Item=&'a mut Vector<T,$dim>> where Vector<T,$dim>: 'a {
719                self.columns.as_array_mut().as_mut_slice()[0..=Self::W_INDEX].iter_mut()
720            }
721        }
722    };
723}
724
725matrix_have_w!(4);
726
727impl<T, const N : usize> HaveRotationX<T> for SquareMatrix<T,N> 
728    where
729        Self : HaveZ<Vector<T,N>> + Zero + Mul<Self, Output = Self>, 
730        T : FloatingNumber 
731{
732    fn rotate_x(&mut self, angle : AngleOf<T>) -> &mut Self
733    {
734        *self = Self::from_rotation_x(angle) * (*self);
735        self
736    }
737}
738
739impl<T, const N : usize> HaveRotationY<T> for SquareMatrix<T,N> 
740    where
741        Self : HaveZ<Vector<T,N>> + Zero + Mul<Self, Output = Self>, 
742        T : FloatingNumber 
743{
744    fn rotate_y(&mut self, angle : AngleOf<T>) -> &mut Self
745    {
746        *self = Self::from_rotation_y(angle) * (*self);
747        self
748    }
749}
750
751impl<T, const N : usize> HaveRotationZ<T> for SquareMatrix<T,N> 
752    where
753        Self : HaveY<Vector<T,N>> + Zero + Mul<Self, Output = Self>, 
754        T : FloatingNumber 
755{
756    fn rotate_z(&mut self, angle : AngleOf<T>) -> &mut Self
757    {
758        *self = Self::from_rotation_z(angle) * (*self);
759        self
760    }
761}
762
763impl<T, const N : usize> Position<Vector<T,N>,N> for SquareMatrix<T,N> where T : Copy
764{
765    fn pos(&self) -> Vector<Vector<T,N>,N> {
766        self.col()
767    }
768
769    fn set_pos(&mut self, pos : Vector<Vector<T,N>,N>) -> &mut Self {
770        *self = Self::from_col(pos);
771        self
772    }
773}
774
775impl<T, const N : usize> Position<T,N> for SquareMatrix<T,N> where T : Copy
776{
777    fn pos(&self) -> Vector<T,N> {
778        self.columns[N-1]
779    }
780
781    fn set_pos(&mut self, pos : Vector<T,N>) -> &mut Self {
782        self.columns[N-1] = pos;
783        self
784    }
785}
786
787
788/// the rotation code is mainly based on glam code
789impl<T, const N : usize> SquareMatrix<T,N> where Self : HaveZ<Vector<T,N>> + Zero, T : FloatingNumber
790{
791    pub fn from_rotation_axis(axis: Vector3<T>, angle : AngleOf<T>) -> Self
792    {
793        let (sin, cos) = T::sin_cos(angle.radian());
794        let axis_sin = axis.mul(sin);
795        let axis_sq = axis.mul(axis);
796        let omc = T::ONE - cos;
797        let xyomc = axis.x * axis.y * omc;
798        let xzomc = axis.x * axis.z * omc;
799        let yzomc = axis.y * axis.z * omc;
800
801        let mut r = Self::ZERO;
802        r[0][0] = axis_sq.x * omc + cos;
803        r[0][1] = xyomc + axis_sin.z;
804        r[0][2] = xzomc - axis_sin.y;
805
806        r[1][0] = xyomc - axis_sin.z;
807        r[1][1] = axis_sq.y * omc + cos;
808        r[1][2] = yzomc + axis_sin.x;
809
810        r[2][0] = xzomc + axis_sin.y;
811        r[2][1] = yzomc - axis_sin.x;
812        r[2][2] = axis_sq.z * omc + cos;
813
814        r
815    }
816
817    pub fn from_rotation_x(angle : AngleOf<T>) -> Self
818    {
819        let (sina, cosa) = T::sin_cos(angle.radian());
820        let mut r = Self::ZERO;
821
822        r[Self::Y_INDEX][Self::Y_INDEX] =  cosa;
823        r[Self::Y_INDEX][Self::Z_INDEX] =  sina;
824        r[Self::Z_INDEX][Self::Y_INDEX] = -sina;
825        r[Self::Z_INDEX][Self::Z_INDEX] =  cosa;
826        r
827    }
828
829    pub fn from_rotation_y(angle : AngleOf<T>) -> Self
830    {
831        let (sina, cosa) = T::sin_cos(angle.radian());
832        let mut r = Self::ZERO;
833
834        r[Self::X_INDEX][Self::X_INDEX] =  cosa;
835        r[Self::X_INDEX][Self::Z_INDEX] = -sina;
836        r[Self::Z_INDEX][Self::X_INDEX] =  sina;
837        r[Self::Z_INDEX][Self::Z_INDEX] =  cosa;
838        r
839    }
840}
841
842impl<T, const N : usize> SquareMatrix<T,N> where Self : HaveY<Vector<T,N>> + Zero, T : FloatingNumber
843{
844    pub fn from_rotation_z(angle : AngleOf<T>) -> Self
845    {
846        let (sina, cosa) = T::sin_cos(angle.radian());
847        let mut r = Self::ZERO;
848        
849        r[Self::X_INDEX][Self::X_INDEX] =  cosa;
850        r[Self::X_INDEX][Self::Y_INDEX] =  sina;
851        r[Self::Y_INDEX][Self::X_INDEX] = -sina;
852        r[Self::Y_INDEX][Self::Y_INDEX] =  cosa;
853        r
854    }
855}
856
857impl<T, const N : usize> SquareMatrix<T,N> where Self : HaveZ<Vector<T,N>> + One, T : Copy + Zero + One
858{
859    pub fn from_scale(scale : impl ToVectorFilled<T,N>) -> Self
860    {
861        let scale = scale.to_vector_filled(T::ONE);
862
863        let mut r = Self::IDENTITY;
864        for i in 0..N
865        {
866            r[i][i] = scale[i];
867        }
868        r
869    }
870
871    pub fn from_translation(translation: impl ToVectorFilled<T,N>) -> Self where T : Zero + AddAssign<T>
872    {
873        let translation = translation.to_vector_filled(T::ZERO);
874        let mut r = Self::IDENTITY;
875        for i in 0..N 
876        {
877            r[N-1][i] += translation[i];
878        }
879        r
880    }
881}
882
883
884#[cfg(test)]
885mod test_matrix
886{
887    use crate::*;
888
889    #[test]
890    fn rotation_zero_on_z() 
891    {
892        let m = Mat2::from_col_array([[1.,2.], [3.,4.]]);
893        assert_eq!(m.rotated_z(0.degree()), m);
894    }
895}
hexga_math/geometry/matrix.rs

hexga_math/geometry/
matrix.rs
