TooN 2.1
|
00001 #ifndef TOON_INCLUDE_DETERMINANT_H 00002 #define TOON_INCLUDE_DETERMINANT_H 00003 #include <TooN/TooN.h> 00004 #include <cstdlib> 00005 #include <utility> 00006 #ifdef TOON_DETERMINANT_LAPACK 00007 #include <TooN/LU.h> 00008 #endif 00009 00010 namespace TooN 00011 { 00012 namespace Internal 00013 { 00014 ///@internal 00015 ///@brief Provides the static size for a square matrix. 00016 ///In 00017 ///the general case, if R != C, then the matrix is not 00018 ///square and so no size is provided. A compile error results. 00019 ///@ingroup gInternal 00020 template<int R, int C> struct Square 00021 { 00022 }; 00023 00024 00025 ///@internal 00026 ///@brief Provides the static size for a square matrix where both dimensions are the same. 00027 ///@ingroup gInternal 00028 template<int R> struct Square<R, R> 00029 { 00030 static const int Size = R; ///<The size 00031 }; 00032 00033 ///@internal 00034 ///@brief Provides the static size for a square matrix where one dimension is static. 00035 ///The size must be equal to the size of the static dimension. 00036 ///@ingroup gInternal 00037 template<int R> struct Square<R, Dynamic> 00038 { 00039 static const int Size = R; ///<The size 00040 }; 00041 ///@internal 00042 ///@brief Provides the static size for a square matrix where one dimension is static. 00043 ///The size must be equal to the size of the static dimension. 00044 ///@ingroup gInternal 00045 template<int C> struct Square<Dynamic, C> 00046 { 00047 static const int Size = C; ///<The size 00048 }; 00049 ///@internal 00050 ///@brief Provides the static size for a square matrix where both dimensions are dynamic. 00051 ///The size must be Dynamic. 00052 ///@ingroup gInternal 00053 template<> struct Square<Dynamic, Dynamic> 00054 { 00055 static const int Size = Dynamic; ///<The size 00056 }; 00057 }; 00058 00059 00060 /** Compute the determinant using Gaussian elimination. 00061 @param A_ The matrix to find the determinant of. 00062 @returns determinant. 00063 @ingroup gLinAlg 00064 */ 00065 template<int R, int C, typename Precision, typename Base> 00066 Precision determinant_gaussian_elimination(const Matrix<R, C, Precision, Base>& A_) 00067 { 00068 Matrix<Internal::Square<R,C>::Size, Internal::Square<R,C>::Size,Precision> A = A_; 00069 TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols()); 00070 using std::swap; 00071 using std::abs; 00072 00073 int size=A.num_rows(); 00074 00075 //If row operations of the form row_a += alpha * row_b 00076 //then the determinant is unaffected. However, if a row 00077 //is scaled, then the determinant is scaled by the same 00078 //amount. 00079 Precision determinant=1; 00080 00081 for (int i=0; i<size; ++i) { 00082 00083 //Find the pivot 00084 int argmax = i; 00085 Precision maxval = abs(A[i][i]); 00086 00087 for (int ii=i+1; ii<size; ++ii) { 00088 Precision v = abs(A[ii][i]); 00089 if (v > maxval) { 00090 maxval = v; 00091 argmax = ii; 00092 } 00093 } 00094 Precision pivot = A[argmax][i]; 00095 00096 //assert(abs(pivot) > 1e-16); 00097 00098 //Swap the current row with the pivot row if necessary. 00099 //A row swap negates the determinant. 00100 if (argmax != i) { 00101 determinant*=-1; 00102 for (int j=i; j<size; ++j) 00103 swap(A[i][j], A[argmax][j]); 00104 } 00105 00106 determinant *= A[i][i]; 00107 00108 if(determinant == 0) 00109 return 0; 00110 00111 for (int u=i+1; u<size; ++u) { 00112 //Do not multiply out the usual 1/pivot term 00113 //to avoid division. It causes poor scaling. 00114 Precision factor = A[u][i]/pivot; 00115 00116 for (int j=i+1; j<size; ++j) 00117 A[u][j] = A[u][j] - factor * A[i][j]; 00118 } 00119 } 00120 00121 return determinant; 00122 } 00123 00124 #ifdef TOON_DETERMINANT_LAPACK 00125 /** Compute the determinant using TooN::LU. 00126 @param A The matrix to find the determinant of. 00127 @returns determinant. 00128 @ingroup gLinAlg 00129 */ 00130 template<int R, int C, class P, class B> 00131 P determinant_LU(const Matrix<R, C, P, B>& A) 00132 { 00133 TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols()); 00134 LU<Internal::Square<R,C>::Size, P> lu(A); 00135 return lu.determinant(); 00136 } 00137 #endif 00138 00139 /** 00140 Compute the determinant of a matrix using an appropriate method. The 00141 obvious method is used for 2x2, otherwise 00142 determinant_gaussian_elimination() or determinant_LU() is used depending 00143 on the value of \c TOON_DETERMINANT_LAPACK. See also \ref sConfigLapack. 00144 @param A The matrix to find the determinant of. 00145 @returns determinant. 00146 @ingroup gLinAlg 00147 */ 00148 template<int R, int C, class P, class B> 00149 P determinant(const Matrix<R, C, P, B>& A) 00150 { 00151 TooN::SizeMismatch<R, C>::test(A.num_rows(), A.num_cols()); 00152 if(A.num_rows() == 2) 00153 return A[0][0]*A[1][1] - A[1][0]*A[0][1]; 00154 #if defined TOON_DETERMINANT_LAPACK && TOON_DETERMINANT_LAPACK != -1 00155 else if(A.num_rows() >= TOON_DETERMINANT_LAPACK) 00156 return determinant_LU(A); 00157 #endif 00158 else 00159 return determinant_gaussian_elimination(A); 00160 } 00161 } 00162 00163 #endif