1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
10 * @brief Compute the inverse of a matrix by LU factorization.
13 #ifndef matrix_inverse_h
14 #define matrix_inverse_h
16 #include <cml/matrix/lu.h>
21 /* Need to use a functional, since template functions cannot be
22 * specialized. _tag is used to specialize based upon dimension:
24 template<typename MatT
, int _tag
> struct inverse_f
;
26 /* @todo: Reciprocal optimization for division by determinant.
29 /* 2x2 inverse. Despite being marked for fixed_size matrices, this can
30 * be used for dynamic-sized ones also:
32 template<typename MatT
>
33 struct inverse_f
<MatT
,2>
35 typename
MatT::temporary_type
operator()(const MatT
& M
) const
37 typedef typename
MatT::temporary_type temporary_type
;
38 typedef typename
temporary_type::value_type value_type
;
40 /* Matrix containing the inverse: */
42 cml::et::detail::Resize(Z
,2,2);
44 /* Compute determinant and inverse: */
45 value_type D
= value_type(1) / (M(0,0)*M(1,1) - M(0,1)*M(1,0));
46 Z(0,0) = M(1,1)*D
; Z(0,1) = - M(0,1)*D
;
47 Z(1,0) = - M(1,0)*D
; Z(1,1) = M(0,0)*D
;
53 /* 3x3 inverse. Despite being marked for fixed_size matrices, this can
54 * be used for dynamic-sized ones also:
56 template<typename MatT
>
57 struct inverse_f
<MatT
,3>
63 typename
MatT::temporary_type
operator()(const MatT
& M
) const
66 typedef typename
MatT::value_type value_type
;
68 /* Compute cofactors for each entry: */
69 value_type m_00
= M(1,1)*M(2,2) - M(1,2)*M(2,1);
70 value_type m_01
= M(1,2)*M(2,0) - M(1,0)*M(2,2);
71 value_type m_02
= M(1,0)*M(2,1) - M(1,1)*M(2,0);
73 value_type m_10
= M(0,2)*M(2,1) - M(0,1)*M(2,2);
74 value_type m_11
= M(0,0)*M(2,2) - M(0,2)*M(2,0);
75 value_type m_12
= M(0,1)*M(2,0) - M(0,0)*M(2,1);
77 value_type m_20
= M(0,1)*M(1,2) - M(0,2)*M(1,1);
78 value_type m_21
= M(0,2)*M(1,0) - M(0,0)*M(1,2);
79 value_type m_22
= M(0,0)*M(1,1) - M(0,1)*M(1,0);
81 /* Compute determinant from the minors: */
83 value_type(1) / (M(0,0)*m_00
+ M(0,1)*m_01
+ M(0,2)*m_02
);
85 /* Matrix containing the inverse: */
86 typename
MatT::temporary_type Z
;
87 cml::et::detail::Resize(Z
,3,3);
89 /* Assign the inverse as (1/D) * (cofactor matrix)^T: */
90 Z(0,0) = m_00
*D
; Z(0,1) = m_10
*D
; Z(0,2) = m_20
*D
;
91 Z(1,0) = m_01
*D
; Z(1,1) = m_11
*D
; Z(1,2) = m_21
*D
;
92 Z(2,0) = m_02
*D
; Z(2,1) = m_12
*D
; Z(2,2) = m_22
*D
;
98 /* 4x4 inverse. Despite being marked for fixed_size matrices, this can
99 * be used for dynamic-sized ones also:
101 template<typename MatT
>
102 struct inverse_f
<MatT
,4>
109 * |11 12 13| |10 12 13|
110 * C00 = |21 22 23| C01 = |20 22 23|
111 * |31 32 33| |30 32 33|
113 * |10 11 13| |10 11 12|
114 * C02 = |20 21 23| C03 = |20 21 22|
115 * |30 31 33| |30 31 32|
117 typename
MatT::temporary_type
operator()(const MatT
& M
) const
120 typedef typename
MatT::value_type value_type
;
122 /* Common cofactors, rows 0,1: */
123 value_type m_22_33_23_32
= M(2,2)*M(3,3) - M(2,3)*M(3,2);
124 value_type m_23_30_20_33
= M(2,3)*M(3,0) - M(2,0)*M(3,3);
125 value_type m_20_31_21_30
= M(2,0)*M(3,1) - M(2,1)*M(3,0);
126 value_type m_21_32_22_31
= M(2,1)*M(3,2) - M(2,2)*M(3,1);
127 value_type m_23_31_21_33
= M(2,3)*M(3,1) - M(2,1)*M(3,3);
128 value_type m_20_32_22_30
= M(2,0)*M(3,2) - M(2,2)*M(3,0);
130 /* Compute minors: */
132 = M(1,1)*m_22_33_23_32
+M(1,2)*m_23_31_21_33
+M(1,3)*m_21_32_22_31
;
135 = M(1,0)*m_22_33_23_32
+M(1,2)*m_23_30_20_33
+M(1,3)*m_20_32_22_30
;
138 = M(1,0)*-m_23_31_21_33
+M(1,1)*m_23_30_20_33
+M(1,3)*m_20_31_21_30
;
141 = M(1,0)*m_21_32_22_31
+M(1,1)*-m_20_32_22_30
+M(1,2)*m_20_31_21_30
;
143 /* Compute minors: */
145 = M(0,1)*m_22_33_23_32
+M(0,2)*m_23_31_21_33
+M(0,3)*m_21_32_22_31
;
148 = M(0,0)*m_22_33_23_32
+M(0,2)*m_23_30_20_33
+M(0,3)*m_20_32_22_30
;
151 = M(0,0)*-m_23_31_21_33
+M(0,1)*m_23_30_20_33
+M(0,3)*m_20_31_21_30
;
154 = M(0,0)*m_21_32_22_31
+M(0,1)*-m_20_32_22_30
+M(0,2)*m_20_31_21_30
;
156 /* Common cofactors, rows 2,3: */
157 value_type m_02_13_03_12
= M(0,2)*M(1,3) - M(0,3)*M(1,2);
158 value_type m_03_10_00_13
= M(0,3)*M(1,0) - M(0,0)*M(1,3);
159 value_type m_00_11_01_10
= M(0,0)*M(1,1) - M(0,1)*M(1,0);
160 value_type m_01_12_02_11
= M(0,1)*M(1,2) - M(0,2)*M(1,1);
161 value_type m_03_11_01_13
= M(0,3)*M(1,1) - M(0,1)*M(1,3);
162 value_type m_00_12_02_10
= M(0,0)*M(1,2) - M(0,2)*M(1,0);
164 /* Compute minors (uses row 3 as the multipliers instead of row 0,
165 * which uses the same signs as row 0):
168 = M(3,1)*m_02_13_03_12
+M(3,2)*m_03_11_01_13
+M(3,3)*m_01_12_02_11
;
171 = M(3,0)*m_02_13_03_12
+M(3,2)*m_03_10_00_13
+M(3,3)*m_00_12_02_10
;
174 = M(3,0)*-m_03_11_01_13
+M(3,1)*m_03_10_00_13
+M(3,3)*m_00_11_01_10
;
177 = M(3,0)*m_01_12_02_11
+M(3,1)*-m_00_12_02_10
+M(3,2)*m_00_11_01_10
;
179 /* Compute minors: */
181 = M(2,1)*m_02_13_03_12
+M(2,2)*m_03_11_01_13
+M(2,3)*m_01_12_02_11
;
184 = M(2,0)*m_02_13_03_12
+M(2,2)*m_03_10_00_13
+M(2,3)*m_00_12_02_10
;
187 = M(2,0)*-m_03_11_01_13
+M(2,1)*m_03_10_00_13
+M(2,3)*m_00_11_01_10
;
190 = M(2,0)*m_01_12_02_11
+M(2,1)*-m_00_12_02_10
+M(2,2)*m_00_11_01_10
;
192 /* Finally, compute determinant from the minors, and assign the
193 * inverse as (1/D) * (cofactor matrix)^T:
195 typename
MatT::temporary_type Z
;
196 cml::et::detail::Resize(Z
,4,4);
198 value_type D
= value_type(1) /
199 (M(0,0)*d00
- M(0,1)*d01
+ M(0,2)*d02
- M(0,3)*d03
);
200 Z(0,0) = +d00
*D
; Z(0,1) = -d10
*D
; Z(0,2) = +d20
*D
; Z(0,3) = -d30
*D
;
201 Z(1,0) = -d01
*D
; Z(1,1) = +d11
*D
; Z(1,2) = -d21
*D
; Z(1,3) = +d31
*D
;
202 Z(2,0) = +d02
*D
; Z(2,1) = -d12
*D
; Z(2,2) = +d22
*D
; Z(2,3) = -d32
*D
;
203 Z(3,0) = -d03
*D
; Z(3,1) = +d13
*D
; Z(3,2) = -d23
*D
; Z(3,3) = +d33
*D
;
209 /* If more extensive general linear algebra functionality is offered in
210 * future versions it may be useful to make the elementary row and column
211 * operations separate functions. For now they're simply performed in place,
212 * but the commented-out lines of code show where the calls to these functions
213 * should go if and when they become available.
216 /* @todo: In-place version, and address memory allocation for pivot vector.
219 /* General NxN inverse by Gauss-Jordan elimination with full pivoting: */
220 template<typename MatT
, int _tag
>
223 typename
MatT::temporary_type
operator()(const MatT
& M
) const
226 typedef typename
MatT::value_type value_type
;
231 /* Matrix containing the inverse: */
232 typename
MatT::temporary_type Z
;
233 cml::et::detail::Resize(Z
,N
,N
);
236 /* For tracking pivots */
237 std::vector
<size_t> row_index(N
);
238 std::vector
<size_t> col_index(N
);
239 std::vector
<size_t> pivoted(N
,0);
241 /* For each column */
242 for (size_t i
= 0; i
< N
; ++i
) {
245 size_t row
= 0, col
= 0;
246 value_type max
= value_type(0);
247 for (size_t j
= 0; j
< N
; ++j
) {
249 for (size_t k
= 0; k
< N
; ++k
) {
251 value_type mag
= std::fabs(Z(j
,k
));
262 /* TODO: Check max against epsilon here to catch singularity */
267 /* Swap rows if necessary */
269 /*Z.row_op_swap(row,col);*/
270 for (size_t j
= 0; j
< Z
.cols(); ++j
) {
271 std::swap(Z(row
,j
),Z(col
,j
));
275 /* Process pivot row */
277 value_type pivot
= Z(col
,col
);
278 Z(col
,col
) = value_type(1);
279 /*Z.row_op_mult(col,value_type(1)/pivot);*/
280 value_type k
= value_type(1)/pivot
;
281 for (size_t j
= 0; j
< Z
.cols(); ++j
) {
285 /* Process other rows */
286 for (size_t j
= 0; j
< N
; ++j
) {
288 value_type mult
= -Z(j
,col
);
289 Z(j
,col
) = value_type(0);
290 /*Z.row_op_add_mult(col,j,mult);*/
291 for (size_t k
= 0; k
< Z
.cols(); ++k
) {
292 Z(j
,k
) += mult
* Z(col
,k
);
298 /* Swap columns if necessary */
299 for (int i
= N
-1; i
>= 0; --i
) {
300 if (row_index
[i
] != col_index
[i
]) {
301 /*Z.col_op_swap(row_index[i],col_index[i]);*/
302 for (size_t j
= 0; j
< Z
.rows(); ++j
) {
303 std::swap(Z(j
,row_index
[i
]),Z(j
,col_index
[i
]));
313 /* Inversion by LU factorization is turned off for now due to lack of
314 * pivoting in the implementation, but we may switch back to it at some future
320 /* General NxN inverse by LU factorization: */
321 template<typename MatT
, int _tag
>
324 typename
MatT::temporary_type
operator()(const MatT
& M
) const
327 typedef typename
MatT::value_type value_type
;
329 /* Compute LU factorization: */
331 typename
MatT::temporary_type LU
;
332 cml::et::detail::Resize(LU
,N
,N
);
335 /* Matrix containing the inverse: */
336 typename
MatT::temporary_type Z
;
337 cml::et::detail::Resize(Z
,N
,N
);
339 typename
MatT::col_vector_type v
, x
;
340 cml::et::detail::Resize(v
,N
);
341 cml::et::detail::Resize(x
,N
);
342 for(size_t i
= 0; i
< N
; ++i
)
343 v
[i
] = value_type(0);
344 /* XXX Need a fill() function here. */
346 /* Use lu_solve to solve M*x = v for x, where v = [0 ... 1 ... 0]^T: */
347 for(size_t i
= 0; i
< N
; ++i
) {
351 /* x is column i of the inverse of LU: */
352 for(size_t k
= 0; k
< N
; ++ k
) {
365 /* Note: force_NxN is for checking general NxN inversion against the special-
366 * case 2x2, 3x3 and 4x4 code. I'm leaving it in for now since we may need to
367 * test the NxN code further if the implementation changes. At some future
368 * time when the implementation is stable, everything related to force_NxN can
372 /* Note: Commenting the force_NxN stuff out, but leaving the code here in
373 * case we need to do more testing in the future.
376 /* Generator for the inverse functional for fixed-size matrices: */
377 template<typename MatT
> typename
MatT::temporary_type
378 inverse(const MatT
& M
, fixed_size_tag
/*, bool force_NxN*/)
380 /* Require a square matrix: */
381 cml::et::CheckedSquare(M
, fixed_size_tag());
385 return inverse_f<MatT,0>()(M);
388 return inverse_f
<MatT
,MatT::array_rows
>()(M
);
394 /* Generator for the inverse functional for dynamic-size matrices: */
395 template<typename MatT
> typename
MatT::temporary_type
396 inverse(const MatT
& M
, dynamic_size_tag
/*, bool force_NxN*/)
398 /* Require a square matrix: */
399 cml::et::CheckedSquare(M
, dynamic_size_tag());
403 return inverse_f<MatT,0>()(M);
406 /* Dispatch based upon the matrix dimension: */
408 case 2: return inverse_f
<MatT
,2>()(M
); // 2x2
409 case 3: return inverse_f
<MatT
,3>()(M
); // 3x3
410 case 4: return inverse_f
<MatT
,4>()(M
); // 4x4
411 default: return inverse_f
<MatT
,0>()(M
); // > 4x4 (or 1x1)
418 } // namespace detail
420 /** Inverse of a matrix. */
421 template<typename E
, class AT
, typename BO
, typename L
> inline
422 typename matrix
<E
,AT
,BO
,L
>::temporary_type
423 inverse(const matrix
<E
,AT
,BO
,L
>& M
/*, bool force_NxN = false*/)
425 typedef typename matrix
<E
,AT
,BO
,L
>::size_tag size_tag
;
426 return detail::inverse(M
,size_tag()/*,force_NxN*/);
429 /** Inverse of a matrix expression. */
430 template<typename XprT
> inline
431 typename
et::MatrixXpr
<XprT
>::temporary_type
432 inverse(const et::MatrixXpr
<XprT
>& e
/*, bool force_NxN = false*/)
434 typedef typename
et::MatrixXpr
<XprT
>::size_tag size_tag
;
435 return detail::inverse(e
,size_tag()/*,force_NxN*/);
442 // -------------------------------------------------------------------------