qm/m/
m4.rs

1//! matrix4
2//!
3
4use num::Float;
5
6use crate::v::{TVector, v4::Vector4};
7use crate::m::TMatrix;
8use crate::q::TQuaternion;
9
10/// TM4
11pub trait TM4<F: Float + std::fmt::Debug> {
12  /// from Quaternion (qp = Q4x4 p4)
13  fn from_q_left(q: &impl TQuaternion<F>) -> Self;
14  /// from Quaternion (qp = P4x4 q4)
15  fn from_p_right(p: &impl TQuaternion<F>) -> Self;
16  /// from Quaternion rot (qrp = P4x4 Q4x4 r4)
17  fn from_rot(q: &impl TQuaternion<F>) -> Self;
18}
19
20/// Matrix4
21pub type Matrix4<F> = [[F; 4]; 4];
22
23/// TMatrix for Matrix4
24impl<F: Float + std::fmt::Debug + std::iter::Sum> TMatrix<F> for Matrix4<F> {
25  /// constructor col major from v4 (move)
26  fn colmajor4(m: Vec<Vector4<F>>) -> Self where Self: Sized {
27    (0..4).into_iter().map(|i|
28      (0..4).into_iter().map(|j|
29        m[j][i]
30      ).collect::<Vec<_>>().try_into().unwrap()
31    ).collect::<Vec<_>>().try_into().unwrap()
32  }
33  /// constructor row major from v4 (move)
34  fn rowmajor4(m: Vec<Vector4<F>>) -> Self where Self: Sized {
35    (0..4).into_iter().map(|j|
36      m[j]
37    ).collect::<Vec<_>>().try_into().unwrap()
38  }
39  /// constructor col major
40  fn col_major(m: &Vec<Vec<F>>) -> Self {
41    (0..4).into_iter().map(|i|
42      (0..4).into_iter().map(|j|
43        m[j][i]
44      ).collect::<Vec<_>>().try_into().unwrap()
45    ).collect::<Vec<_>>().try_into().unwrap()
46  }
47  /// constructor row major
48  fn row_major(m: &Vec<Vec<F>>) -> Self {
49    (0..4).into_iter().map(|j|
50      (0..4).into_iter().map(|i|
51        m[j][i]
52      ).collect::<Vec<_>>().try_into().unwrap()
53    ).collect::<Vec<_>>().try_into().unwrap()
54  }
55  /// constructor row major
56  fn new(m: &Vec<Vec<F>>) -> Self {
57    Self::row_major(m)
58  }
59  /// constructor
60  fn identity() -> Self {
61    let o = <F>::from(0).unwrap();
62    let l = <F>::from(1).unwrap();
63    Self::new(&vec![
64      vec![l, o, o, o],
65      vec![o, l, o, o],
66      vec![o, o, l, o],
67      vec![o, o, o, l]])
68  }
69  /// check equal with precision
70  fn prec_eq(&self, e: F, m: &impl TMatrix<F>) -> bool {
71    let m = m.mev4();
72    for j in 0..4 {
73      for i in 0..4 {
74        if (self[j][i] - m[j][i]).abs() >= e { return false; }
75      }
76    }
77    true
78  }
79  /// like as slice v4
80  fn mev4(&self) -> &[Vector4<F>] {
81    self
82  }
83  /// m dot self
84  fn dot_m(&self, m: &impl TMatrix<F>) -> Self {
85    Matrix4::<F>::colmajor4((0..4).into_iter().map(|i| {
86      self.colv4(i).dot_mv(m) // m dot self.col
87    }).collect())
88  }
89  /// row to v4
90  fn rowv4(&self, j: usize) -> Vector4<F> {
91    self[j]
92//    Vector4::<F>::new(&(0..4).into_iter().map(|i| self[j][i]).collect())
93  }
94  /// col to v4
95  fn colv4(&self, i: usize) -> Vector4<F> {
96    Vector4::<F>::new(&(0..4).into_iter().map(|j| self[j][i]).collect())
97  }
98  /// to_vec
99  fn to_vec(&self) -> Vec<Vec<F>> {
100    self.iter().map(|r| r.to_vec()).collect::<Vec<_>>()
101  }
102}
103
104/// TM4 for Matrix4
105impl<F: Float + std::fmt::Debug> TM4<F> for Matrix4<F> {
106  /// from Quaternion (qp = Q4x4 p4)
107  fn from_q_left(q: &impl TQuaternion<F>) -> Self {
108    let q = q.me();
109    [
110/*
111      [q[0], -q[1], -q[2], -q[3]],
112      [q[1], q[0], -q[3], q[2]],
113      [q[2], q[3], q[0], -q[1]],
114      [q[3], -q[2], q[1], q[0]]
115*/
116      [q[0], -q[3], q[2], q[1]],
117      [q[3], q[0], -q[1], q[2]],
118      [-q[2], q[1], q[0], q[3]],
119      [-q[1], -q[2], -q[3], q[0]]
120    ]
121  }
122  /// from Quaternion (qp = P4x4 q4)
123  fn from_p_right(p: &impl TQuaternion<F>) -> Self {
124    let p = p.me();
125    [
126/*
127      [p[0], -p[1], -p[2], -p[3]],
128      [p[1], p[0], p[3], -p[2]],
129      [p[2], -p[3], p[0], p[1]],
130      [p[3], p[2], -p[1], p[0]]
131*/
132      [p[0], p[3], -p[2], p[1]],
133      [-p[3], p[0], p[1], p[2]],
134      [p[2], -p[1], p[0], p[3]],
135      [-p[1], -p[2], -p[3], p[0]]
136    ]
137  }
138  /// from Quaternion rot (qrp = P4x4 Q4x4 r4)
139  fn from_rot(q: &impl TQuaternion<F>) -> Self {
140    q.to_m4_rot()
141  }
142}
qm/m/m4.rs

qm/m/
m4.rs
