00001 #ifndef VVLIB_UTIL_MATRIX_IMPL_HPP 00002 #define VVLIB_UTIL_MATRIX_IMPL_HPP 00003 00005 00006 template<size_t nRows,size_t intSize,size_t nCols,typename T> 00007 Matrix<nRows,nCols,T> operator*(const Matrix<nRows,intSize,T>& mat1, 00008 const Matrix<intSize,nCols,T>& mat2) 00009 { 00010 Matrix<nRows,nCols,T> ans; 00011 for(size_t i = 0 ; i < nRows ; i++) 00012 for(size_t j = 0 ; j < nCols ; j++) 00013 { 00014 T acc = 0; 00015 for(size_t k = 0 ; k < intSize ; k++) 00016 acc += mat1[i][k] * mat2[k][j]; 00017 ans[i][j] = acc; 00018 } 00019 return ans; 00020 } 00021 00022 template<typename T> 00023 T det(const Matrix<1,1,T>& mat) 00024 { 00025 return mat(0,0); 00026 } 00027 00028 template<typename T> 00029 T det(const Matrix<2,2,T>& mat) 00030 { 00031 return mat(0,0)*mat(1,1) - mat(0,1)*mat(1,0); 00032 } 00033 00034 template<typename T> 00035 T det(const Matrix<3,3,T>& mat) 00036 { 00037 return mat(0,0)*mat(1,1)*mat(2,2) + mat(0,1)*mat(1,2)*mat(2,0) + mat(0,2)*mat(1,0)*mat(2,1) - 00038 mat(0,0)*mat(1,2)*mat(2,1) - mat(0,1)*mat(1,0)*mat(2,2) - mat(0,2)*mat(1,1)*mat(2,0); 00039 } 00040 00041 template <size_t nRows, typename T> 00042 T det(const Matrix<nRows,nRows,T>& mat) 00043 { 00044 STATIC_ASSERT(nRows > 3, "This determinant method should be called only for more than 3x3 matrix."); 00045 T acc = 0; 00046 for(size_t i = 0 ; i < nRows ; i++) 00047 { 00048 acc += mat(i,0) * cofactor(mat,i,0); 00049 } 00050 return acc; 00051 } 00052 00053 template <size_t nRows, typename T> 00054 T matrix_minor(const Matrix<nRows,nRows,T>& mat, size_t i, size_t j) 00055 { 00056 STATIC_ASSERT(nRows>0, "The matrix minor function requires a non-empty square matrix."); 00057 Matrix<nRows-1,nRows-1,T> ans; 00058 for(size_t i1 = 0, i2 = 0 ; i1 < nRows ; i1++, i2++) 00059 { 00060 if( i1 == i ) 00061 { 00062 i2--; 00063 } 00064 else 00065 { 00066 for(size_t j1=0, j2 = 0 ; j1 < nRows ; j1++, j2++) 00067 { 00068 if(j1 == j) 00069 { 00070 j2--; 00071 } 00072 else 00073 { 00074 ans(i2,j2) = mat(i1,j1); 00075 } 00076 } 00077 } 00078 } 00079 return det(ans); 00080 } 00081 00082 template <size_t nRows, typename T> 00083 T cofactor(const Matrix<nRows,nRows,T>& mat, size_t i, size_t j) 00084 { 00085 T inv = 1; 00086 if((i+j) % 2) 00087 { 00088 inv = -1; 00089 } 00090 return inv*matrix_minor(mat, i, j); 00091 } 00092 00093 template <typename T> 00094 Matrix<1,1,T> inverse(const Matrix<1,1,T>& mat) 00095 { 00096 Matrix<1,1,T> ans; 00097 ans[0][0] = 1/mat(0,0); 00098 return ans; 00099 } 00100 00101 template <typename T> 00102 Matrix<2,2,T> inverse(const Matrix<2,2,T>& mat) 00103 { 00104 Matrix<2,2,T> ans; 00105 T d; 00106 d = det(mat); 00107 if(d == 0) 00108 return ans; 00109 ans(0,0) = mat(1,1) / d; 00110 ans(0,1) = mat(0,1) / -d; 00111 ans(1,0) = mat(1,0) / -d; 00112 ans(1,1) = mat(0,0) / d; 00113 return ans; 00114 } 00115 00116 template <typename T> 00117 Matrix<3,3,T> inverse(const Matrix<3,3,T>& mat) 00118 { 00119 Matrix<3,3,T> ans; 00120 T d; 00121 d = det(mat); 00122 if(d == 0) 00123 return ans; 00124 ans(0,0) = (mat(1,1)*mat(2,2)-mat(1,2)*mat(2,1)) / d; 00125 ans(1,1) = (mat(0,0)*mat(2,2)-mat(0,2)*mat(2,0)) / d; 00126 ans(2,2) = (mat(1,1)*mat(0,0)-mat(1,0)*mat(0,1)) / d; 00127 ans(1,0) = (mat(1,2)*mat(2,0)-mat(1,0)*mat(2,2)) / d; 00128 ans(0,1) = (mat(2,1)*mat(0,2)-mat(0,1)*mat(2,2)) / d; 00129 ans(2,0) = (mat(1,0)*mat(2,1)-mat(1,1)*mat(2,0)) / d; 00130 ans(0,2) = (mat(0,1)*mat(1,2)-mat(1,1)*mat(0,2)) / d; 00131 ans(1,2) = (mat(0,2)*mat(1,0)-mat(0,0)*mat(1,2)) / d; 00132 ans(2,1) = (mat(2,0)*mat(0,1)-mat(0,0)*mat(2,1)) / d; 00133 return ans; 00134 } 00135 00136 template <size_t nRows, typename T> 00137 Matrix<nRows,nRows,T> inverse(const Matrix<nRows,nRows,T>& mat) 00138 { 00139 Matrix<nRows,nRows,T> ans; 00140 T d = det(mat); 00141 if(d == 0) 00142 return ans; 00143 for(size_t i = 0 ; i < nRows ; ++i) 00144 for(size_t j = 0 ; j < nRows ; ++j) 00145 { 00146 ans(i,j) = cofactor(mat,j,i)/d; 00147 } 00148 return ans; 00149 } 00150 00151 template <size_t nRows, size_t nCols, typename T> 00152 Matrix<nCols,nRows,T> transpose( const Matrix<nRows, nCols, T>& mat) 00153 { 00154 Matrix<nCols,nRows,T> ans; 00155 for(size_t i = 0 ; i < nRows ; i++) 00156 for(size_t j = 0 ; j < nCols ; j++) 00157 ans(j,i) = mat(i,j); 00158 return ans; 00159 } 00160 00161 template <size_t nRows, size_t nCols, typename T> 00162 T normsq(const Matrix<nRows,nCols,T>& mat) 00163 { 00164 T acc = 0; 00165 for(size_t i = 0 ; i < nRows ; i++) 00166 for(size_t j = 0 ; j < nCols ; j++) 00167 { 00168 const T& v = mat(i,j); 00169 acc += v*v; 00170 } 00171 return acc; 00172 } 00173 00174 template <size_t nRows, size_t nCols, typename T> 00175 T norm(const Matrix<nRows,nCols,T>& mat) 00176 { 00177 return std::sqrt(normsq(mat)); 00178 } 00179 00181 00182 #endif // VVLIB/UTIL/MATRIX_IMPL_HPP 00183