Dogcows Code - chaz/yoink/blob - src/cml/matrix/determinant.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 determinant of a square matrix using LU factorization.
  11  *
  12  *  @todo This should be specialized on the matrix size for small matrices.
  13  */
  14
  15 #ifndef determinant_h
  16 #define determinant_h
  17
  18 #include <cml/matrix/lu.h>
  19
  20 namespace cml {
  21 namespace detail {
  22
  23 /* Need to use a functional, since template functions cannot be
  24  * specialized.  N is used to differentiate dimension only, so this can be
  25  * used for any matrix size type:
  26  */
  27 template<typename MatT, int N> struct determinant_f;
  28
  29 /* 2x2 determinant.  Despite being marked for fixed_size matrices, this can
  30  * be used for dynamic-sized ones also:
  31  */
  32 template<typename MatT>
  33 struct determinant_f<MatT,2>
  34 {
  35     typename MatT::value_type operator()(const MatT& M) const
  36     {
  37         return M(0,0)*M(1,1) - M(1,0)*M(0,1);
  38     }
  39
  40 };
  41
  42 /* 3x3 determinant.  Despite being marked for fixed_size matrices, this can
  43  * be used for dynamic-sized ones also:
  44  */
  45 template<typename MatT>
  46 struct determinant_f<MatT,3>
  47 {
  48     /*     [00 01 02]
  49      * M = [10 11 12]
  50      *     [20 21 22]
  51      */
  52     typename MatT::value_type operator()(const MatT& M) const
  53     {
  54         return M(0,0)*(M(1,1)*M(2,2) - M(1,2)*M(2,1))
  55              + M(0,1)*(M(1,2)*M(2,0) - M(1,0)*M(2,2))
  56              + M(0,2)*(M(1,0)*M(2,1) - M(1,1)*M(2,0));
  57     }
  58
  59 };
  60
  61 /* 4x4 determinant.  Despite being marked for fixed_size matrices, this can
  62  * be used for dynamic-sized ones also:
  63  */
  64 template<typename MatT>
  65 struct determinant_f<MatT,4>
  66 {
  67     /*     [00 01 02 03]
  68      * M = [10 11 12 13]
  69      *     [20 21 22 23]
  70      *     [30 31 32 33]
  71      *
  72      *       |11 12 13|         |10 12 13|
  73      * C00 = |21 22 23|   C01 = |20 22 23|
  74      *       |31 32 33|         |30 32 33|
  75      *
  76      *       |10 11 13|         |10 11 12|
  77      * C02 = |20 21 23|   C03 = |20 21 22|
  78      *       |30 31 33|         |30 31 32|
  79      *
  80      * d00 =   11 * (22*33 - 23*32)  d01 =   10 * (22*33 - 23*32)
  81      *       + 12 * (23*31 - 21*33)        + 12 * (23*30 - 20*33)
  82      *       + 13 * (21*32 - 22*31)        + 13 * (20*32 - 22*30)
  83      *
  84      * d02 =   10 * (21*33 - 23*31)  d03 =   10 * (21*32 - 22*31)
  85      *       + 11 * (23*30 - 20*33)        + 11 * (22*30 - 20*32)
  86      *       + 13 * (20*31 - 21*30)        + 12 * (20*31 - 21*30)
  87      */
  88     typename MatT::value_type operator()(const MatT& M) const
  89     {
  90         /* Shorthand. */
  91         typedef typename MatT::value_type value_type;
  92
  93         /* Common cofactors: */
  94         value_type m_22_33_23_32 = M(2,2)*M(3,3) - M(2,3)*M(3,2);
  95         value_type m_23_30_20_33 = M(2,3)*M(3,0) - M(2,0)*M(3,3);
  96         value_type m_20_31_21_30 = M(2,0)*M(3,1) - M(2,1)*M(3,0);
  97         value_type m_21_32_22_31 = M(2,1)*M(3,2) - M(2,2)*M(3,1);
  98         value_type m_23_31_21_33 = M(2,3)*M(3,1) - M(2,1)*M(3,3);
  99         value_type m_20_32_22_30 = M(2,0)*M(3,2) - M(2,2)*M(3,0);
 100
 101         value_type d00 = M(0,0)*(
 102                 M(1,1) * m_22_33_23_32
 103               + M(1,2) * m_23_31_21_33
 104               + M(1,3) * m_21_32_22_31);
 105
 106         value_type d01 = M(0,1)*(
 107                 M(1,0) * m_22_33_23_32
 108               + M(1,2) * m_23_30_20_33
 109               + M(1,3) * m_20_32_22_30);
 110
 111         value_type d02 = M(0,2)*(
 112                 M(1,0) * - m_23_31_21_33
 113               + M(1,1) * m_23_30_20_33
 114               + M(1,3) * m_20_31_21_30);
 115
 116         value_type d03 = M(0,3)*(
 117                 M(1,0) * m_21_32_22_31
 118               + M(1,1) * - m_20_32_22_30
 119               + M(1,2) * m_20_31_21_30);
 120
 121         return d00 - d01 + d02 - d03;
 122     }
 123
 124 };
 125
 126 /* General NxN determinant by LU factorization:  */
 127 template<typename MatT, int N>
 128 struct determinant_f
 129 {
 130     typename MatT::value_type operator()(const MatT& M) const
 131     {
 132         /* Compute the LU factorization: */
 133         typename MatT::temporary_type LU = lu(M);
 134
 135         /* The product of the diagonal entries is the determinant: */
 136         typename MatT::value_type det = LU(0,0);
 137         for(size_t i = 1; i < LU.rows(); ++ i)
 138             det *= LU(i,i);
 139         return det;
 140     }
 141
 142 };
 143
 144 /* Generator for the determinant functional for fixed-size matrices: */
 145 template<typename MatT> typename MatT::value_type
 146 determinant(const MatT& M, fixed_size_tag)
 147 {
 148     /* Require a square matrix: */
 149     cml::et::CheckedSquare(M, fixed_size_tag());
 150     return determinant_f<MatT,MatT::array_rows>()(M);
 151 }
 152
 153 /* Generator for the determinant functional for dynamic-size matrices: */
 154 template<typename MatT> typename MatT::value_type
 155 determinant(const MatT& M, dynamic_size_tag)
 156 {
 157     /* Require a square matrix: */
 158     cml::et::CheckedSquare(M, dynamic_size_tag());
 159
 160     /* Dispatch based upon the matrix dimension: */
 161     switch(M.rows()) {
 162         case 2:  return determinant_f<MatT,2>()(M);
 163         case 3:  return determinant_f<MatT,3>()(M);
 164         case 4:  return determinant_f<MatT,4>()(M);
 165         default: return determinant_f<MatT,0>()(M);     // > 4x4.
 166     }
 167 }
 168
 169 } // namespace detail
 170
 171 /** Determinant of a matrix. */
 172 template<typename E, class AT, class BO, class L> inline E
 173 determinant(const matrix<E,AT,BO,L>& M)
 174 {
 175     typedef typename matrix<E,AT,BO,L>::size_tag size_tag;
 176     return detail::determinant(M,size_tag());
 177 }
 178
 179 /** Determinant of a matrix expression. */
 180 template<typename XprT> inline typename XprT::value_type
 181 determinant(const et::MatrixXpr<XprT>& e)
 182 {
 183     typedef typename et::MatrixXpr<XprT>::size_tag size_tag;
 184     return detail::determinant(e,size_tag());
 185 }
 186
 187 } // namespace cml
 188
 189 #endif
 190
 191 // -------------------------------------------------------------------------
 192 // vim:ft=cpp