nalgebra/linalg/
inverse.rs

1use simba::scalar::ComplexField;
2
3use crate::base::allocator::Allocator;
4use crate::base::dimension::Dim;
5use crate::base::storage::{Storage, StorageMut};
6use crate::base::{DefaultAllocator, OMatrix, SquareMatrix};
7
8use crate::linalg::lu;
9
10impl<T: ComplexField, D: Dim, S: Storage<T, D, D>> SquareMatrix<T, D, S> {
11    /// Attempts to invert this square matrix.
12    ///
13    /// # Panics
14    ///
15    /// Panics if `self` isn’t a square matrix.
16    #[inline]
17    #[must_use = "Did you mean to use try_inverse_mut()?"]
18    pub fn try_inverse(self) -> Option<OMatrix<T, D, D>>
19    where
20        DefaultAllocator: Allocator<D, D>,
21    {
22        let mut me = self.into_owned();
23        if me.try_inverse_mut() { Some(me) } else { None }
24    }
25}
26
27impl<T: ComplexField, D: Dim, S: StorageMut<T, D, D>> SquareMatrix<T, D, S> {
28    /// Attempts to invert this square matrix in-place.
29    ///
30    /// Returns `true` if the inversion succeeded, `false` otherwise.
31    ///
32    /// # Behavior
33    ///
34    /// - For small dimensions (`n < 5`), the matrix is left unchanged if the inversion fails.
35    /// - For dimensions `n >= 5`, the matrix may be **partially modified** even if the inversion fails,
36    ///   because LU decomposition is used and it modifies the matrix in-place.
37    ///
38    /// If you need to preserve the original matrix regardless of success or failure,
39    /// consider using [`Self::try_inverse`] instead.
40    ///
41    /// # Panics
42    ///
43    /// Panics if `self` isn’t a square matrix.
44    #[inline]
45    pub fn try_inverse_mut(&mut self) -> bool
46    where
47        DefaultAllocator: Allocator<D, D>,
48    {
49        assert!(self.is_square(), "Unable to invert a non-square matrix.");
50
51        let dim = self.shape().0;
52
53        unsafe {
54            match dim {
55                0 => true,
56                1 => {
57                    let determinant = self.get_unchecked((0, 0)).clone();
58                    if determinant.is_zero() {
59                        false
60                    } else {
61                        *self.get_unchecked_mut((0, 0)) = T::one() / determinant;
62                        true
63                    }
64                }
65                2 => {
66                    let m11 = self.get_unchecked((0, 0)).clone();
67                    let m12 = self.get_unchecked((0, 1)).clone();
68                    let m21 = self.get_unchecked((1, 0)).clone();
69                    let m22 = self.get_unchecked((1, 1)).clone();
70
71                    let determinant = m11.clone() * m22.clone() - m21.clone() * m12.clone();
72
73                    if determinant.is_zero() {
74                        false
75                    } else {
76                        *self.get_unchecked_mut((0, 0)) = m22 / determinant.clone();
77                        *self.get_unchecked_mut((0, 1)) = -m12 / determinant.clone();
78
79                        *self.get_unchecked_mut((1, 0)) = -m21 / determinant.clone();
80                        *self.get_unchecked_mut((1, 1)) = m11 / determinant;
81
82                        true
83                    }
84                }
85                3 => {
86                    let m11 = self.get_unchecked((0, 0)).clone();
87                    let m12 = self.get_unchecked((0, 1)).clone();
88                    let m13 = self.get_unchecked((0, 2)).clone();
89
90                    let m21 = self.get_unchecked((1, 0)).clone();
91                    let m22 = self.get_unchecked((1, 1)).clone();
92                    let m23 = self.get_unchecked((1, 2)).clone();
93
94                    let m31 = self.get_unchecked((2, 0)).clone();
95                    let m32 = self.get_unchecked((2, 1)).clone();
96                    let m33 = self.get_unchecked((2, 2)).clone();
97
98                    let minor_m12_m23 = m22.clone() * m33.clone() - m32.clone() * m23.clone();
99                    let minor_m11_m23 = m21.clone() * m33.clone() - m31.clone() * m23.clone();
100                    let minor_m11_m22 = m21.clone() * m32.clone() - m31.clone() * m22.clone();
101
102                    let determinant = m11.clone() * minor_m12_m23.clone()
103                        - m12.clone() * minor_m11_m23.clone()
104                        + m13.clone() * minor_m11_m22.clone();
105
106                    if determinant.is_zero() {
107                        false
108                    } else {
109                        *self.get_unchecked_mut((0, 0)) = minor_m12_m23 / determinant.clone();
110                        *self.get_unchecked_mut((0, 1)) = (m13.clone() * m32.clone()
111                            - m33.clone() * m12.clone())
112                            / determinant.clone();
113                        *self.get_unchecked_mut((0, 2)) = (m12.clone() * m23.clone()
114                            - m22.clone() * m13.clone())
115                            / determinant.clone();
116
117                        *self.get_unchecked_mut((1, 0)) = -minor_m11_m23 / determinant.clone();
118                        *self.get_unchecked_mut((1, 1)) =
119                            (m11.clone() * m33 - m31.clone() * m13.clone()) / determinant.clone();
120                        *self.get_unchecked_mut((1, 2)) =
121                            (m13 * m21.clone() - m23 * m11.clone()) / determinant.clone();
122
123                        *self.get_unchecked_mut((2, 0)) = minor_m11_m22 / determinant.clone();
124                        *self.get_unchecked_mut((2, 1)) =
125                            (m12.clone() * m31 - m32 * m11.clone()) / determinant.clone();
126                        *self.get_unchecked_mut((2, 2)) = (m11 * m22 - m21 * m12) / determinant;
127
128                        true
129                    }
130                }
131                4 => {
132                    let oself = self.clone_owned();
133                    do_inverse4(&oself, self)
134                }
135                _ => {
136                    let oself = self.clone_owned();
137                    lu::try_invert_to(oself, self)
138                }
139            }
140        }
141    }
142}
143
144// NOTE: this is an extremely efficient, loop-unrolled matrix inverse from MESA (MIT licensed).
145fn do_inverse4<T: ComplexField, D: Dim, S: StorageMut<T, D, D>>(
146    m: &OMatrix<T, D, D>,
147    out: &mut SquareMatrix<T, D, S>,
148) -> bool
149where
150    DefaultAllocator: Allocator<D, D>,
151{
152    let m = m.as_slice();
153
154    let cofactor00 = m[5].clone() * m[10].clone() * m[15].clone()
155        - m[5].clone() * m[11].clone() * m[14].clone()
156        - m[9].clone() * m[6].clone() * m[15].clone()
157        + m[9].clone() * m[7].clone() * m[14].clone()
158        + m[13].clone() * m[6].clone() * m[11].clone()
159        - m[13].clone() * m[7].clone() * m[10].clone();
160
161    let cofactor01 = -m[4].clone() * m[10].clone() * m[15].clone()
162        + m[4].clone() * m[11].clone() * m[14].clone()
163        + m[8].clone() * m[6].clone() * m[15].clone()
164        - m[8].clone() * m[7].clone() * m[14].clone()
165        - m[12].clone() * m[6].clone() * m[11].clone()
166        + m[12].clone() * m[7].clone() * m[10].clone();
167
168    let cofactor02 = m[4].clone() * m[9].clone() * m[15].clone()
169        - m[4].clone() * m[11].clone() * m[13].clone()
170        - m[8].clone() * m[5].clone() * m[15].clone()
171        + m[8].clone() * m[7].clone() * m[13].clone()
172        + m[12].clone() * m[5].clone() * m[11].clone()
173        - m[12].clone() * m[7].clone() * m[9].clone();
174
175    let cofactor03 = -m[4].clone() * m[9].clone() * m[14].clone()
176        + m[4].clone() * m[10].clone() * m[13].clone()
177        + m[8].clone() * m[5].clone() * m[14].clone()
178        - m[8].clone() * m[6].clone() * m[13].clone()
179        - m[12].clone() * m[5].clone() * m[10].clone()
180        + m[12].clone() * m[6].clone() * m[9].clone();
181
182    let det = m[0].clone() * cofactor00.clone()
183        + m[1].clone() * cofactor01.clone()
184        + m[2].clone() * cofactor02.clone()
185        + m[3].clone() * cofactor03.clone();
186
187    if det.is_zero() {
188        return false;
189    }
190    out[(0, 0)] = cofactor00;
191
192    out[(1, 0)] = -m[1].clone() * m[10].clone() * m[15].clone()
193        + m[1].clone() * m[11].clone() * m[14].clone()
194        + m[9].clone() * m[2].clone() * m[15].clone()
195        - m[9].clone() * m[3].clone() * m[14].clone()
196        - m[13].clone() * m[2].clone() * m[11].clone()
197        + m[13].clone() * m[3].clone() * m[10].clone();
198
199    out[(2, 0)] = m[1].clone() * m[6].clone() * m[15].clone()
200        - m[1].clone() * m[7].clone() * m[14].clone()
201        - m[5].clone() * m[2].clone() * m[15].clone()
202        + m[5].clone() * m[3].clone() * m[14].clone()
203        + m[13].clone() * m[2].clone() * m[7].clone()
204        - m[13].clone() * m[3].clone() * m[6].clone();
205
206    out[(3, 0)] = -m[1].clone() * m[6].clone() * m[11].clone()
207        + m[1].clone() * m[7].clone() * m[10].clone()
208        + m[5].clone() * m[2].clone() * m[11].clone()
209        - m[5].clone() * m[3].clone() * m[10].clone()
210        - m[9].clone() * m[2].clone() * m[7].clone()
211        + m[9].clone() * m[3].clone() * m[6].clone();
212
213    out[(0, 1)] = cofactor01;
214
215    out[(1, 1)] = m[0].clone() * m[10].clone() * m[15].clone()
216        - m[0].clone() * m[11].clone() * m[14].clone()
217        - m[8].clone() * m[2].clone() * m[15].clone()
218        + m[8].clone() * m[3].clone() * m[14].clone()
219        + m[12].clone() * m[2].clone() * m[11].clone()
220        - m[12].clone() * m[3].clone() * m[10].clone();
221
222    out[(2, 1)] = -m[0].clone() * m[6].clone() * m[15].clone()
223        + m[0].clone() * m[7].clone() * m[14].clone()
224        + m[4].clone() * m[2].clone() * m[15].clone()
225        - m[4].clone() * m[3].clone() * m[14].clone()
226        - m[12].clone() * m[2].clone() * m[7].clone()
227        + m[12].clone() * m[3].clone() * m[6].clone();
228
229    out[(3, 1)] = m[0].clone() * m[6].clone() * m[11].clone()
230        - m[0].clone() * m[7].clone() * m[10].clone()
231        - m[4].clone() * m[2].clone() * m[11].clone()
232        + m[4].clone() * m[3].clone() * m[10].clone()
233        + m[8].clone() * m[2].clone() * m[7].clone()
234        - m[8].clone() * m[3].clone() * m[6].clone();
235
236    out[(0, 2)] = cofactor02;
237
238    out[(1, 2)] = -m[0].clone() * m[9].clone() * m[15].clone()
239        + m[0].clone() * m[11].clone() * m[13].clone()
240        + m[8].clone() * m[1].clone() * m[15].clone()
241        - m[8].clone() * m[3].clone() * m[13].clone()
242        - m[12].clone() * m[1].clone() * m[11].clone()
243        + m[12].clone() * m[3].clone() * m[9].clone();
244
245    out[(2, 2)] = m[0].clone() * m[5].clone() * m[15].clone()
246        - m[0].clone() * m[7].clone() * m[13].clone()
247        - m[4].clone() * m[1].clone() * m[15].clone()
248        + m[4].clone() * m[3].clone() * m[13].clone()
249        + m[12].clone() * m[1].clone() * m[7].clone()
250        - m[12].clone() * m[3].clone() * m[5].clone();
251
252    out[(0, 3)] = cofactor03;
253
254    out[(3, 2)] = -m[0].clone() * m[5].clone() * m[11].clone()
255        + m[0].clone() * m[7].clone() * m[9].clone()
256        + m[4].clone() * m[1].clone() * m[11].clone()
257        - m[4].clone() * m[3].clone() * m[9].clone()
258        - m[8].clone() * m[1].clone() * m[7].clone()
259        + m[8].clone() * m[3].clone() * m[5].clone();
260
261    out[(1, 3)] = m[0].clone() * m[9].clone() * m[14].clone()
262        - m[0].clone() * m[10].clone() * m[13].clone()
263        - m[8].clone() * m[1].clone() * m[14].clone()
264        + m[8].clone() * m[2].clone() * m[13].clone()
265        + m[12].clone() * m[1].clone() * m[10].clone()
266        - m[12].clone() * m[2].clone() * m[9].clone();
267
268    out[(2, 3)] = -m[0].clone() * m[5].clone() * m[14].clone()
269        + m[0].clone() * m[6].clone() * m[13].clone()
270        + m[4].clone() * m[1].clone() * m[14].clone()
271        - m[4].clone() * m[2].clone() * m[13].clone()
272        - m[12].clone() * m[1].clone() * m[6].clone()
273        + m[12].clone() * m[2].clone() * m[5].clone();
274
275    out[(3, 3)] = m[0].clone() * m[5].clone() * m[10].clone()
276        - m[0].clone() * m[6].clone() * m[9].clone()
277        - m[4].clone() * m[1].clone() * m[10].clone()
278        + m[4].clone() * m[2].clone() * m[9].clone()
279        + m[8].clone() * m[1].clone() * m[6].clone()
280        - m[8].clone() * m[2].clone() * m[5].clone();
281
282    let inv_det = T::one() / det;
283
284    for j in 0..4 {
285        for i in 0..4 {
286            out[(i, j)] *= inv_det.clone();
287        }
288    }
289    true
290}
nalgebra/linalg/inverse.rs

nalgebra/linalg/
inverse.rs
