1use crate::small_linalg::{Matrix2, Matrix3x2, Vector2};
2use crate::{small_linalg::Vector3, PhysicsQuantity};
3#[cfg(feature = "ndarray")]
4use crate::{LinalgError, LinalgResult, LinalgShape};
5use core::ops::{Add, Div, Index, IndexMut, Mul, Neg, Sub};
6#[cfg(feature = "ndarray")]
7use ndarray::{Array2, ArrayView2};
8
9#[derive(Clone, Copy, PartialEq, Debug)]
10pub struct Matrix2x3<T: PhysicsQuantity> {
11 pub data: [[T; 3]; 2],
12}
13
14impl<T: PhysicsQuantity> Matrix2x3<T> {
15 pub fn new(data: [[T; 3]; 2]) -> Self {
16 Self { data }
17 }
18
19 pub fn zero() -> Self {
20 Self {
21 data: [[T::zero(); 3]; 2],
22 }
23 }
24
25 pub fn from_f64(data: [[f64; 3]; 2]) -> Self {
26 let mut converted = [[T::zero(); 3]; 2];
27 for i in 0..2 {
28 for j in 0..3 {
29 converted[i][j] = T::from_raw(data[i][j]);
30 }
31 }
32 Self::new(converted)
33 }
34
35 pub fn as_f64(&self) -> Matrix2x3<f64> {
36 Matrix2x3::new([
37 [
38 self.data[0][0].as_f64(),
39 self.data[0][1].as_f64(),
40 self.data[0][2].as_f64(),
41 ],
42 [
43 self.data[1][0].as_f64(),
44 self.data[1][1].as_f64(),
45 self.data[1][2].as_f64(),
46 ],
47 ])
48 }
49
50 pub fn transpose(&self) -> super::matrix3x2::Matrix3x2<T> {
51 super::matrix3x2::Matrix3x2::new([
52 [self.data[0][0], self.data[1][0]],
53 [self.data[0][1], self.data[1][1]],
54 [self.data[0][2], self.data[1][2]],
55 ])
56 }
57
58 pub fn row(&self, i: usize) -> Vector3<T> {
59 assert!(i < 2);
60 Vector3::new(self.data[i])
61 }
62
63 pub fn column(&self, i: usize) -> Vector2<T> {
64 assert!(i < 3);
65 Vector2::new([self.data[0][i], self.data[1][i]])
66 }
67
68 pub fn optimize(&mut self) {
69 for row in &mut self.data {
70 for value in row {
71 value.optimize();
72 }
73 }
74 }
75
76 pub fn abs(&self) -> Self {
77 Self {
78 data: [
79 [
80 self.data[0][0].abs(),
81 self.data[0][1].abs(),
82 self.data[0][2].abs(),
83 ],
84 [
85 self.data[1][0].abs(),
86 self.data[1][1].abs(),
87 self.data[1][2].abs(),
88 ],
89 ],
90 }
91 }
92
93 #[cfg(feature = "ndarray")]
94 pub fn to_ndarray(&self) -> LinalgResult<Array2<T>> {
95 Array2::from_shape_vec(
96 (2, 3),
97 vec![
98 self.data[0][0],
99 self.data[0][1],
100 self.data[0][2],
101 self.data[1][0],
102 self.data[1][1],
103 self.data[1][2],
104 ],
105 )
106 .map_err(|_| LinalgError::ShapeConstructionFailed)
107 }
108
109 #[cfg(feature = "ndarray")]
110 pub fn from_ndarray(array: ArrayView2<T>) -> LinalgResult<Self> {
111 if array.shape() != [2, 3] {
112 return Err(LinalgError::ShapeMismatch {
113 expected: LinalgShape::Matrix { rows: 2, cols: 3 },
114 actual: LinalgShape::Matrix {
115 rows: array.nrows(),
116 cols: array.ncols(),
117 },
118 });
119 }
120
121 Ok(Self::new([
122 [array[[0, 0]], array[[0, 1]], array[[0, 2]]],
123 [array[[1, 0]], array[[1, 1]], array[[1, 2]]],
124 ]))
125 }
126}
127
128impl<T: PhysicsQuantity + Neg<Output = T>> Neg for Matrix2x3<T> {
129 type Output = Self;
130 fn neg(self) -> Self {
131 Self::new([
132 [-self.data[0][0], -self.data[0][1], -self.data[0][2]],
133 [-self.data[1][0], -self.data[1][1], -self.data[1][2]],
134 ])
135 }
136}
137
138impl<T: PhysicsQuantity + Add<Output = T>> Add for Matrix2x3<T> {
139 type Output = Self;
140
141 fn add(self, rhs: Self) -> Self {
142 let mut out = [[T::zero(); 3]; 2];
143 for (i, row) in out.iter_mut().enumerate() {
144 for (j, val) in row.iter_mut().enumerate() {
145 *val = self.data[i][j] + rhs.data[i][j];
146 }
147 }
148 Self::new(out)
149 }
150}
151
152impl<T: PhysicsQuantity + Sub<Output = T>> Sub for Matrix2x3<T> {
153 type Output = Self;
154
155 fn sub(self, rhs: Self) -> Self {
156 let mut out = [[T::zero(); 3]; 2];
157 for (i, row) in out.iter_mut().enumerate() {
158 for (j, val) in row.iter_mut().enumerate() {
159 *val = self.data[i][j] - rhs.data[i][j];
160 }
161 }
162 Self::new(out)
163 }
164}
165
166impl<T: PhysicsQuantity> Index<(usize, usize)> for Matrix2x3<T> {
167 type Output = T;
168 fn index(&self, idx: (usize, usize)) -> &Self::Output {
169 &self.data[idx.0][idx.1]
170 }
171}
172
173impl<T: PhysicsQuantity> IndexMut<(usize, usize)> for Matrix2x3<T> {
174 fn index_mut(&mut self, idx: (usize, usize)) -> &mut Self::Output {
175 &mut self.data[idx.0][idx.1]
176 }
177}
178
179impl<T: PhysicsQuantity> Matrix2x3<T> {
180 pub fn dot_vector3<U, V>(&self, vec: &Vector3<U>) -> Vector2<V>
181 where
182 U: PhysicsQuantity,
183 V: PhysicsQuantity + Add<Output = V>,
184 T: Mul<U, Output = V>,
185 {
186 let r0 = self.data[0][0] * vec[0] + self.data[0][1] * vec[1] + self.data[0][2] * vec[2];
187 let r1 = self.data[1][0] * vec[0] + self.data[1][1] * vec[1] + self.data[1][2] * vec[2];
188 Vector2::new([r0, r1])
189 }
190}
191
192impl<T, U, V> Mul<Vector3<U>> for Matrix2x3<T>
193where
194 T: PhysicsQuantity + Mul<U, Output = V>,
195 U: PhysicsQuantity,
196 V: PhysicsQuantity + Add<Output = V>,
197{
198 type Output = Vector2<V>;
199
200 fn mul(self, rhs: Vector3<U>) -> Vector2<V> {
201 self.dot_vector3(&rhs)
202 }
203}
204
205impl<T, U, V> Mul<Matrix2x3<U>> for Matrix2<T>
206where
207 T: PhysicsQuantity + Mul<U, Output = V>,
208 U: PhysicsQuantity,
209 V: PhysicsQuantity + Add<Output = V>,
210{
211 type Output = Matrix2x3<V>;
212
213 fn mul(self, rhs: Matrix2x3<U>) -> Self::Output {
214 let mut result = [[V::zero(); 3]; 2];
215 for i in 0..2 {
216 for j in 0..3 {
217 result[i][j] = self[(i, 0)] * rhs[(0, j)] + self[(i, 1)] * rhs[(1, j)];
218 }
219 }
220 Matrix2x3::new(result)
221 }
222}
223
224impl<T, U, V> Mul<Matrix3x2<U>> for Matrix2x3<T>
225where
226 T: PhysicsQuantity + Mul<U, Output = V>,
227 U: PhysicsQuantity,
228 V: PhysicsQuantity + Add<Output = V>,
229{
230 type Output = Matrix2<V>;
231
232 fn mul(self, rhs: Matrix3x2<U>) -> Self::Output {
233 let mut result = [[V::zero(); 2]; 2];
234 for i in 0..2 {
235 for j in 0..2 {
236 result[i][j] = self[(i, 0)] * rhs[(0, j)]
237 + self[(i, 1)] * rhs[(1, j)]
238 + self[(i, 2)] * rhs[(2, j)];
239 }
240 }
241 Matrix2::new(result)
242 }
243}
244
245impl<T> Matrix2x3<T>
246where
247 T: PhysicsQuantity + Copy,
248{
249 pub fn pseudoinverse<U, D, E, V>(&self) -> Option<Matrix3x2<V>>
250 where
251 T: Mul<T, Output = U>,
252 U: PhysicsQuantity + Copy + Mul<U, Output = D> + Div<D, Output = E>,
253
254 D: PhysicsQuantity + Copy + Sub<Output = D>,
255 E: PhysicsQuantity + Copy,
256
257 T: Mul<E, Output = V>,
258 V: PhysicsQuantity + Copy + Add<Output = V>,
259 {
260 let a_t: Matrix3x2<T> = self.transpose();
261 let a_at: Matrix2<U> = *self * a_t;
262
263 let det: D = a_at[(0, 0)] * a_at[(1, 1)] - a_at[(0, 1)] * a_at[(1, 0)];
264
265 if det.is_zero() {
266 return None;
267 }
268
269 let inv_00: E = a_at[(1, 1)] / det;
270 let inv_01: E = -a_at[(0, 1)] / det;
271 let inv_10: E = -a_at[(1, 0)] / det;
272 let inv_11: E = a_at[(0, 0)] / det;
273
274 let inv = Matrix2::new([[inv_00, inv_01], [inv_10, inv_11]]);
275
276 let pinv: Matrix3x2<V> = a_t * inv;
277
278 Some(pinv)
279 }
280}