00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef _matrix_h_
00027 #define _matrix_h_
00028
00029 #include <assert.h>
00030 #include <iostream>
00031 #include <vector>
00032 #include <list>
00033 #include <map>
00034 #include <memory>
00035 #include <cstdio>
00036
00037 class transf;
00038 class mat3;
00039
00041
00074 class Matrix{
00075 private:
00077 double *mData;
00079 void initialize(int m, int n);
00081 void setFromColMajor(const double *M);
00083 void setFromRowMajor(const double *M);
00084
00086 mutable int mSequentialI;
00088 mutable int mSequentialJ;
00089 protected:
00091 int mRows, mCols;
00092 public:
00093 enum Type{DENSE, SPARSE};
00095 Matrix(int m, int n);
00097 Matrix(const Matrix &M);
00099 Matrix(const double *M, int m, int n, bool colMajor);
00100
00101 virtual ~Matrix();
00102
00104 virtual void resize(int m, int n);
00105
00106 virtual Type getType() const {return DENSE;}
00108 virtual double& elem(int m, int n);
00110 virtual const double& elem(int m, int n) const;
00112 virtual std::auto_ptr<double> getDataCopy() const;
00114 virtual void getData(std::vector<double> *data) const;
00116 virtual double* getDataPointer();
00117
00119 int rows() const {return mRows;}
00121 int cols() const {return mCols;}
00122 Matrix getColumn(int c) const;
00123 Matrix getRow(int r) const;
00124 Matrix getSubMatrix(int startRow, int startCol, int rows, int cols) const;
00125
00126 virtual void setAllElements(double val);
00128 virtual void copySubBlock(int startRow, int startCol, int rows, int cols,
00129 const Matrix &m, int startMRow, int startMCol);
00131 void copySubMatrix(int startRow, int startCol, const Matrix &m){
00132 copySubBlock(startRow, startCol, m.rows(), m.cols(), m, 0, 0);}
00134 void copyMatrix(const Matrix &m){copySubMatrix(0, 0, m);}
00135
00136 friend std::ostream& operator<<(std::ostream &os, const Matrix &m);
00137 void print(FILE *fp = stderr) const;
00138
00140 virtual void sequentialReset() const;
00142 virtual bool nextSequentialElement(int &i, int &j, double &val) const;
00144
00145 virtual double getDefault() const {return 0.0;}
00146
00148 virtual int numElements() const {return mRows * mCols;}
00150 int rank() const;
00152 double fnorm() const;
00154 double absMax() const;
00156 double elementSum() const;
00157 void swapRows(int r1, int r2);
00158 void swapCols(int c1, int c2);
00160 virtual void transpose();
00162 void eye();
00164 void multiply(double s);
00165
00167 Matrix transposed() const;
00168
00170 static Matrix EYE(int m, int n);
00172 static Matrix NEGEYE(int m, int n);
00173
00175 static Matrix PERMUTATION(int n, int *jpvt);
00177 static Matrix TRANSFORM(const transf &t);
00179 static Matrix ROTATION(const mat3 &rot);
00181 static Matrix ROTATION2D(double theta);
00183 static Matrix MAX_VECTOR(int rows);
00185 static Matrix MIN_VECTOR(int rows);
00186
00188 template <class MatrixType>
00189 static MatrixType ZEROES(int m, int n);
00190
00192 template <class MatrixType>
00193 static MatrixType BLOCKDIAG(const Matrix &M1, const Matrix &M2);
00195 template <class MatrixType>
00196 static MatrixType BLOCKCOLUMN(const Matrix &M1, const Matrix &M2);
00198 template <class MatrixType>
00199 static MatrixType BLOCKROW(const Matrix &M1, const Matrix &M2);
00200
00202 template <class MatrixType>
00203 static MatrixType BLOCKDIAG(std::list<Matrix*> *blocks);
00205 template <class MatrixType>
00206 static MatrixType BLOCKCOLUMN(std::list<Matrix*> *blocks);
00208 template <class MatrixType>
00209 static MatrixType BLOCKROW(std::list<Matrix*> *blocks);
00210
00212 static const double EPS;
00213 };
00214
00242 class SparseMatrix : public Matrix
00243 {
00244 private:
00246 double mDefaultValue;
00248 double mFoo;
00250
00251 std::map<int, double> mSparseData;
00252
00254
00258 int key(int m, int n) const{
00259 return n*mRows + m;
00260 }
00262 void reverseKey(int k, int &m, int &n) const {
00263 m = k % mRows;
00264 n = k / mRows;
00265 assert( key(m,n) == k );
00266 }
00267
00268 mutable std::map<int, double>::const_iterator mSequentialIt;
00269 public:
00271 SparseMatrix(int m, int n, double defaultValue=0.0) : Matrix(0,0){
00272
00273 mRows = m;
00274 mCols = n;
00275 mDefaultValue = defaultValue;
00276 mFoo = 0.0;
00277 }
00279 SparseMatrix(const SparseMatrix &SM);
00280 ~SparseMatrix() {
00281
00282 mRows = 0;
00283 }
00284
00286 virtual void resize(int m, int n);
00287
00288 virtual Type getType() const {return SPARSE;}
00290 virtual double getDefault() const {return mDefaultValue;}
00291
00293 virtual int numElements() const {
00294 return mSparseData.size();
00295 }
00296
00297 virtual double& elem(int m, int n);
00298 virtual const double& elem(int m, int n) const;
00299 virtual std::auto_ptr<double> getDataCopy() const;
00300 virtual void getData(std::vector<double> *data) const;
00302 virtual double* getDataPointer();
00303 virtual void transpose();
00304
00306 void copySubBlock(int startRow, int startCol, int rows, int cols,
00307 const Matrix &m, int startMRow, int startMCol);
00308
00309 virtual void setAllElements(double val){
00310 mDefaultValue = val;
00311 mSparseData.clear();
00312 }
00313
00315 static SparseMatrix EYE(int m, int n);
00317 static SparseMatrix NEGEYE(int m, int n);
00318
00320 virtual void sequentialReset() const;
00322 virtual bool nextSequentialElement(int &i, int &j, double &val) const;
00323 };
00324
00326 void matrixMultiply(const Matrix &L, const Matrix &R, Matrix &M);
00328 void matrixAdd(const Matrix &L, const Matrix &R, Matrix &M);
00330 bool matrixEqual(const Matrix &R, const Matrix &L);
00332 int triangularSolve(Matrix &A, Matrix &B);
00334 int linearSolveMPInv(Matrix &A, Matrix &B, Matrix &X);
00336 int linearSolveSVD(Matrix &A, Matrix &B, Matrix &X);
00338 int underDeterminedSolveQR(Matrix &A, Matrix &B, Matrix &X);
00340 int matrixInverse(const Matrix &A, Matrix &AInv);
00342 int QPSolver(const Matrix &Q,
00343 const Matrix &Eq, const Matrix &b,
00344 const Matrix &InEq, const Matrix &ib,
00345 const Matrix &lowerBounds, const Matrix &upperBounds,
00346 Matrix &sol, double *objVal);
00348 int factorizedQPSolver(const Matrix &Qf,
00349 const Matrix &Eq, const Matrix &b,
00350 const Matrix &InEq, const Matrix &ib,
00351 const Matrix &lowerBounds, const Matrix &upperBounds,
00352 Matrix &sol, double *objVal);
00354 void testQP();
00356 int LPSolver(const Matrix &Q,
00357 const Matrix &Eq, const Matrix &b,
00358 const Matrix &InEq, const Matrix &ib,
00359 const Matrix &lowerBounds, const Matrix &upperBounds,
00360 Matrix &sol, double *objVal);
00362 void testLP();
00363
00364 template <class MatrixType>
00365 MatrixType Matrix::ZEROES(int m, int n)
00366 {
00367 assert( m>0 && n>0 );
00368 MatrixType Z(m,n);
00369 Z.setAllElements(0.0);
00370 return Z;
00371 }
00372
00373 template <class MatrixType>
00374 MatrixType Matrix::BLOCKROW(std::list<Matrix*> *blocks)
00375 {
00376 int numCols = 0;
00377 std::list<Matrix*>::iterator it;
00378 int numRows = 0;
00379 for(it=blocks->begin(); it!=blocks->end(); it++) {
00380 numCols += (*it)->cols();
00381 if ( (*it)->cols() ) {
00382 if (numRows == 0) {
00383 numRows = (*it)->rows();
00384 } else {
00385 assert((*it)->rows() == numRows);
00386 }
00387 }
00388 }
00389 if (!numCols) return MatrixType(0, 0);
00390 MatrixType C(numRows, numCols);
00391 numCols = 0;
00392 for(it=blocks->begin(); it!=blocks->end(); it++) {
00393 if (!(*it)->cols()) continue;
00394 C.copySubMatrix(0, numCols, *(*it));
00395 numCols += (*it)->cols();
00396 }
00397 return C;
00398 }
00399
00400
00401 template <class MatrixType>
00402 MatrixType Matrix::BLOCKDIAG(std::list<Matrix*> *blocks)
00403 {
00404 int numRows=0, numCols=0;
00405 std::list<Matrix*>::iterator it;
00406 for(it=blocks->begin(); it!=blocks->end(); it++) {
00407 numRows += (*it)->rows();
00408 numCols += (*it)->cols();
00409 }
00410 if (!numRows || !numCols) return MatrixType(numRows, numCols);
00411 MatrixType B(numRows, numCols);
00412 B.setAllElements(0.0);
00413 numRows = numCols = 0;
00414 for(it=blocks->begin(); it!=blocks->end(); it++) {
00415 if (!(*it)->rows() || !(*it)->cols()) continue;
00416 B.copySubMatrix(numRows, numCols, *(*it));
00417 numRows += (*it)->rows();
00418 numCols += (*it)->cols();
00419 }
00420 return B;
00421 }
00422
00423 template <class MatrixType>
00424 MatrixType Matrix::BLOCKCOLUMN(std::list<Matrix*> *blocks)
00425 {
00426 int numRows = 0;
00427 std::list<Matrix*>::iterator it;
00428 int numCols = 0;
00429 for(it=blocks->begin(); it!=blocks->end(); it++) {
00430 numRows += (*it)->rows();
00431 if ( (*it)->rows() ) {
00432 if (numCols == 0) {
00433 numCols = (*it)->cols();
00434 } else {
00435 assert((*it)->cols() == numCols);
00436 }
00437 }
00438 }
00439 if (!numRows) return MatrixType(0, 0);
00440 MatrixType C(numRows, numCols);
00441 numRows = 0;
00442 for(it=blocks->begin(); it!=blocks->end(); it++) {
00443 if (!(*it)->rows()) continue;
00444 C.copySubMatrix(numRows, 0, *(*it));
00445 numRows += (*it)->rows();
00446 }
00447 return C;
00448 }
00449
00450 template <class MatrixType>
00451 MatrixType Matrix::BLOCKCOLUMN(const Matrix &M1, const Matrix &M2)
00452 {
00453 if ( M1.rows() && M2.rows() ) {
00454 assert(M1.cols() == M2.cols());
00455 }
00456 MatrixType M(M1.rows() + M2.rows(), std::max(M1.cols(), M2.cols()));
00457 M.copySubMatrix(0, 0, M1);
00458 M.copySubMatrix(M1.rows(), 0, M2);
00459 return M;
00460 }
00461
00462 template <class MatrixType>
00463 MatrixType Matrix::BLOCKROW(const Matrix &M1, const Matrix &M2)
00464 {
00465 assert(M1.rows() == M2.rows());
00466 MatrixType M(M1.rows(), M1.cols() + M2.cols());
00467 M.copySubMatrix(0, 0, M1);
00468 M.copySubMatrix(0, M1.cols(), M2);
00469 return M;
00470 }
00471
00472 template <class MatrixType>
00473 MatrixType Matrix::BLOCKDIAG(const Matrix &M1, const Matrix &M2)
00474 {
00475 MatrixType M( M1.rows() + M2.rows(), M1.cols() + M2.cols() );
00476 M.setAllElements(0.0);
00477 M.copySubMatrix(0, 0, M1);
00478 M.copySubMatrix(M1.rows(), M1.cols(), M2);
00479 return M;
00480 }
00481
00482
00483 #endif
