00001 #ifndef _VVLIB_UTIL_MATRIX_H 00002 #define _VVLIB_UTIL_MATRIX_H 00003 00009 #include <config.h> 00010 #include <util/vector.h> 00011 #include <util/static_assert.h> 00012 #include <cmath> 00013 #include <QTextStream> 00014 00015 namespace util 00016 { 00024 template <size_t nRows,size_t nCols,typename T = double> 00025 class Matrix 00026 { 00027 private: 00028 Vector<nCols,T> rows[nRows]; 00029 00030 public: 00031 typedef T value_type; 00032 typedef T& reference_type; 00033 typedef const T& const_reference_type; 00034 typedef T* pointer_type; 00035 typedef const T* const_pointer_type; 00036 typedef T* iterator; 00037 typedef const T* const_iterator; 00038 00042 Matrix(void) 00043 { 00044 for( size_t i = 0 ; i < nRows ; i++ ) 00045 for( size_t j = 0 ; j < nCols ; j++ ) 00046 rows[ i ][ j ] = 0; 00047 } 00048 00052 Matrix(const Matrix& mat) 00053 { 00054 for(size_t i = 0 ; i < nRows ; i++) 00055 rows[i] = mat.rows[i]; 00056 } 00057 00063 template <typename T1> 00064 explicit Matrix(const Matrix<nRows,nCols,T1>& mat) 00065 { 00066 for(size_t i = 0 ; i < nRows ; i++) 00067 rows[i] = mat.rows[i]; 00068 } 00069 00078 template <size_t nRows1,size_t nCols1,typename T1> 00079 explicit Matrix(const Matrix<nRows1,nCols1,T1>& mat) 00080 { 00081 int rowMax = nRows1 < nRows ? nRows1 : nRows; 00082 int colMax = nCols1 < nCols ? nCols1 : nCols; 00083 for(size_t i = 0 ; i < rowMax ; i++) 00084 { 00085 Vector<nCols,T>& c = rows[i]; 00086 const Vector<nCols1,T1>& c1 = mat[i]; 00087 for(size_t j = 0 ; j < colMax ; j++) 00088 c[j] = c1[j]; 00089 if(colMax < nCols) 00090 for(size_t j = colMax ; j < nCols ; ++j) 00091 c[j] = i==j ? T(1) : T(0); 00092 } 00093 if(rowMax < nRows) 00094 { 00095 for(size_t i = rowMax ; i < nRows ; ++i) 00096 { 00097 Vector<nCols,T>& c = rows[i]; 00098 for(size_t j = colMax ; j < nCols ; ++j) 00099 c[j] = i==j ? T(1) : T(0); 00100 } 00101 } 00102 } 00103 00104 #ifdef USE_CXX0X 00105 Matrix(Matrix&&) = default; 00106 Matrix& operator=(Matrix&&) = default; 00107 #endif 00108 00114 template <typename T1> 00115 Matrix(const Vector<nCols,T1>* vecs) 00116 { 00117 for(size_t i = 0 ; i < nRows ; i++) 00118 rows[i] = vecs[i]; 00119 } 00120 00130 Matrix( const T* values, bool c_style = true) 00131 { 00132 if(c_style) 00133 { 00134 for(size_t i = 0 ; i < nRows ; i++) 00135 { 00136 rows[i] = Vector<nCols,T>(values + (i*nCols)); 00137 } 00138 } 00139 else 00140 { 00141 for(size_t i = 0 ; i < nRows ; i++) 00142 for(size_t j = 0 ; j < nCols ; j++) 00143 { 00144 rows[i][j] = values[i+j*nRows]; 00145 } 00146 } 00147 } 00148 00154 Matrix(const T& value) 00155 { 00156 for(size_t i = 0 ; i < nRows ; i++) 00157 for(size_t j = 0 ; j < nCols ; j++) 00158 rows[i][j] = (i==j)?value:0; 00159 } 00160 00166 static Vector<2,size_t> size() { return Vector<2,size_t>(nRows, nCols); } 00167 00171 static size_t nbRows() { return nRows; } 00172 00176 static size_t nbColumns() { return nCols; } 00177 00183 const T* c_data() const { return rows[0].c_data(); } 00184 00190 T* data() { return rows[0].data(); } 00191 00195 Matrix operator-(void) const 00196 { 00197 Matrix ans; 00198 00199 for(size_t i = 0 ; i < nRows ; i++) 00200 ans.rows[i] = -rows[i]; 00201 00202 return ans; 00203 } 00204 00208 Matrix operator+(const Matrix& mat) const 00209 { 00210 Matrix ans; 00211 00212 for(size_t i = 0 ; i < nRows ; i++) 00213 ans.rows[i] = rows[i] + mat.rows[i]; 00214 00218 return ans; 00219 } 00220 00221 Matrix operator-(const Matrix& mat) const 00222 { 00223 Matrix ans; 00224 00225 for(size_t i = 0 ; i < nRows ; i++) 00226 ans.rows[i] = rows[i] - mat.rows[i]; 00227 00228 return ans; 00229 } 00230 00234 Matrix operator*(const T& scalar) const 00235 { 00236 Matrix ans; 00237 00238 for(size_t i = 0 ; i < nRows ; i++) 00239 ans[i] = rows[i] * scalar; 00240 00241 return ans; 00242 } 00243 00247 Matrix operator/(const T& scalar) const 00248 { 00249 Matrix ans; 00250 00251 for(size_t i = 0 ; i < nRows ; i++) 00252 ans[i] = rows[i] / scalar; 00253 00254 return ans; 00255 } 00256 00260 friend Matrix operator*(const T& scalar,const Matrix& mat) 00261 { 00262 Matrix ans; 00263 00264 for(size_t i = 0 ; i < nRows ; i++) 00265 ans[i] = scalar * mat.rows[i]; 00266 00267 return ans; 00268 } 00269 00273 Vector<nRows,T> operator*(const Vector<nCols,T>& vec) const 00274 { 00275 Vector<nRows,T> ans; 00276 for(size_t i = 0 ; i < nRows ; ++i) 00277 { 00278 T value = 0; 00279 for(size_t j = 0 ; j < nCols ; ++j) 00280 value += rows[i][j] * vec[j]; 00281 ans[i] = value; 00282 } 00283 return ans; 00284 } 00285 00286 Matrix& operator=(const Matrix& mat) 00287 { 00288 for(size_t i = 0 ; i < nRows ; i++) 00289 rows[i] = mat.rows[i]; 00290 00291 return (*this); 00292 } 00293 00294 Matrix& operator+=(const Matrix& mat) 00295 { 00296 return ((*this) = (*this) + mat); 00297 } 00298 00299 Matrix& operator-=(const Matrix& mat) 00300 { 00301 return ((*this) = (*this) - mat); 00302 } 00303 00304 Matrix& operator*=(const T& scalar) 00305 { 00306 return ((*this) = (*this) * scalar); 00307 } 00308 00309 Matrix& operator/=(const T& scalar) 00310 { 00311 return ((*this) = (*this) / scalar); 00312 } 00313 00314 Matrix& operator*=(const Matrix& mat) 00315 { 00316 return ((*this) = (*this) * mat); 00317 } 00318 00319 bool operator==(const Matrix& mat) const 00320 { 00321 for(size_t i = 0 ; i < nRows ; i++) 00322 if(rows[i] != mat.rows[i]) 00323 return false; 00324 00325 return true; 00326 } 00327 00328 bool operator!=(const Matrix& mat) const 00329 { 00330 return (!((*this) == mat)); 00331 } 00332 00333 friend QTextStream& operator<<(QTextStream& out,const Matrix& mat) 00334 { 00335 for(size_t i = 0 ; i < nRows ; i++) 00336 { 00337 out << mat.rows[i]; 00338 if(i != (nRows - 1)) 00339 out << " "; 00340 } 00341 00342 return out; 00343 } 00344 00345 friend QTextStream& operator>>(QTextStream& in,Matrix& mat) 00346 { 00347 for(size_t i = 0 ; i < nRows && !in.atEnd() ; i++) 00348 in >> mat.rows[i]; 00349 return in ; 00350 } 00351 00352 friend std::ostream& operator<<(std::ostream& out,const Matrix& mat) 00353 { 00354 for(size_t i = 0 ; i < nRows ; i++) 00355 { 00356 out << mat.rows[i]; 00357 if(i != (nRows - 1)) 00358 out << " "; 00359 } 00360 00361 return out; 00362 } 00363 00364 friend std::istream& operator>>(std::istream& in,Matrix& mat) 00365 { 00366 for(size_t i = 0 ; i < nRows && in ; i++) 00367 in >> mat.rows[i]; 00368 return in ; 00369 } 00370 00376 Vector<nCols,T>& operator[](size_t idx) 00377 { 00378 return rows[idx]; 00379 } 00380 00386 Vector<nCols,T> operator[](size_t idx) const 00387 { 00388 return rows[idx]; 00389 } 00390 00394 T& operator()(size_t i,size_t j) 00395 { 00396 return rows[i][j]; 00397 } 00398 00402 T operator()(size_t i,size_t j) const 00403 { 00404 return rows[i][j]; 00405 } 00406 00410 static Matrix identity() 00411 { 00412 Matrix mat(1); 00413 return mat; 00414 } 00415 00419 Matrix& zero(void) 00420 { 00421 for(size_t i = 0 ; i < nRows ; i++) 00422 for(size_t j = 0 ; j < nCols ; j++) 00423 rows[i][j] = 0.0; 00424 return (*this); 00425 } 00426 00432 Matrix& operator=( const T& value ) 00433 { 00434 for( size_t i = 0 ; i < nRows ; ++i ) 00435 { 00436 for( size_t j = 0 ; j < nCols ; ++j ) 00437 { 00438 if( i == j ) 00439 rows[ i ][ j ] = value; 00440 else 00441 rows[ i ][ j ] = 0; 00442 } 00443 } 00444 return *this; 00445 } 00446 00450 Matrix<nCols,nRows,T> operator~() 00451 { 00452 Matrix<nCols,nRows,T> t; 00453 for( size_t i = 0 ; i < nRows ; ++i ) 00454 for( size_t j = 0 ; j < nCols ; ++j ) 00455 t[ i ][ j ] = rows[ j ][ i ]; 00456 return t; 00457 } 00458 00465 static Matrix<3,3,T> rotation( const Vector<3, T>& direction, T angle ) 00466 { 00467 T ca = std::cos( angle ); 00468 T sa = std::sin( angle ); 00469 Matrix<3,3,T> r; 00470 double x = direction.x(); 00471 double y = direction.y(); 00472 double z = direction.z(); 00473 r[ 0 ].set( ca+(1-ca)*x*x, (1-ca)*x*y-sa*z, (1-ca)*z*x+sa*y ); 00474 r[ 1 ].set( (1-ca)*y*x+sa*z, ca+(1-ca)*y*y, (1-ca)*z*y-sa*x ); 00475 r[ 2 ].set( (1-ca)*x*z-sa*y, (1-ca)*y*z+sa*x, ca+(1-ca)*z*z ); 00476 return r; 00477 } 00478 00485 static Matrix<4,4,T> rotation( const Vector<4, T>& direction, T angle ) 00486 { 00487 T ca = std::cos( angle ); 00488 T sa = std::sin( angle ); 00489 Matrix<4,4,T> r; 00490 double x = direction.x()/direction.t(); 00491 double y = direction.y()/direction.t(); 00492 double z = direction.z()/direction.t(); 00493 r[ 0 ].set( ca+(1-ca)*x*x, (1-ca)*x*y-sa*z, (1-ca)*z*x+sa*y, 0 ); 00494 r[ 1 ].set( (1-ca)*y*x+sa*z, ca+(1-ca)*y*y, (1-ca)*z*y-sa*x, 0 ); 00495 r[ 2 ].set( (1-ca)*x*z-sa*y, (1-ca)*y*z+sa*x, ca+(1-ca)*z*z, 0 ); 00496 r[ 3 ].set( 0, 0, 0, 1 ); 00497 return r; 00498 } 00499 00503 T trace() const 00504 { 00505 T acc = 0; 00506 for(size_t i = 0 ; i < std::min(nRows,nCols) ; ++i) 00507 { 00508 acc += rows[i][i]; 00509 } 00510 return acc; 00511 } 00512 00513 void fillArray(T* array, bool row_first = true) 00514 { 00515 if(row_first) 00516 { 00517 memcpy(array, &rows[0][0], sizeof(T)*nRows*nCols); 00518 } 00519 else 00520 { 00521 size_t k = 0; 00522 for(size_t c = 0 ; c < nCols ; ++c) 00523 for(size_t r = 0 ; r < nRows ; ++r, ++k) 00524 array[k] = rows[r][c]; 00525 } 00526 } 00527 00528 }; 00529 00534 template<size_t nRows,size_t nSize,size_t nCols,typename T> 00535 Matrix<nRows,nCols,T> operator*(const Matrix<nRows,nSize,T>& mat1, 00536 const Matrix<nSize,nCols,T>& mat2); 00537 00542 template<typename T> T det(const Matrix<1,1,T>& mat); 00547 template<typename T> T det(const Matrix<2,2,T>& mat); 00552 template<typename T> T det(const Matrix<3,3,T>& mat); 00559 template <size_t nRows, typename T> T det(const Matrix<nRows,nRows,T>& mat); 00560 00561 template <size_t nRows, typename T> 00562 T matrix_minor(const Matrix<nRows,nRows,T>& mat, size_t i, size_t j); 00563 00568 template <size_t nRows, typename T> 00569 T cofactor(const Matrix<nRows,nRows,T>& mat, size_t i, size_t j); 00570 00576 template <typename T> 00577 Matrix<1,1,T> inverse(const Matrix<1,1,T>& mat); 00583 template <typename T> 00584 Matrix<2,2,T> inverse(const Matrix<2,2,T>& mat); 00590 template <typename T> 00591 Matrix<3,3,T> inverse(const Matrix<3,3,T>& mat); 00592 00600 template <size_t nRows, typename T> 00601 Matrix<nRows,nRows,T> inverse(const Matrix<nRows,nRows,T>& mat); 00602 00608 template <size_t nRows, size_t nCols, typename T> 00609 Matrix<nCols,nRows,T> transpose( const Matrix<nRows, nCols, T>& mat); 00610 00619 template <size_t nRows, size_t nCols, typename T> 00620 T norm(const Matrix<nRows,nCols,T>& mat); 00621 00629 template <size_t nRows, size_t nCols, typename T> 00630 T normsq(const Matrix<nRows,nCols,T>& mat); 00631 00637 template <size_t nRows, size_t nCols, typename T> 00638 Matrix<nRows,nCols,T> map(const T& (*fct)(const T&), const Matrix<nRows,nCols,T>& m) 00639 { 00640 Matrix<nRows,nCols,T> result; 00641 for(size_t i = 0 ; i < nRows ; ++i) 00642 { 00643 const Vector<nCols,T>& mrow = m[i]; 00644 Vector<nCols,T>& rrow = result[i]; 00645 for(size_t j = 0 ; j < nCols ; ++j) 00646 { 00647 rrow[j] = (*fct)(mrow[j]); 00648 } 00649 } 00650 return result; 00651 } 00652 00658 template <size_t nRows, size_t nCols, typename T> 00659 Matrix<nRows,nCols,T> map(T (*fct)(T), const Matrix<nRows,nCols,T>& m) 00660 { 00661 Matrix<nRows,nCols,T> result; 00662 for(size_t i = 0 ; i < nRows ; ++i) 00663 { 00664 const Vector<nCols,T>& mrow = m[i]; 00665 Vector<nCols,T>& rrow = result[i]; 00666 for(size_t j = 0 ; j < nCols ; ++j) 00667 { 00668 rrow[j] = (*fct)(mrow[j]); 00669 } 00670 } 00671 return result; 00672 } 00673 00674 00680 template <size_t nRows, size_t nCols, typename T> 00681 Matrix<nRows,nCols,T> map(T (*fct)(const T&), const Matrix<nRows,nCols,T>& m) 00682 { 00683 Matrix<nRows,nCols,T> result; 00684 for(size_t i = 0 ; i < nRows ; ++i) 00685 { 00686 const Vector<nCols,T>& mrow = m[i]; 00687 Vector<nCols,T>& rrow = result[i]; 00688 for(size_t j = 0 ; j < nCols ; ++j) 00689 { 00690 rrow[j] = (*fct)(mrow[j]); 00691 } 00692 } 00693 return result; 00694 } 00695 00701 template <size_t nRows, size_t nCols, typename T, typename T1> 00702 Matrix<nRows,nCols,T> map(const T& (*fct)(const T1&), const Matrix<nRows,nCols,T1>& m) 00703 { 00704 Matrix<nRows,nCols,T> result; 00705 for(size_t i = 0 ; i < nRows ; ++i) 00706 { 00707 const Vector<nCols,T1>& mrow = m[i]; 00708 Vector<nCols,T>& rrow = result[i]; 00709 for(size_t j = 0 ; j < nCols ; ++j) 00710 { 00711 rrow[j] = (*fct)(mrow[j]); 00712 } 00713 } 00714 return result; 00715 } 00716 00722 template <size_t nRows, size_t nCols, typename T, typename T1> 00723 Matrix<nRows,nCols,T> map(T (*fct)(T1), const Matrix<nRows,nCols,T1>& m) 00724 { 00725 Matrix<nRows,nCols,T> result; 00726 for(size_t i = 0 ; i < nRows ; ++i) 00727 { 00728 const Vector<nCols,T1>& mrow = m[i]; 00729 Vector<nCols,T>& rrow = result[i]; 00730 for(size_t j = 0 ; j < nCols ; ++j) 00731 { 00732 rrow[j] = (*fct)(mrow[j]); 00733 } 00734 } 00735 return result; 00736 } 00737 00738 00744 template <size_t nRows, size_t nCols, typename T, typename T1> 00745 Matrix<nRows,nCols,T> map(T (*fct)(const T1&), const Matrix<nRows,nCols,T1>& m) 00746 { 00747 Matrix<nRows,nCols,T> result; 00748 for(size_t i = 0 ; i < nRows ; ++i) 00749 { 00750 const Vector<nCols,T1>& mrow = m[i]; 00751 Vector<nCols,T>& rrow = result[i]; 00752 for(size_t j = 0 ; j < nCols ; ++j) 00753 { 00754 rrow[j] = (*fct)(mrow[j]); 00755 } 00756 } 00757 return result; 00758 } 00759 00765 template <size_t nRows, size_t nCols, typename T> 00766 Matrix<nRows,nCols,T> map(T (*fct)(T,T), const Matrix<nRows,nCols,T>& m1, const Matrix<nRows,nCols,T>& m2) 00767 { 00768 Matrix<nRows,nCols,T> result; 00769 for(size_t i = 0 ; i < nRows ; ++i) 00770 { 00771 const Vector<nCols,T>& mrow1 = m1[i]; 00772 const Vector<nCols,T>& mrow2 = m2[i]; 00773 Vector<nCols,T>& rrow = result[i]; 00774 for(size_t j = 0 ; j < nCols ; ++j) 00775 { 00776 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00777 } 00778 } 00779 return result; 00780 } 00781 00787 template <size_t nRows, size_t nCols, typename T> 00788 Matrix<nRows,nCols,T> map(T (*fct)(const T&, const T&), const Matrix<nRows,nCols,T>& m1, const Matrix<nRows,nCols,T>& m2) 00789 { 00790 Matrix<nRows,nCols,T> result; 00791 for(size_t i = 0 ; i < nRows ; ++i) 00792 { 00793 const Vector<nCols,T>& mrow1 = m1[i]; 00794 const Vector<nCols,T>& mrow2 = m2[i]; 00795 Vector<nCols,T>& rrow = result[i]; 00796 for(size_t j = 0 ; j < nCols ; ++j) 00797 { 00798 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00799 } 00800 } 00801 return result; 00802 } 00803 00809 template <size_t nRows, size_t nCols, typename T> 00810 Matrix<nRows,nCols,T> map(const T& (*fct)(const T&, const T&), const Matrix<nRows,nCols,T>& m1, const Matrix<nRows,nCols,T>& m2) 00811 { 00812 Matrix<nRows,nCols,T> result; 00813 for(size_t i = 0 ; i < nRows ; ++i) 00814 { 00815 const Vector<nCols,T>& mrow1 = m1[i]; 00816 const Vector<nCols,T>& mrow2 = m2[i]; 00817 Vector<nCols,T>& rrow = result[i]; 00818 for(size_t j = 0 ; j < nCols ; ++j) 00819 { 00820 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00821 } 00822 } 00823 return result; 00824 } 00825 00831 template <size_t nRows, size_t nCols, typename T, typename T1, typename T2> 00832 Matrix<nRows,nCols,T> map(T (*fct)(T1,T2), const Matrix<nRows,nCols,T1>& m1, const Matrix<nRows,nCols,T2>& m2) 00833 { 00834 Matrix<nRows,nCols,T> result; 00835 for(size_t i = 0 ; i < nRows ; ++i) 00836 { 00837 const Vector<nCols,T1>& mrow1 = m1[i]; 00838 const Vector<nCols,T2>& mrow2 = m2[i]; 00839 Vector<nCols,T>& rrow = result[i]; 00840 for(size_t j = 0 ; j < nCols ; ++j) 00841 { 00842 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00843 } 00844 } 00845 return result; 00846 } 00847 00853 template <size_t nRows, size_t nCols, typename T, typename T1, typename T2> 00854 Matrix<nRows,nCols,T> map(T (*fct)(const T1&, const T2&), const Matrix<nRows,nCols,T1>& m1, const Matrix<nRows,nCols,T2>& m2) 00855 { 00856 Matrix<nRows,nCols,T> result; 00857 for(size_t i = 0 ; i < nRows ; ++i) 00858 { 00859 const Vector<nCols,T1>& mrow1 = m1[i]; 00860 const Vector<nCols,T2>& mrow2 = m2[i]; 00861 Vector<nCols,T>& rrow = result[i]; 00862 for(size_t j = 0 ; j < nCols ; ++j) 00863 { 00864 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00865 } 00866 } 00867 return result; 00868 } 00869 00875 template <size_t nRows, size_t nCols, typename T, typename T1, typename T2> 00876 Matrix<nRows,nCols,T> map(const T& (*fct)(const T1&, const T2&), const Matrix<nRows,nCols,T1>& m1, const Matrix<nRows,nCols,T2>& m2) 00877 { 00878 Matrix<nRows,nCols,T> result; 00879 for(size_t i = 0 ; i < nRows ; ++i) 00880 { 00881 const Vector<nCols,T1>& mrow1 = m1[i]; 00882 const Vector<nCols,T2>& mrow2 = m2[i]; 00883 Vector<nCols,T>& rrow = result[i]; 00884 for(size_t j = 0 ; j < nCols ; ++j) 00885 { 00886 rrow[j] = (*fct)(mrow1[j], mrow2[j]); 00887 } 00888 } 00889 return result; 00890 } 00891 00892 #include <util/matrix_impl.h> 00893 00894 } 00895 00896 #endif // __MATRIX_H