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
17 #include <cml/matrix/lu.h>
22 /* Need to use a functional, since template functions cannot be
23 * specialized. _tag is used to specialize based upon dimension:
25 template<typename MatT
, int _tag
> struct inverse_f
;
27 /* @todo: Reciprocal optimization for division by determinant.
30 /* 2x2 inverse. Despite being marked for fixed_size matrices, this can
31 * be used for dynamic-sized ones also:
33 template<typename MatT
>
34 struct inverse_f
<MatT
,2>
36 typename
MatT::temporary_type
operator()(const MatT
& M
) const
38 typedef typename
MatT::temporary_type temporary_type
;
39 typedef typename
temporary_type::value_type value_type
;
41 /* Matrix containing the inverse: */
43 cml::et::detail::Resize(Z
,2,2);
45 /* Compute determinant and inverse: */
46 value_type D
= value_type(1) / (M(0,0)*M(1,1) - M(0,1)*M(1,0));
47 Z(0,0) = M(1,1)*D
; Z(0,1) = - M(0,1)*D
;
48 Z(1,0) = - M(1,0)*D
; Z(1,1) = M(0,0)*D
;
54 /* 3x3 inverse. Despite being marked for fixed_size matrices, this can
55 * be used for dynamic-sized ones also:
57 template<typename MatT
>
58 struct inverse_f
<MatT
,3>
64 typename
MatT::temporary_type
operator()(const MatT
& M
) const
67 typedef typename
MatT::value_type value_type
;
69 /* Compute cofactors for each entry: */
70 value_type m_00
= M(1,1)*M(2,2) - M(1,2)*M(2,1);
71 value_type m_01
= M(1,2)*M(2,0) - M(1,0)*M(2,2);
72 value_type m_02
= M(1,0)*M(2,1) - M(1,1)*M(2,0);
74 value_type m_10
= M(0,2)*M(2,1) - M(0,1)*M(2,2);
75 value_type m_11
= M(0,0)*M(2,2) - M(0,2)*M(2,0);
76 value_type m_12
= M(0,1)*M(2,0) - M(0,0)*M(2,1);
78 value_type m_20
= M(0,1)*M(1,2) - M(0,2)*M(1,1);
79 value_type m_21
= M(0,2)*M(1,0) - M(0,0)*M(1,2);
80 value_type m_22
= M(0,0)*M(1,1) - M(0,1)*M(1,0);
82 /* Compute determinant from the minors: */
84 value_type(1) / (M(0,0)*m_00
+ M(0,1)*m_01
+ M(0,2)*m_02
);
86 /* Matrix containing the inverse: */
87 typename
MatT::temporary_type Z
;
88 cml::et::detail::Resize(Z
,3,3);
90 /* Assign the inverse as (1/D) * (cofactor matrix)^T: */
91 Z(0,0) = m_00
*D
; Z(0,1) = m_10
*D
; Z(0,2) = m_20
*D
;
92 Z(1,0) = m_01
*D
; Z(1,1) = m_11
*D
; Z(1,2) = m_21
*D
;
93 Z(2,0) = m_02
*D
; Z(2,1) = m_12
*D
; Z(2,2) = m_22
*D
;
99 /* 4x4 inverse. Despite being marked for fixed_size matrices, this can
100 * be used for dynamic-sized ones also:
102 template<typename MatT
>
103 struct inverse_f
<MatT
,4>
110 * |11 12 13| |10 12 13|
111 * C00 = |21 22 23| C01 = |20 22 23|
112 * |31 32 33| |30 32 33|
114 * |10 11 13| |10 11 12|
115 * C02 = |20 21 23| C03 = |20 21 22|
116 * |30 31 33| |30 31 32|
118 typename
MatT::temporary_type
operator()(const MatT
& M
) const
121 typedef typename
MatT::value_type value_type
;
123 /* Common cofactors, rows 0,1: */
124 value_type m_22_33_23_32
= M(2,2)*M(3,3) - M(2,3)*M(3,2);
125 value_type m_23_30_20_33
= M(2,3)*M(3,0) - M(2,0)*M(3,3);
126 value_type m_20_31_21_30
= M(2,0)*M(3,1) - M(2,1)*M(3,0);
127 value_type m_21_32_22_31
= M(2,1)*M(3,2) - M(2,2)*M(3,1);
128 value_type m_23_31_21_33
= M(2,3)*M(3,1) - M(2,1)*M(3,3);
129 value_type m_20_32_22_30
= M(2,0)*M(3,2) - M(2,2)*M(3,0);
131 /* Compute minors: */
133 = M(1,1)*m_22_33_23_32
+M(1,2)*m_23_31_21_33
+M(1,3)*m_21_32_22_31
;
136 = M(1,0)*m_22_33_23_32
+M(1,2)*m_23_30_20_33
+M(1,3)*m_20_32_22_30
;
139 = M(1,0)*-m_23_31_21_33
+M(1,1)*m_23_30_20_33
+M(1,3)*m_20_31_21_30
;
142 = M(1,0)*m_21_32_22_31
+M(1,1)*-m_20_32_22_30
+M(1,2)*m_20_31_21_30
;
144 /* Compute minors: */
146 = M(0,1)*m_22_33_23_32
+M(0,2)*m_23_31_21_33
+M(0,3)*m_21_32_22_31
;
149 = M(0,0)*m_22_33_23_32
+M(0,2)*m_23_30_20_33
+M(0,3)*m_20_32_22_30
;
152 = M(0,0)*-m_23_31_21_33
+M(0,1)*m_23_30_20_33
+M(0,3)*m_20_31_21_30
;
155 = M(0,0)*m_21_32_22_31
+M(0,1)*-m_20_32_22_30
+M(0,2)*m_20_31_21_30
;
157 /* Common cofactors, rows 2,3: */
158 value_type m_02_13_03_12
= M(0,2)*M(1,3) - M(0,3)*M(1,2);
159 value_type m_03_10_00_13
= M(0,3)*M(1,0) - M(0,0)*M(1,3);
160 value_type m_00_11_01_10
= M(0,0)*M(1,1) - M(0,1)*M(1,0);
161 value_type m_01_12_02_11
= M(0,1)*M(1,2) - M(0,2)*M(1,1);
162 value_type m_03_11_01_13
= M(0,3)*M(1,1) - M(0,1)*M(1,3);
163 value_type m_00_12_02_10
= M(0,0)*M(1,2) - M(0,2)*M(1,0);
165 /* Compute minors (uses row 3 as the multipliers instead of row 0,
166 * which uses the same signs as row 0):
169 = M(3,1)*m_02_13_03_12
+M(3,2)*m_03_11_01_13
+M(3,3)*m_01_12_02_11
;
172 = M(3,0)*m_02_13_03_12
+M(3,2)*m_03_10_00_13
+M(3,3)*m_00_12_02_10
;
175 = M(3,0)*-m_03_11_01_13
+M(3,1)*m_03_10_00_13
+M(3,3)*m_00_11_01_10
;
178 = M(3,0)*m_01_12_02_11
+M(3,1)*-m_00_12_02_10
+M(3,2)*m_00_11_01_10
;
180 /* Compute minors: */
182 = M(2,1)*m_02_13_03_12
+M(2,2)*m_03_11_01_13
+M(2,3)*m_01_12_02_11
;
185 = M(2,0)*m_02_13_03_12
+M(2,2)*m_03_10_00_13
+M(2,3)*m_00_12_02_10
;
188 = M(2,0)*-m_03_11_01_13
+M(2,1)*m_03_10_00_13
+M(2,3)*m_00_11_01_10
;
191 = M(2,0)*m_01_12_02_11
+M(2,1)*-m_00_12_02_10
+M(2,2)*m_00_11_01_10
;
193 /* Finally, compute determinant from the minors, and assign the
194 * inverse as (1/D) * (cofactor matrix)^T:
196 typename
MatT::temporary_type Z
;
197 cml::et::detail::Resize(Z
,4,4);
199 value_type D
= value_type(1) /
200 (M(0,0)*d00
- M(0,1)*d01
+ M(0,2)*d02
- M(0,3)*d03
);
201 Z(0,0) = +d00
*D
; Z(0,1) = -d10
*D
; Z(0,2) = +d20
*D
; Z(0,3) = -d30
*D
;
202 Z(1,0) = -d01
*D
; Z(1,1) = +d11
*D
; Z(1,2) = -d21
*D
; Z(1,3) = +d31
*D
;
203 Z(2,0) = +d02
*D
; Z(2,1) = -d12
*D
; Z(2,2) = +d22
*D
; Z(2,3) = -d32
*D
;
204 Z(3,0) = -d03
*D
; Z(3,1) = +d13
*D
; Z(3,2) = -d23
*D
; Z(3,3) = +d33
*D
;
210 /* If more extensive general linear algebra functionality is offered in
211 * future versions it may be useful to make the elementary row and column
212 * operations separate functions. For now they're simply performed in place,
213 * but the commented-out lines of code show where the calls to these functions
214 * should go if and when they become available.
217 /* @todo: In-place version, and address memory allocation for pivot vector.
220 /* General NxN inverse by Gauss-Jordan elimination with full pivoting: */
221 template<typename MatT
, int _tag
>
224 typename
MatT::temporary_type
operator()(const MatT
& M
) const
227 typedef typename
MatT::value_type value_type
;
232 /* Matrix containing the inverse: */
233 typename
MatT::temporary_type Z
;
234 cml::et::detail::Resize(Z
,N
,N
);
237 /* For tracking pivots */
238 std::vector
<size_t> row_index(N
);
239 std::vector
<size_t> col_index(N
);
240 std::vector
<size_t> pivoted(N
,0);
242 /* For each column */
243 for (size_t i
= 0; i
< N
; ++i
) {
246 size_t row
= 0, col
= 0;
247 value_type max
= value_type(0);
248 for (size_t j
= 0; j
< N
; ++j
) {
250 for (size_t k
= 0; k
< N
; ++k
) {
252 value_type mag
= std::fabs(Z(j
,k
));
263 /* TODO: Check max against epsilon here to catch singularity */
268 /* Swap rows if necessary */
270 /*Z.row_op_swap(row,col);*/
271 for (size_t j
= 0; j
< Z
.cols(); ++j
) {
272 std::swap(Z(row
,j
),Z(col
,j
));
276 /* Process pivot row */
278 value_type pivot
= Z(col
,col
);
279 Z(col
,col
) = value_type(1);
280 /*Z.row_op_mult(col,value_type(1)/pivot);*/
281 value_type k
= value_type(1)/pivot
;
282 for (size_t j
= 0; j
< Z
.cols(); ++j
) {
286 /* Process other rows */
287 for (size_t j
= 0; j
< N
; ++j
) {
289 value_type mult
= -Z(j
,col
);
290 Z(j
,col
) = value_type(0);
291 /*Z.row_op_add_mult(col,j,mult);*/
292 for (size_t k
= 0; k
< Z
.cols(); ++k
) {
293 Z(j
,k
) += mult
* Z(col
,k
);
299 /* Swap columns if necessary */
300 for (int i
= N
-1; i
>= 0; --i
) {
301 if (row_index
[i
] != col_index
[i
]) {
302 /*Z.col_op_swap(row_index[i],col_index[i]);*/
303 for (size_t j
= 0; j
< Z
.rows(); ++j
) {
304 std::swap(Z(j
,row_index
[i
]),Z(j
,col_index
[i
]));
314 /* Inversion by LU factorization is turned off for now due to lack of
315 * pivoting in the implementation, but we may switch back to it at some future
321 /* General NxN inverse by LU factorization: */
322 template<typename MatT
, int _tag
>
325 typename
MatT::temporary_type
operator()(const MatT
& M
) const
328 typedef typename
MatT::value_type value_type
;
330 /* Compute LU factorization: */
332 typename
MatT::temporary_type LU
;
333 cml::et::detail::Resize(LU
,N
,N
);
336 /* Matrix containing the inverse: */
337 typename
MatT::temporary_type Z
;
338 cml::et::detail::Resize(Z
,N
,N
);
340 typename
MatT::col_vector_type v
, x
;
341 cml::et::detail::Resize(v
,N
);
342 cml::et::detail::Resize(x
,N
);
343 for(size_t i
= 0; i
< N
; ++i
)
344 v
[i
] = value_type(0);
345 /* XXX Need a fill() function here. */
347 /* Use lu_solve to solve M*x = v for x, where v = [0 ... 1 ... 0]^T: */
348 for(size_t i
= 0; i
< N
; ++i
) {
352 /* x is column i of the inverse of LU: */
353 for(size_t k
= 0; k
< N
; ++ k
) {
366 /* Note: force_NxN is for checking general NxN inversion against the special-
367 * case 2x2, 3x3 and 4x4 code. I'm leaving it in for now since we may need to
368 * test the NxN code further if the implementation changes. At some future
369 * time when the implementation is stable, everything related to force_NxN can
373 /* Note: Commenting the force_NxN stuff out, but leaving the code here in
374 * case we need to do more testing in the future.
377 /* Generator for the inverse functional for fixed-size matrices: */
378 template<typename MatT
> typename
MatT::temporary_type
379 inverse(const MatT
& M
, fixed_size_tag
/*, bool force_NxN*/)
381 /* Require a square matrix: */
382 cml::et::CheckedSquare(M
, fixed_size_tag());
386 return inverse_f<MatT,0>()(M);
389 return inverse_f
<MatT
,MatT::array_rows
>()(M
);
395 /* Generator for the inverse functional for dynamic-size matrices: */
396 template<typename MatT
> typename
MatT::temporary_type
397 inverse(const MatT
& M
, dynamic_size_tag
/*, bool force_NxN*/)
399 /* Require a square matrix: */
400 cml::et::CheckedSquare(M
, dynamic_size_tag());
404 return inverse_f<MatT,0>()(M);
407 /* Dispatch based upon the matrix dimension: */
409 case 2: return inverse_f
<MatT
,2>()(M
); // 2x2
410 case 3: return inverse_f
<MatT
,3>()(M
); // 3x3
411 case 4: return inverse_f
<MatT
,4>()(M
); // 4x4
412 default: return inverse_f
<MatT
,0>()(M
); // > 4x4 (or 1x1)
419 } // namespace detail
421 /** Inverse of a matrix. */
422 template<typename E
, class AT
, typename BO
, typename L
> inline
423 typename matrix
<E
,AT
,BO
,L
>::temporary_type
424 inverse(const matrix
<E
,AT
,BO
,L
>& M
/*, bool force_NxN = false*/)
426 typedef typename matrix
<E
,AT
,BO
,L
>::size_tag size_tag
;
427 return detail::inverse(M
,size_tag()/*,force_NxN*/);
430 /** Inverse of a matrix expression. */
431 template<typename XprT
> inline
432 typename
et::MatrixXpr
<XprT
>::temporary_type
433 inverse(const et::MatrixXpr
<XprT
>& e
/*, bool force_NxN = false*/)
435 typedef typename
et::MatrixXpr
<XprT
>::size_tag size_tag
;
436 return detail::inverse(e
,size_tag()/*,force_NxN*/);
443 // -------------------------------------------------------------------------