fixed_general_matrix.h
00001 /* Copyright (C) 2003-2005  Martin Förg
00002 
00003  * This library is free software; you can redistribute it and/or
00004  * modify it under the terms of the GNU Lesser General Public
00005  * License as published by the Free Software Foundation; either
00006  * version 2.1 of the License, or (at your option) any later version.
00007  * 
00008  * This library is distributed in the hope that it will be useful,
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011  * Lesser General Public License for more details.
00012  * 
00013  * You should have received a copy of the GNU Lesser General Public
00014  * License along with this library; if not, write to the Free Software
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00016  *
00017  * Contact:
00018  *   martin.o.foerg@googlemail.com
00019  *
00020  */
00021 
00022 #ifndef fixed_general_matrix_h
00023 #define fixed_general_matrix_h
00024 
00025 #include "types.h"
00026 #include <stdlib.h>
00027 
00028 namespace fmatvec {
00029 
00038   template <int M, int N, class AT> class Matrix<General,Fixed<M>,Fixed<N>,AT> {
00039 
00040     public:
00041 
00043 
00044     protected:
00045 
00046       AT ele[M][N];
00047 
00048       template <class Type, class Row, class Col> inline void deepCopy(const Matrix<Type,Row,Col,AT> &A); 
00049       template<class Row> inline void deepCopy(const Matrix<Symmetric,Row,Row,AT> &A);
00050 
00052  
00053     public:
00054 
00055 // Works with -std=gnu++0x only
00056 //      template<class Ini=All<AT> >
00057 //      Matrix(Ini ini=All<AT>()) {
00058 //        init(ini);
00059 //      }
00060 //      template<class Ini=All<AT> >
00061 //      Matrix(int m_, int n_, Ini ini=All<AT>()) {
00062 //        init(ini);
00063 //      }
00064 
00065       Matrix(Noinit) { }
00066       Matrix(Init ini=INIT, const AT &a=0) { init(a); }
00067       Matrix(Eye ini, const AT &a=1) { init(ini,a); }
00068       Matrix(int m, int n, Noinit) { }
00069       Matrix(int m, int n, Init ini=INIT, const AT &a=0) { init(a); }
00070       Matrix(int m, int n, Eye ini, const AT &a=1) { init(ini,a); }
00071 
00072       template<class Row, class Col>
00073       Matrix(const Matrix<General,Row,Col,AT> &A) {
00074 #ifndef FMATVEC_NO_SIZE_CHECK
00075         assert(A.rows() == M); 
00076         assert(A.cols() == N);
00077 #endif
00078 
00079         deepCopy(A);
00080       }
00081 
00082       template<class Type, class Row, class Col>
00083       explicit Matrix(const Matrix<Type,Row,Col,AT> &A) {
00084 
00085 #ifndef FMATVEC_NO_SIZE_CHECK
00086         assert(A.rows() == M); 
00087         assert(A.cols() == N);
00088 #endif
00089 
00090         deepCopy(A);
00091       }
00092 
00105       Matrix(const char *str);
00106 
00107       template <class Type, class Row, class Col>
00108       inline Matrix<General,Fixed<M>,Fixed<N>,AT>& operator=(const Matrix<Type,Row,Col,AT> &A);
00120       AT& operator()(int i, int j) {
00121 #ifndef FMATVEC_NO_BOUNDS_CHECK
00122         assert(i>=0);
00123         assert(j>=0);
00124         assert(i<M);
00125         assert(j<N);
00126 #endif
00127 
00128         return e(i,j);
00129       };
00130 
00135       const AT& operator()(int i, int j) const {
00136 #ifndef FMATVEC_NO_BOUNDS_CHECK
00137         assert(i>=0);
00138         assert(j>=0);
00139         assert(i<M);
00140         assert(j<N);
00141 #endif
00142 
00143         return e(i,j);
00144       };
00145 
00146       AT& e(int i, int j) {
00147         return ele[i][j];
00148       };
00149 
00154       const AT& e(int i, int j) const {
00155         return ele[i][j];
00156       };
00157 
00163       AT* operator()() {return &(ele[0][0]);};
00164 
00169       const AT* operator()() const {return &(ele[0][0]);};
00170 
00175       int rows() const {return M;};
00176 
00181       int cols() const {return N;};
00182 
00187       int ldim() const {return M;};
00188 
00195       const CBLAS_TRANSPOSE blasTrans() const {
00196         return CblasNoTrans;
00197       };
00198 
00206       const CBLAS_ORDER blasOrder() const {
00207         return  CblasRowMajor;
00208       };
00209 
00210 //      /*! \brief Submatrix operator.
00211 //       *
00212 //       * Returns a submatrix of the calling matrix. 
00213 //       * \attention The submatrix and the
00214 //       * calling matrix will share the same physical memory.
00215 //       * \param i1 The starting row. 
00216 //       * \param j1 The starting column.
00217 //       * \param i2 The ending row.
00218 //       * \param j2 The ending column.
00219 //       * \return A submatrix of the calling matrix.
00220 //       * */
00221 //      inline Matrix<General, AT> operator()(int i1, int j1, int i2, int j2);
00222 //
00223 //      /*! \brief Submatrix operator.
00224 //       *
00225 //       * See operator()(int,int,int,int);
00226 //       * */
00227 //      inline const Matrix<General, AT> operator()(int i1, int j1, int i2, int j2) const;
00228 
00255 //      inline Matrix<General, Var, AT> operator()(const Range<Var,Var> &I, const Range<Var,Var> &J);
00256 
00261       inline const Matrix<General,Var,Var,AT> operator()(const Range<Var,Var> &I, const Range<Var,Var> &J) const;
00262       
00263       template <int M1, int M2, int N1, int N2>
00264       inline const Matrix<General,Fixed<M2-M1+1>,Fixed<N2-N1+1>,AT> operator()(const Range<Fixed<M1>,Fixed<M2> > &I, const Range<Fixed<N1>,Fixed<N2> > &J) const;
00265       template <int M1, int M2>
00266       inline const Matrix<General,Fixed<M2-M1+1>,Var,AT> operator()(const Range<Fixed<M1>,Fixed<M2> > &I, const Range<Var,Var > &J) const;
00267 
00268       template <int N1, int N2>
00269       inline const Matrix<General,Var,Fixed<N2-N1+1>,AT> operator()(const Range<Var,Var> &I, const Range<Fixed<N1>,Fixed<N2> > &J) const;
00270 
00271       inline const RowVector<Fixed<N>,AT> row(int j) const;
00272       inline const Vector<Fixed<M>,AT> col(int i) const;
00273 
00281       inline Matrix<General,Fixed<M>,Fixed<N>,AT>& init(const AT &a=0); 
00282       inline Matrix<General,Fixed<M>,Fixed<N>,AT>& init(Init, const AT &a=0) { return init(a); };
00283       inline Matrix<General,Fixed<M>,Fixed<N>,AT>& init(Eye, const AT &a=1);
00284       inline Matrix<General,Fixed<M>,Fixed<N>,AT>& init(Noinit, const AT &a=0) { return *this; }
00285 
00290       inline operator std::vector<std::vector<AT> >();
00291 
00297       Matrix(std::vector<std::vector<AT> > m);
00298 
00299       inline const Matrix<General,Fixed<N>,Fixed<M>,AT> T() const;
00300 
00304       template<class Row> inline void set(int j, const Vector<Row,AT> &x);
00305 
00309       template<class Col> inline void set(int i, const RowVector<Col,AT> &x);
00310 
00317       template<class Type, class Row, class Col> inline void set(const Range<Var,Var> &I, const Range<Var,Var> &J, const Matrix<Type,Row,Col,AT> &A);
00318 
00322       template<class Row> inline void add(int j, const Vector<Row,AT> &x);
00323 
00327       template<class Col> inline void add(int i, const RowVector<Col,AT> &x);
00328 
00329       template<class Type, class Row, class Col> inline void add(const Range<Var,Var> &I, const Range<Var,Var> &J, const Matrix<Type,Row,Col,AT> &A);
00330 
00331   };
00332 
00333   template <int M, int N, class AT> 
00334     Matrix<General,Fixed<M>,Fixed<N>,AT>::Matrix(const char *strs) {
00335       // if 'strs' is a single scalar, surround it first with '[' and ']'.
00336       // This is more Matlab-like, because e.g. '5' and '[5]' is just the same.
00337       // (This functionallitiy is needed e.g. by MBXMLUtils (OpenMBV,MBSim))
00338       std::istringstream iss(strs);
00339       char c;
00340       iss>>c;
00341       if(c=='[') iss.str(strs);
00342       else iss.str(std::string("[")+strs+"]");
00343 
00344       int m=0, n=0;
00345       int buf=0;
00346       iss >> c;
00347       iss >> c;
00348       if(c!=']') {
00349         iss.putback(c);
00350         AT x;
00351         do {
00352           iss >> x;
00353           iss >> c;
00354           if(c==';') {
00355             if(buf)
00356               assert(buf == n);
00357 
00358             buf=n;
00359             n=0;
00360             m++;
00361           }
00362           else if(c==',')
00363             n++;
00364           c='0';
00365         } while(iss);
00366 
00367         n++; m++;
00368         iss.clear();
00369         iss.seekg(0);
00370         iss >> c;
00371         for(int i=0; i<M; i++)
00372           for(int j=0; j<N; j++) {
00373             iss >> e(i,j);
00374             iss >> c;
00375           }
00376       }
00377       assert(m==M);
00378       assert(n==N);
00379     }
00380 
00381   template <int M, int N, class AT> template<class Type, class Row, class Col>
00382     inline Matrix<General,Fixed<M>,Fixed<N>,AT>& Matrix<General,Fixed<M>,Fixed<N>,AT>::operator=(const Matrix<Type,Row,Col,AT> &A) { 
00383 
00384 #ifndef FMATVEC_NO_SIZE_CHECK
00385       assert(A.rows() == M); 
00386       assert(A.cols() == N);
00387 #endif
00388 
00389       deepCopy(A);
00390 
00391       return *this;
00392     }
00393 
00394   template <int M, int N, class AT>
00395     inline Matrix<General,Fixed<M>,Fixed<N>,AT>& Matrix<General,Fixed<M>,Fixed<N>,AT>::init(const AT &val) {
00396        for(int i=0; i<M; i++) 
00397         for(int j=0; j<N; j++) 
00398           e(i,j) = val;
00399       return *this;
00400     }
00401 
00402   template <int M, int N, class AT>
00403     inline Matrix<General,Fixed<M>,Fixed<N>,AT>& Matrix<General,Fixed<M>,Fixed<N>,AT>::init(Eye, const AT &val) {
00404       for(int i=0; i<M; i++)
00405         for(int j=0; j<N; j++)
00406           e(i,j) = (i==j) ? val : 0;
00407       return *this;
00408     }
00409 
00410   template <int M, int N, class AT> 
00411     inline const Matrix<General,Var,Var,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::operator()(const Range<Var,Var> &I, const Range<Var,Var> &J) const {
00412 #ifndef FMATVEC_NO_BOUNDS_CHECK
00413       assert(I.end()<M);
00414       assert(J.end()<N);
00415 #endif
00416       Matrix<General,Var,Var,AT> A(I.end()-I.start()+1,J.end()-J.start()+1,NONINIT);
00417 
00418       for(int i=0; i<A.rows(); i++) 
00419         for(int j=0; j<A.cols(); j++)
00420           A.e(i,j) = e(I.start()+i,J.start()+j);
00421 
00422       return A;
00423     }
00424 
00425  template <int M, int N, class AT> template <int M1, int M2, int N1, int N2>
00426     inline const Matrix<General,Fixed<M2-M1+1>,Fixed<N2-N1+1>,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::operator()(const Range<Fixed<M1>,Fixed<M2> > &I, const Range<Fixed<N1>,Fixed<N2> > &J) const {
00427 #ifndef FMATVEC_NO_BOUNDS_CHECK
00428       assert(M2<M);
00429       assert(N2<N);
00430 #endif
00431       Matrix<General,Fixed<M2-M1+1>,Fixed<N2-N1+1>,AT> A(NONINIT);
00432 
00433       for(int i=0; i<A.rows(); i++) 
00434         for(int j=0; j<A.cols(); j++)
00435           A.e(i,j) = e(M1+i,J.start()+j);
00436 
00437       return A;
00438     }
00439 
00440  template <int M, int N, class AT> template <int M1, int M2>
00441     inline const Matrix<General,Fixed<M2-M1+1>,Var,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::operator()(const Range<Fixed<M1>,Fixed<M2> > &I, const Range<Var,Var> &J) const {
00442 #ifndef FMATVEC_NO_BOUNDS_CHECK
00443       assert(M2<M);
00444       assert(J.end()<N);
00445 #endif
00446       Matrix<General,Fixed<M2-M1+1>,Var,AT> A(J.end()-J.start()+1,NONINIT);
00447 
00448       for(int i=0; i<A.rows(); i++) 
00449         for(int j=0; j<A.cols(); j++)
00450           A.e(i,j) = e(I.start()+i,J.start()+j);
00451 
00452       return A;
00453     }
00454 
00455   template <int M, int N, class AT> template <int N1, int N2>
00456     inline const Matrix<General,Var,Fixed<N2-N1+1>,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::operator()(const Range<Var,Var> &I, const Range<Fixed<N1>,Fixed<N2> > &J) const {
00457 #ifndef FMATVEC_NO_BOUNDS_CHECK
00458       assert(I.end()<M);
00459       assert(N2<N);
00460 #endif
00461       Matrix<General,Var,Fixed<N2-N1+1>,AT> A(I.end()-I.start()+1,NONINIT);
00462 
00463       for(int i=0; i<A.rows(); i++) 
00464         for(int j=0; j<A.cols(); j++)
00465           A.e(i,j) = e(I.start()+i,J.start()+j);
00466 
00467       return A;
00468     }
00469 
00470   template <int M, int N, class AT>
00471     inline const RowVector<Fixed<N>,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::row(int i) const {
00472 
00473 #ifndef FMATVEC_NO_BOUNDS_CHECK
00474       assert(i>=0);
00475       assert(i<M);
00476 #endif
00477 
00478       RowVector<Fixed<N>,AT> x(NONINIT);
00479 
00480       for(int j=0; j<N; j++)
00481         x.e(j) = e(i,j);
00482 
00483       return x;
00484 
00485     }
00486 
00487   template <int M, int N, class AT>
00488     inline const Vector<Fixed<M>,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::col(int j) const {
00489 
00490 #ifndef FMATVEC_NO_BOUNDS_CHECK
00491       assert(j>=0);
00492       assert(j<N);
00493 #endif
00494 
00495       Vector<Fixed<M>,AT> x(NONINIT);
00496 
00497       for(int i=0; i<M; i++)
00498         x.e(i) = e(i,j);
00499 
00500       return x;
00501 
00502     }
00503 
00504   template <int M, int N, class AT>
00505     inline const Matrix<General,Fixed<N>,Fixed<M>,AT> Matrix<General,Fixed<M>,Fixed<N>,AT>::T() const {
00506       Matrix<General,Fixed<N>,Fixed<M>,AT> A(NONINIT);
00507       for(int i=0; i<N; i++)
00508         for(int j=0; j<M; j++)
00509           A.e(i,j) = e(j,i);
00510       return A;
00511     }
00512 
00513   template <int M, int N, class AT> template <class Row>
00514     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::set(int j, const Vector<Row,AT> &x) {
00515 #ifndef FMATVEC_NO_BOUNDS_CHECK
00516       assert(j<cols());
00517       assert(rows()==x.size());
00518 #endif
00519       for(int i=0; i<rows(); i++)
00520         e(i,j) = x.e(i);
00521     }
00522 
00523   template <int M, int N, class AT> template <class Col>
00524     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::set(int i, const RowVector<Col,AT> &x) {
00525 #ifndef FMATVEC_NO_BOUNDS_CHECK
00526       assert(i<rows());
00527       assert(cols()==x.size());
00528 #endif
00529       for(int j=0; j<cols(); j++)
00530         e(i,j) = x.e(j);
00531     }
00532 
00533   template <int M, int N, class AT> template<class Type, class Row, class Col>
00534     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::set(const Range<Var,Var> &I, const Range<Var,Var> &J, const Matrix<Type,Row,Col,AT> &A) {
00535 
00536 #ifndef FMATVEC_NO_BOUNDS_CHECK
00537       assert(I.end()<rows());
00538       assert(J.end()<cols());
00539       assert(I.size()==A.rows());
00540       assert(J.size()==A.cols());
00541 #endif
00542 
00543       for(int i=I.start(), ii=0; i<=I.end(); i++, ii++)
00544         for(int j=J.start(), jj=0; j<=J.end(); j++, jj++)
00545           e(i,j) = A.e(ii,jj);
00546     }
00547 
00548   template <int M, int N, class AT> template <class Row>
00549     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::add(int j, const Vector<Row,AT> &x) {
00550 #ifndef FMATVEC_NO_BOUNDS_CHECK
00551       assert(j<cols());
00552       assert(rows()==x.size());
00553 #endif
00554       for(int i=0; i<rows(); i++)
00555         e(i,j) += x.e(i);
00556     }
00557 
00558   template <int M, int N, class AT> template <class Col>
00559     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::add(int i, const RowVector<Col,AT> &x) {
00560 #ifndef FMATVEC_NO_BOUNDS_CHECK
00561       assert(i<rows());
00562       assert(cols()==x.size());
00563 #endif
00564       for(int j=0; j<cols(); j++)
00565         e(i,j) += x.e(j);
00566     }
00567 
00568   template <int M, int N, class AT> template<class Type, class Row, class Col>
00569     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::add(const Range<Var,Var> &I, const Range<Var,Var> &J, const Matrix<Type,Row,Col,AT> &A) {
00570 
00571 #ifndef FMATVEC_NO_BOUNDS_CHECK
00572       assert(I.end()<rows());
00573       assert(J.end()<cols());
00574       assert(I.size()==A.rows());
00575       assert(J.size()==A.cols());
00576 #endif
00577 
00578       for(int i=I.start(), ii=0; i<=I.end(); i++, ii++)
00579         for(int j=J.start(), jj=0; j<=J.end(); j++, jj++)
00580           e(i,j) += A.e(ii,jj);
00581     }
00582 
00583   template <int M, int N, class AT>
00584     inline Matrix<General,Fixed<M>,Fixed<N>,AT>::operator std::vector<std::vector<AT> >() {
00585       std::vector<std::vector<AT> > ret(rows());
00586       for(int r=0; r<rows(); r++) {
00587         ret[r].resize(cols());
00588         for(int c=0; c<cols(); c++)
00589           ret[r][c]=e(r,c);
00590       }
00591       return ret;
00592     }
00593 
00594   template <int M, int N, class AT>
00595     inline Matrix<General,Fixed<M>,Fixed<N>,AT>::Matrix(std::vector<std::vector<AT> > m) {
00596 #ifndef FMATVEC_NO_SIZE_CHECK
00597       assert(m.size() == M);
00598       assert(m[0].size() == N);
00599 #endif
00600       for(int r=0; r<rows(); r++) {
00601         assert(m[r].size()==cols());
00602         for(int c=0; c<cols(); c++)
00603           e(r,c)=m[r][c];
00604       }
00605     }
00606 
00608 
00609   template <int M, int N, class AT> template <class Type, class Row, class Col>
00610     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::deepCopy(const Matrix<Type,Row,Col,AT> &A) { 
00611       for(int i=0; i<M; i++) 
00612         for(int j=0; j<N; j++)
00613           e(i,j) = A.e(i,j);
00614     }
00615 
00616   template<int M, int N, class AT> template<class Row>
00617     inline void Matrix<General,Fixed<M>,Fixed<N>,AT>::deepCopy(const Matrix<Symmetric,Row,Row,AT> &A) {
00618       for(int i=0; i<A.size(); i++) {
00619         e(i,i) = A.ej(i,i);
00620         for(int j=i+1; j<A.size(); j++)
00621           e(i,j) = e(j,i) = A.ej(i,j);
00622       }
00623     }
00624 
00626 
00634   template<int M>
00635   class Matrix<Rotation,Fixed<M>,Fixed<M>,double> : public Matrix<General,Fixed<M>,Fixed<M>,double> {
00636     public:
00637       // Constructors are not inherited. Hence we must redefine all ctors here.
00638 
00639       Matrix(Noinit ini) { }
00640       Matrix(Init ini=INIT, const double &a=0) { this->init(a); }
00641       Matrix(Eye ini, const double &a=1) { this->init(ini,a); }
00642       Matrix(int m, int n, Noinit ini) { }
00643       Matrix(int m, int n, Init ini, const double &a=0) { this->init(a); }
00644       Matrix(int m, int n, Eye ini, const double &a=1) { this->init(ini,a); }
00645       explicit Matrix(const Matrix<General,Fixed<M>,Fixed<M>,double>&  A) { this->deepCopy(A); }
00646 
00647       template<class Row>
00648       Matrix(const Matrix<Symmetric,Row,Row,double> &A) {
00649 #ifndef FMATVEC_NO_SIZE_CHECK
00650         assert(A.size() == M); 
00651 #endif
00652         this->deepCopy(A);
00653       }
00654 
00655       template<class Type, class Row, class Col>
00656       explicit Matrix(const Matrix<Type,Row,Col,double> &A) {
00657 #ifndef FMATVEC_NO_SIZE_CHECK
00658         assert(A.rows() == M); 
00659         assert(A.cols() == M);
00660 #endif
00661         this->deepCopy(A);
00662       }
00663 
00664       // TODO: we should add some overloaded member functions here which capitalize the special
00665       // properties of rotation matrices, link "RotMat inv() { return trans(*this); }"
00666   };
00667 
00668 
00669 }
00670 
00671 #endif