Dogcows Code - chaz/yoink/blob - src/moof/cml/matrix/inverse.h

   1 /* -*- C++ -*- ------------------------------------------------------------
   2
   3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
   4
   5 The Configurable Math Library (CML) is distributed under the terms of the
   6 Boost Software License, v1.0 (see cml/LICENSE for details).
   7
   8  *-----------------------------------------------------------------------*/
   9 /** @file
  10  *  @brief Compute the inverse of a matrix by LU factorization.
  11  */
  12
  13 #ifndef matrix_inverse_h
  14 #define matrix_inverse_h
  15
  16 #include <vector>
  17 #include <cml/matrix/lu.h>
  18
  19 namespace cml {
  20 namespace detail {
  21
  22 /* Need to use a functional, since template functions cannot be
  23  * specialized. _tag is used to specialize based upon dimension:
  24  */
  25 template<typename MatT, int _tag> struct inverse_f;
  26
  27 /* @todo: Reciprocal optimization for division by determinant.
  28  */
  29
  30 /* 2x2 inverse.  Despite being marked for fixed_size matrices, this can
  31  * be used for dynamic-sized ones also:
  32  */
  33 template<typename MatT>
  34 struct inverse_f<MatT,2>
  35 {
  36     typename MatT::temporary_type operator()(const MatT& M) const
  37     {
  38         typedef typename MatT::temporary_type temporary_type;
  39         typedef typename temporary_type::value_type value_type;
  40
  41         /* Matrix containing the inverse: */
  42         temporary_type Z;
  43         cml::et::detail::Resize(Z,2,2);
  44
  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;
  49
  50         return Z;
  51     }
  52 };
  53
  54 /* 3x3 inverse.  Despite being marked for fixed_size matrices, this can
  55  * be used for dynamic-sized ones also:
  56  */
  57 template<typename MatT>
  58 struct inverse_f<MatT,3>
  59 {
  60     /*     [00 01 02]
  61      * M = [10 11 12]
  62      *     [20 21 22]
  63      */
  64     typename MatT::temporary_type operator()(const MatT& M) const
  65     {
  66         /* Shorthand. */
  67         typedef typename MatT::value_type value_type;
  68
  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);
  73
  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);
  77
  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);
  81
  82         /* Compute determinant from the minors: */
  83         value_type D =
  84             value_type(1) / (M(0,0)*m_00 + M(0,1)*m_01 + M(0,2)*m_02);
  85
  86         /* Matrix containing the inverse: */
  87         typename MatT::temporary_type Z;
  88         cml::et::detail::Resize(Z,3,3);
  89
  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;
  94
  95         return Z;
  96     }
  97 };
  98
  99 /* 4x4 inverse.  Despite being marked for fixed_size matrices, this can
 100  * be used for dynamic-sized ones also:
 101  */
 102 template<typename MatT>
 103 struct inverse_f<MatT,4>
 104 {
 105     /*     [00 01 02 03]
 106      * M = [10 11 12 13]
 107      *     [20 21 22 23]
 108      *     [30 31 32 33]
 109      *
 110      *       |11 12 13|         |10 12 13|
 111      * C00 = |21 22 23|   C01 = |20 22 23|
 112      *       |31 32 33|         |30 32 33|
 113      *
 114      *       |10 11 13|         |10 11 12|
 115      * C02 = |20 21 23|   C03 = |20 21 22|
 116      *       |30 31 33|         |30 31 32|
 117      */
 118     typename MatT::temporary_type operator()(const MatT& M) const
 119     {
 120         /* Shorthand. */
 121         typedef typename MatT::value_type value_type;
 122
 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);
 130
 131         /* Compute minors: */
 132         value_type d00
 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;
 134
 135         value_type d01
 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;
 137
 138         value_type d02
 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;
 140
 141         value_type d03
 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;
 143
 144         /* Compute minors: */
 145         value_type d10
 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;
 147
 148         value_type d11
 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;
 150
 151         value_type d12
 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;
 153
 154         value_type d13
 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;
 156
 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);
 164
 165         /* Compute minors (uses row 3 as the multipliers instead of row 0,
 166          * which uses the same signs as row 0):
 167          */
 168         value_type d20
 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;
 170
 171         value_type d21
 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;
 173
 174         value_type d22
 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;
 176
 177         value_type d23
 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;
 179
 180         /* Compute minors: */
 181         value_type d30
 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;
 183
 184         value_type d31
 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;
 186
 187         value_type d32
 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;
 189
 190         value_type d33
 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;
 192
 193         /* Finally, compute determinant from the minors, and assign the
 194          * inverse as (1/D) * (cofactor matrix)^T:
 195          */
 196         typename MatT::temporary_type Z;
 197         cml::et::detail::Resize(Z,4,4);
 198
 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;
 205
 206         return Z;
 207     }
 208 };
 209
 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.
 215  */
 216
 217 /* @todo: In-place version, and address memory allocation for pivot vector.
 218  */
 219
 220 /* General NxN inverse by Gauss-Jordan elimination with full pivoting:  */
 221 template<typename MatT, int _tag>
 222 struct inverse_f
 223 {
 224     typename MatT::temporary_type operator()(const MatT& M) const
 225     {
 226         /* Shorthand. */
 227         typedef typename MatT::value_type value_type;
 228
 229         /* Size of matrix */
 230         size_t N = M.rows();
 231
 232         /* Matrix containing the inverse: */
 233         typename MatT::temporary_type Z;
 234         cml::et::detail::Resize(Z,N,N);
 235         Z = M;
 236
 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);
 241
 242         /* For each column */
 243         for (size_t i = 0; i < N; ++i) {
 244
 245             /* Find the pivot */
 246             size_t row = 0, col = 0;
 247             value_type max = value_type(0);
 248             for (size_t j = 0; j < N; ++j) {
 249                 if (!pivoted[j]) {
 250                     for (size_t k = 0; k < N; ++k) {
 251                         if (!pivoted[k]) {
 252                             value_type mag = std::fabs(Z(j,k));
 253                             if (mag > max) {
 254                                 max = mag;
 255                                 row = j;
 256                                 col = k;
 257                             }
 258                         }
 259                     }
 260                 }
 261             }
 262
 263             /* TODO: Check max against epsilon here to catch singularity */
 264
 265             row_index[i] = row;
 266             col_index[i] = col;
 267
 268             /* Swap rows if necessary */
 269             if (row != col) {
 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));
 273                 }
 274             }
 275
 276             /* Process pivot row */
 277             pivoted[col] = true;
 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) {
 283                 Z(col,j) *= k;
 284             }
 285
 286             /* Process other rows */
 287             for (size_t j = 0; j < N; ++j) {
 288                 if (j != col) {
 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);
 294                     }
 295                 }
 296             }
 297         }
 298
 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]));
 305                 }
 306             }
 307         }
 308
 309         /* Return result */
 310         return Z;
 311     }
 312 };
 313
 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
 316  * time.
 317  */
 318
 319 #if 0
 320
 321 /* General NxN inverse by LU factorization:  */
 322 template<typename MatT, int _tag>
 323 struct inverse_f
 324 {
 325     typename MatT::temporary_type operator()(const MatT& M) const
 326     {
 327         /* Shorthand. */
 328         typedef typename MatT::value_type value_type;
 329
 330         /* Compute LU factorization: */
 331         size_t N = M.rows();
 332         typename MatT::temporary_type LU;
 333         cml::et::detail::Resize(LU,N,N);
 334         LU = lu(M);
 335
 336         /* Matrix containing the inverse: */
 337         typename MatT::temporary_type Z;
 338         cml::et::detail::Resize(Z,N,N);
 339
 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. */
 346
 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) {
 349             v[i] = 1.;
 350             x = lu_solve(LU,v);
 351
 352             /* x is column i of the inverse of LU: */
 353             for(size_t k = 0; k < N; ++ k) {
 354                 Z(k,i) = x[k];
 355             }
 356             v[i] = 0.;
 357         }
 358
 359         return Z;
 360     }
 361
 362 };
 363
 364 #endif
 365
 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
 370  * be taken out.
 371  */
 372
 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.
 375  */
 376
 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*/)
 380 {
 381     /* Require a square matrix: */
 382     cml::et::CheckedSquare(M, fixed_size_tag());
 383
 384     /*
 385     if (force_NxN) {
 386         return inverse_f<MatT,0>()(M);
 387     } else {
 388     */
 389     return inverse_f<MatT,MatT::array_rows>()(M);
 390     /*
 391     }
 392     */
 393 }
 394
 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*/)
 398 {
 399     /* Require a square matrix: */
 400     cml::et::CheckedSquare(M, dynamic_size_tag());
 401
 402     /*
 403     if (force_NxN) {
 404         return inverse_f<MatT,0>()(M);
 405     } else {
 406     */
 407     /* Dispatch based upon the matrix dimension: */
 408     switch(M.rows()) {
 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)
 413     }
 414     /*
 415     }
 416     */
 417 }
 418
 419 } // namespace detail
 420
 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*/)
 425 {
 426     typedef typename matrix<E,AT,BO,L>::size_tag size_tag;
 427     return detail::inverse(M,size_tag()/*,force_NxN*/);
 428 }
 429
 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*/)
 434 {
 435     typedef typename et::MatrixXpr<XprT>::size_tag size_tag;
 436     return detail::inverse(e,size_tag()/*,force_NxN*/);
 437 }
 438
 439 } // namespace cml
 440
 441 #endif
 442
 443 // -------------------------------------------------------------------------
 444 // vim:ft=cpp