Matrix.h

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Eric Sauser, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #ifndef MATRIX_H
00020 #define MATRIX_H
00021 
00022 #ifdef WIN32
00023 #include "windows.h"
00024 #endif
00025 
00026 #include <math.h>
00027 #include <stdio.h>
00028 #include <stdlib.h>
00029 #include <iostream>
00030 
00031 #include "Macros.h"
00032 #include "Vector.h"
00033 
00034 #ifdef  USE_T_EXTENSIONS
00035 template<unsigned int ROW> class TMatrix;
00036 #endif
00037     
00038 class Matrix
00039 {
00040   friend class Vector;
00041 #ifdef  USE_T_EXTENSIONS
00042   template<unsigned int ROW> friend class TMatrix;
00043 #endif
00044   
00045 protected: 
00046   static int bInverseOk;
00047   
00048   unsigned int  row;    //number of rows
00049   unsigned int  column;//number of columns
00050   float        *_;
00051 
00052 public:
00053 
00054   inline Matrix() {
00055     row    = 0;
00056     column = 0;
00057     _      = NULL;
00058   }
00059   
00060   inline virtual ~Matrix(){
00061     Release(); 
00062   }
00063 
00064   inline Matrix(const Matrix &matrix)
00065   {
00066     row    = 0;
00067     column = 0;
00068     _      = NULL;
00069     Resize(matrix.row,matrix.column,false);
00070     for (unsigned int j = 0; j < row; j++)
00071       for (unsigned int i = 0; i < column; i++)
00072         _[j*column+i] = matrix._[j*column+i];
00073   }
00074 
00075   inline Matrix(unsigned int rowSize, unsigned int colSize, bool clear = true)
00076   {
00077     row    = 0;
00078     column = 0;
00079     _      = NULL;
00080     Resize(rowSize,colSize,false);
00081     if(clear)
00082       Zero();
00083   }
00084   
00085   inline Matrix(const float _[], unsigned int rowSize, unsigned int colSize)
00086   {
00087     row       = 0;
00088     column    = 0;
00089     this->_   = NULL;
00090     Resize(rowSize,colSize,false);
00091     for (unsigned int j = 0; j < row; j++)
00092       for (unsigned int i = 0; i < column; i++)
00093         this->_[j*column+i] = _[j*column+i];
00094   }
00095 
00096 #ifdef  USE_T_EXTENSIONS
00097   template<unsigned int ROW> inline Matrix(const TMatrix<ROW> &matrix)
00098   {
00099     row    = 0;
00100     column = 0;
00101     _      = NULL;
00102     Resize(ROW,ROW,false);
00103     for (unsigned int j = 0; j < row; j++)
00104       for (unsigned int i = 0; i < column; i++)
00105         _[j*column+i] = matrix._[j*column+i];
00106   }
00107 #endif
00108    
00109   inline Matrix& Zero()
00110   {
00111     for (unsigned int j = 0; j < row; j++)
00112       for (unsigned int i = 0; i < column; i++)
00113         _[j*column+i] = 0.0f;
00114     return *this;
00115   }
00116 
00117   //actually the size of the column (or the number of rows)    
00118   inline unsigned int RowSize() const{
00119     return row;
00120   }
00121 
00122   //actually the size of the row (or the number of columns)    
00123   inline unsigned int ColumnSize() const{
00124     return column;
00125   } 
00126   inline float *Array() const{
00127     return _;
00128   }
00129 
00130   inline float& operator() (const unsigned int row, const unsigned int col)
00131   {
00132     if((row<this->row)&&(col<this->column))
00133       return _[row*column+col];
00134     return Vector::undef; 
00135   }
00136 
00137   inline Vector GetRow(const unsigned int row) const
00138   {
00139     Vector result(column,false);    
00140     return GetRow(row,result);     
00141   }
00142 
00143   inline Vector& GetRow(const unsigned int row, Vector& result) const
00144   {
00145     result.Resize(column,false);
00146     for (unsigned int i = 0; i < column; i++)
00147       result._[i] = _[row*column+i];
00148     return result;     
00149   }
00150 
00151   inline Vector GetColumn(const unsigned int col) const
00152   {
00153     Vector result(row,false);    
00154     return GetColumn(col,result);     
00155   }
00156 
00157   inline Vector& GetColumn(const unsigned int col, Vector& result) const
00158   {
00159     result.Resize(row,false);
00160     if(col<column){
00161       for (unsigned int j = 0; j < row; j++)
00162         result._[j] = _[j*column+col];
00163     }else{
00164       result.Zero();
00165     }
00166     return result;     
00167   }
00168 
00169   inline Matrix GetColumnSpace(const unsigned int col, const unsigned int len) const  
00170   {
00171     if(len>0){
00172       Matrix result(row,len,false);    
00173       return GetColumnSpace(col,len,result);
00174     }else
00175       return Matrix();     
00176   }
00177 
00178   inline Matrix GetRowSpace(const unsigned int row, const unsigned int len) const
00179   {
00180     if(len>0){
00181       Matrix result(len,column,false);    
00182       return GetRowSpace(row,len,result);
00183     }else
00184       return Matrix();     
00185   }
00186 
00187 
00188   inline Matrix& GetColumnSpace(const unsigned int col, const unsigned int len, Matrix &result) const
00189   {
00190     if(len>0){
00191       const unsigned int end  = col+len-1;    
00192       const unsigned int size = len; 
00193       result.Resize(row,size,false);
00194       
00195       if(col<column){
00196         const unsigned int k = (end+1<=column?end+1:column);  
00197         
00198         for (unsigned int i = col; i < k; i++)
00199           for (unsigned int j = 0; j < row; j++)
00200             result._[j*size+(i-col)] = _[j*column+i];
00201         for (unsigned int i = k; i < end+1; i++)
00202           for (unsigned int j = 0; j < row; j++)
00203            result._[j*size+(i-col)] = 0.0f;            
00204       }else{
00205         result.Zero();
00206       }
00207     }else{
00208       result.Resize(0,0,false);
00209     }
00210     return result;     
00211   }
00212 
00213   inline Matrix& GetRowSpace(const unsigned int row, const unsigned int len, Matrix &result) const
00214   {      
00215     if(len>0){
00216       const unsigned int end  = row+len-1;
00217       const unsigned int size = len; 
00218       result.Resize(size,column,false);
00219       
00220       if(row<this->row){
00221         const unsigned int k = (end+1<=this->row?end+1:this->row);  
00222         
00223         for (unsigned int j = 0; j < column; j++)
00224           for (unsigned int i = row; i < k; i++)
00225             result._[(i-row)*column+j] = _[i*column+j];
00226         for (unsigned int j = 0; j < column; j++)
00227           for (unsigned int i = k; i < end+1; i++)
00228            result._[(i-row)*column+j] = 0.0f;            
00229       }else{
00230         result.Zero();
00231       }
00232     }else{
00233       result.Resize(0,0,false);
00234     }
00235     return result;     
00236   }
00237 
00238   inline Matrix& SetRow(const Vector &vector, const unsigned int row)
00239   {
00240     if(row<this->row){    
00241       const unsigned int ki = (column<=vector.row?column:vector.row);
00242       for (unsigned int i = 0; i < ki; i++)
00243         _[row*column+i] = vector._[i]; 
00244     }
00245     return *this;     
00246   }
00247 
00248   inline Matrix& SetColumn(const Vector &vector, const unsigned int col)
00249   {
00250     if(col<this->column){    
00251       const unsigned int kj = (row<=vector.row?row:vector.row);
00252       for (unsigned int j = 0; j < kj; j++)
00253         _[j*column+col] = vector._[j];
00254     }
00255     return *this;
00256   }
00257 
00258   inline Matrix& SetColumnSpace(const Matrix &matrix, const unsigned int col)
00259   {
00260     if(col<this->column){    
00261       const unsigned int kj = (row<=matrix.row?row:matrix.row);
00262       const unsigned int ki = (col+matrix.column<=this->column?col+matrix.column:this->column);
00263       for (unsigned int j = 0; j < kj; j++)
00264         for (unsigned int i = col; i < ki; i++)
00265           _[j*column+i] = matrix._[j*matrix.column+(i-col)];
00266     }
00267     return *this;
00268   }
00269 
00270   inline Matrix& SetRowSpace(const Matrix &matrix, const unsigned int row)
00271   {
00272     if(row<this->row){
00273       const unsigned int ki = (column<=matrix.column?column:matrix.column);
00274       const unsigned int kj = (row+matrix.row<=this->row?row+matrix.row:this->row);
00275       for (unsigned int j = row; j < kj; j++)
00276         for (unsigned int i = 0; i < ki; i++)
00277           _[j*column+i] = matrix._[(j-row)*matrix.column+i]; 
00278     }
00279     return *this;     
00280   }
00281 
00282   inline Matrix GetRowSpace(const Vector &ids) const
00283   {
00284     Matrix result(ids.Size(),column);
00285     return GetRowSpace(ids,result);
00286   }
00287 
00288   inline Matrix GetColumnSpace(const Vector &ids) const
00289   {
00290     Matrix result(row,ids.Size());
00291     return GetColumnSpace(ids,result);
00292   }
00293 
00294   inline Matrix GetMatrixSpace(const Vector &rowIds,const Vector &colIds) const
00295   {
00296     Matrix result(rowIds.Size(),colIds.Size());
00297     return GetMatrixSpace(rowIds,colIds,result);
00298   }
00299 
00300   inline Matrix& GetColumnSpace(const Vector &ids, Matrix &result) const
00301   {
00302     const unsigned int k=ids.Size();
00303     result.Resize(row,k);
00304     for(unsigned int i=0;i<k;i++){
00305       const unsigned int g = (unsigned int)(fabs(ROUND(ids._[i])));
00306       if(g<column){
00307         for(unsigned int j=0;j<row;j++)
00308           result._[j*k+i] = _[j*column+g];
00309       }else{
00310         for(unsigned int j=0;j<row;j++)
00311           result._[j*k+i] = 0.0f;        
00312       }
00313     }
00314     return result;     
00315   }
00316 
00317   inline Matrix& GetRowSpace(const Vector &ids, Matrix &result) const
00318   {
00319     const unsigned int k=ids.Size();
00320     result.Resize(k,column);
00321     for(unsigned int i=0;i<k;i++){
00322       const unsigned int g = (unsigned int)(fabs(ROUND(ids._[i])));
00323       if(g<row){
00324         for(unsigned int j=0;j<column;j++)
00325           result._[i*column+j] = _[g*column+j];
00326       }else{
00327         for(unsigned int j=0;j<column;j++)
00328           result._[i*column+j] = 0.0f;
00329       }
00330     }
00331     return result;     
00332   }
00333 
00334   inline Matrix& GetMatrixSpace(const Vector &rowIds,const Vector &colIds, Matrix &result) const
00335   {
00336     const unsigned int k1=rowIds.Size();
00337     const unsigned int k2=colIds.Size();
00338     result.Resize(k1,k2);
00339     for(unsigned int i=0;i<k1;i++){
00340       const unsigned int g1 = (unsigned int)(fabs(ROUND(rowIds._[i])));
00341       if(g1<row){
00342         for(unsigned int j=0;j<k2;j++){      
00343           const unsigned int g2 = (unsigned int)(fabs(ROUND(colIds._[j])));
00344           if(g2<column){
00345             result._[i*k2+j] = _[g1*column+g2];            
00346           }else{
00347             result._[i*k2+j] = 0.0f;
00348           }
00349         }
00350       }else{
00351         for(unsigned int j=0;j<k2;j++)
00352           result._[i*k2+j] = 0.0f;
00353       }
00354     }
00355     return result;     
00356   }
00357 
00358   inline Matrix operator - () const
00359   {
00360     Matrix result(row,column,false);
00361     for (unsigned int j = 0; j < row; j++)
00362       for (unsigned int i = 0; i < column; i++)
00363         result._[j*column+i] = -_[j*column+i];
00364     return result;
00365   }
00366   
00367   inline virtual Matrix& operator = (const Matrix &matrix)
00368   {
00369     Resize(matrix.row,matrix.column,false);
00370     for (unsigned int j = 0; j < row; j++)
00371       for (unsigned int i = 0; i < column; i++)
00372         _[j*column+i] = matrix._[j*column+i];
00373     return *this;    
00374   }
00375 
00376   inline Matrix& operator += (const Matrix &matrix)
00377   {
00378     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00379     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00380     for (unsigned int j = 0; j < kj; j++)
00381       for (unsigned int i = 0; i < ki; i++)
00382         _[j*column+i] += matrix._[j*column+i];
00383     return *this;
00384   }
00385 
00386   inline Matrix& operator -= (const Matrix &matrix)
00387   {
00388     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00389     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00390     for (unsigned int j = 0; j < kj; j++)
00391       for (unsigned int i = 0; i < ki; i++)
00392         _[j*column+i] -= matrix._[j*column+i];
00393     return *this;
00394   }
00395 
00396   inline Matrix& operator ^= (const Matrix &matrix)
00397   {
00398     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00399     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00400     for (unsigned int j = 0; j < kj; j++)
00401       for (unsigned int i = 0; i < ki; i++)
00402         _[j*column+i] *= matrix._[j*column+i];
00403     return *this;
00404   }
00405 
00406   inline Matrix& operator /= (const Matrix &matrix)
00407   {
00408     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00409     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00410     for (unsigned int j = 0; j < kj; j++)
00411       for (unsigned int i = 0; i < ki; i++)
00412         _[j*column+i] /= matrix._[j*column+i];
00413     return *this;
00414   }
00415 
00416   inline Matrix& operator += (float scalar)
00417   {
00418     for (unsigned int j = 0; j < row; j++)
00419       for (unsigned int i = 0; i < column; i++)
00420         _[j*column+i] += scalar;
00421     return *this;
00422   }
00423 
00424   inline Matrix& operator -= (float scalar)
00425   {
00426     for (unsigned int j = 0; j < row; j++)
00427       for (unsigned int i = 0; i < column; i++)
00428         _[j*column+i] -= scalar;
00429     return *this;
00430   }
00431 
00432   inline Matrix& operator *= (float scalar)
00433   {
00434     for (unsigned int j = 0; j < row; j++)
00435       for (unsigned int i = 0; i < column; i++)
00436         _[j*column+i] *= scalar;
00437     return *this;
00438   }
00439 
00440   inline Matrix& operator /= (float scalar)
00441   {
00442     scalar = 1.0f/scalar;
00443     for (unsigned int j = 0; j < row; j++)
00444       for (unsigned int i = 0; i < column; i++)
00445         _[j*column+i] *= scalar;
00446     return *this;
00447   }
00448   
00449   inline Matrix operator + (const Matrix &matrix) const
00450   {
00451     Matrix result(row,column,false);  
00452     return Add(matrix,result);
00453   }
00454   
00455   inline Matrix& Add(const Matrix &matrix, Matrix &result) const
00456   {   
00457     result.Resize(row,column,false);
00458     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00459     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00460     for (unsigned int j = 0; j < kj; j++){
00461       for (unsigned int i = 0; i < ki; i++)
00462         result._[j*column+i] = _[j*column+i] + matrix._[j*column+i];
00463       for (unsigned int i = ki; i < column; i++)
00464         result._[j*column+i] = _[j*column+i];  
00465     }
00466     for (unsigned int j = kj; j < row; j++)
00467       for (unsigned int i = 0; i < column; i++)
00468         result._[j*column+i] = _[j*column+i];  
00469     return result;
00470   }
00471   
00472   inline Matrix operator - (const Matrix &matrix) const
00473   {
00474     Matrix result(row,column,false);  
00475     return Sub(matrix,result);
00476   }
00477   
00478   inline Matrix& Sub(const Matrix &matrix, Matrix &result) const
00479   {   
00480     result.Resize(row,column,false);
00481     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00482     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00483     for (unsigned int j = 0; j < kj; j++){
00484       for (unsigned int i = 0; i < ki; i++)
00485         result._[j*column+i] = _[j*column+i] - matrix._[j*column+i];
00486       for (unsigned int i = ki; i < column; i++)
00487         result._[j*column+i] = _[j*column+i];  
00488     }
00489     for (unsigned int j = kj; j < row; j++)
00490       for (unsigned int i = 0; i < column; i++)
00491         result._[j*column+i] = _[j*column+i];  
00492     return result;
00493   }
00494   
00495   inline Matrix operator ^ (const Matrix &matrix) const
00496   {
00497     Matrix result(row,column,false);  
00498     return PMult(matrix,result);
00499   }
00500   
00501   inline Matrix& PMult(const Matrix &matrix, Matrix &result) const
00502   {   
00503     result.Resize(row,column,false);
00504     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00505     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00506     for (unsigned int j = 0; j < kj; j++){
00507       for (unsigned int i = 0; i < ki; i++)
00508         result._[j*column+i] = _[j*column+i] * matrix._[j*column+i];
00509       for (unsigned int i = ki; i < column; i++)
00510         result._[j*column+i] = _[j*column+i];  
00511     }
00512     for (unsigned int j = kj; j < row; j++)
00513       for (unsigned int i = 0; i < column; i++)
00514         result._[j*column+i] = _[j*column+i];  
00515     return result;
00516   }
00517   
00518   inline Matrix operator / (const Matrix &matrix) const
00519   {
00520     Matrix result(row,column,false);  
00521     return PDiv(matrix,result);
00522   }
00523   
00524   inline Matrix& PDiv(const Matrix &matrix, Matrix &result) const
00525   {   
00526     result.Resize(row,column,false);
00527     const unsigned int kj = (row<=matrix.row?row:matrix.row);
00528     const unsigned int ki = (column<=matrix.column?column:matrix.column);
00529     for (unsigned int j = 0; j < kj; j++){
00530       for (unsigned int i = 0; i < ki; i++)
00531         result._[j*column+i] = _[j*column+i] / matrix._[j*column+i];
00532       for (unsigned int i = ki; i < column; i++)
00533         result._[j*column+i] = _[j*column+i];  
00534     }
00535     for (unsigned int j = kj; j < row; j++)
00536       for (unsigned int i = 0; i < column; i++)
00537         result._[j*column+i] = _[j*column+i];  
00538     return result;
00539   }
00540 
00541   inline Matrix operator + (float scalar) const
00542   {
00543     Matrix result(row,column,false);  
00544     return Add(scalar,result);    
00545   }
00546 
00547   inline Matrix& Add(float scalar, Matrix& result) const
00548   {
00549     result.Resize(row,column,false);
00550     for (unsigned int j = 0; j < row; j++)
00551       for (unsigned int i = 0; i < column; i++)
00552         result._[j*column+i] = _[j*column+i] + scalar;    
00553     return result;
00554   }
00555 
00556   inline Matrix operator - (float scalar) const
00557   {
00558     Matrix result(row,column,false);  
00559     return Sub(scalar,result);    
00560   }
00561   
00562   inline Matrix& Sub(float scalar, Matrix& result) const
00563   {
00564     result.Resize(row,column,false);
00565     for (unsigned int j = 0; j < row; j++)
00566       for (unsigned int i = 0; i < column; i++)
00567         result._[j*column+i] = _[j*column+i] - scalar;    
00568     return result;
00569   }
00570 
00571   inline Matrix operator * (float scalar) const
00572   {
00573     Matrix result(row,column,false);  
00574     return Mult(scalar,result);    
00575   }
00576 
00577   inline Matrix& Mult(float scalar, Matrix& result) const
00578   {
00579     result.Resize(row,column,false);
00580     for (unsigned int j = 0; j < row; j++)
00581       for (unsigned int i = 0; i < column; i++)
00582         result._[j*column+i] = _[j*column+i] * scalar;    
00583     return result;
00584   }
00585 
00586 
00587   inline Matrix operator / (float scalar) const
00588   {
00589     Matrix result(row,column,false);  
00590     return Div(scalar,result);    
00591   }
00592 
00593   inline Matrix& Div(float scalar, Matrix& result) const
00594   {
00595     result.Resize(row,column,false);
00596     for (unsigned int j = 0; j < row; j++)
00597       for (unsigned int i = 0; i < column; i++)
00598         result._[j*column+i] = _[j*column+i] / scalar;    
00599     return result;    
00600   }
00601 
00602   inline bool operator == (const Matrix& matrix) const
00603   {
00604     if((row!=matrix.row)||(column!=matrix.column)) return false;
00605     for (unsigned int j = 0; j < row; j++)
00606       for (unsigned int i = 0; i < column; i++)
00607         if(_[j*column+i] != matrix._[j*column+i]) return false;
00608     return true;
00609   }
00610 
00611   inline bool operator != (const Matrix& matrix) const
00612   {
00613     return !(*this ==  matrix);
00614   }
00615 
00616   inline Vector operator * (const Vector &vector) const
00617   {
00618     Vector result(row,false);  
00619     return Mult(vector,result);    
00620   }  
00621 
00622   inline Vector Mult(const Vector &vector) const
00623   {
00624     Vector result(row,false);  
00625     return Mult(vector,result);    
00626   }
00627 
00628   inline Vector& Mult(const Vector &vector, Vector &result) const
00629   {
00630     result.Resize(row,false);
00631     const unsigned int ki = (column<=vector.row?column:vector.row);
00632     for (unsigned int j = 0; j < row; j++){
00633       result._[j] = 0.0f;
00634       for (unsigned int i = 0; i < ki; i++)
00635         result._[j] += _[j*column+i] * vector._[i];
00636     }
00637     return result;
00638   }
00639 
00640 
00641   inline Matrix operator * (const Matrix &matrix) const  
00642   {
00643     Matrix result(row,matrix.column,false);  
00644     return Mult(matrix,result);
00645   }  
00646 
00647   inline Matrix& Mult(const Matrix &matrix, Matrix &result) const
00648   {
00649     result.Resize(row,matrix.column,false);
00650     const unsigned int rrow = result.row;
00651     const unsigned int rcol = result.column;
00652     const unsigned int kk = (column<=matrix.row?column:matrix.row);
00653     for (unsigned int j = 0; j < rrow; j++){
00654       for (unsigned int i = 0; i < rcol; i++){
00655         result._[j*rcol+i] = 0.0f;
00656         for(unsigned int k = 0; k< kk; k++)    
00657           result._[j*rcol+i] += _[j*column+k] * matrix._[k*rcol+i];
00658       }
00659     }    
00660     return result;
00661   }
00662 
00663 
00664 
00665   inline Matrix& Identity()
00666   {
00667     const unsigned int k = (row>column?column:row);
00668     Zero();
00669     for (unsigned int i = 0; i < k; i++)
00670       _[i*column+i] = 1.0f;
00671     return *this;    
00672   }
00673 
00674   inline Matrix& Diag(const Vector &vector)
00675   {
00676     const unsigned int k = (row>column?column:row);
00677     const unsigned int k2 = (k>vector.row?vector.row:k);
00678     Zero();
00679     for (unsigned int i = 0; i < k2; i++)
00680       _[i*column+i] = vector._[i];
00681     return *this;    
00682   }
00683 
00684   inline Matrix& Random(){
00685     for (unsigned int j = 0; j < row; j++)
00686       for (unsigned int i = 0; i < column; i++)
00687         _[j*column+i] =((float)rand())/((float)(RAND_MAX+1.0));    
00688     return *this;    
00689   }
00690 
00691   inline Matrix Transpose() const
00692   {
00693     Matrix result(row,column,false);
00694     return Transpose(result);    
00695   }
00696 
00697   inline Matrix& Transpose(Matrix &result) const
00698   {    
00699     result.Resize(column,row,false);
00700     for (unsigned int j = 0; j < row; j++)
00701       for (unsigned int i = 0; i < column; i++)
00702         result._[i*row+j] = _[j*column+i];
00703     return result;    
00704   }
00705 
00706   inline Matrix VCat(const Matrix& matrix)
00707   {
00708     Matrix result;
00709     return VCat(matrix,result);    
00710   }
00711   
00712   inline Matrix& VCat(const Matrix& matrix, Matrix & result)
00713   {
00714     unsigned int k1 = (column>matrix.column?column:matrix.column);
00715     result.Resize(row+matrix.row,k1,false);
00716     for (unsigned int j = 0; j < row; j++){
00717       for (unsigned int i = 0; i < column; i++)
00718         result._[j*k1+i] = _[j*column+i];
00719       for (unsigned int i = column; i < k1; i++)
00720         result._[j*k1+i] = 0.0f;
00721     }
00722     for (unsigned int j = 0; j < matrix.row; j++){
00723       for (unsigned int i = 0; i < matrix.column; i++)
00724         result._[(row+j)*k1+i] = matrix._[j*matrix.column+i];
00725       for (unsigned int i = matrix.column; i < k1; i++)
00726         result._[(row+j)*k1+i] = 0.0f;
00727     }
00728     return result;
00729   }
00730 
00731   inline Matrix HCat(const Matrix& matrix)
00732   {
00733     Matrix result;
00734     return VCat(matrix,result);    
00735   }
00736   
00737   inline Matrix& HCat(const Matrix& matrix, Matrix & result)
00738   {
00739     unsigned int k1 = (row>matrix.row?row:matrix.row);
00740     unsigned int k2 = column+matrix.column;
00741     result.Resize(k1,k2,false);
00742     for (unsigned int j = 0; j < row; j++)
00743       for (unsigned int i = 0; i < column; i++)
00744         result._[j*k2+i] = _[j*column+i];
00745     for (unsigned int j = row; j < k1; j++)
00746       for (unsigned int i = 0; i < column; i++)
00747         result._[j*k2+i] = 0.0f;
00748 
00749     for (unsigned int j = 0; j < matrix.row; j++)
00750       for (unsigned int i = 0; i < matrix.column; i++)
00751         result._[j*k2+i+column] = matrix._[j*matrix.column+i];
00752     for (unsigned int j = matrix.row; j < k1; j++)
00753       for (unsigned int i = 0; i < matrix.column; i++)
00754         result._[j*k2+i+column] = 0.0f;
00755     
00756     return result;
00757   }
00758 
00759   inline static int IsInverseOk(){
00760     return bInverseOk;
00761   }
00762 
00763   inline Matrix Inverse() const
00764   {
00765     Matrix result;
00766     return Inverse(result);
00767   }  
00768 
00769   inline Matrix& Inverse(Matrix &result) const
00770   {
00771     bInverseOk = TRUE;   
00772     if(row==column){ // Square matrix
00773       result.Resize(row,column,false);
00774       const unsigned int n = row;
00775       Matrix MM(*this);
00776       result.Identity();
00777       for(unsigned int i=0;i<n;i++){
00778         float pivot = MM._[i*column+i]; 
00779         if(fabs(pivot)<=EPSILON){
00780           for(unsigned int j=i+1;j<n;j++){
00781             if((pivot = MM._[j*column+i])!=0.0f){
00782               MM.SwapRow(i,j);
00783               result.SwapRow(i,j);
00784               break;  
00785             }
00786           }
00787           if(fabs(pivot)<=EPSILON){
00788             bInverseOk = FALSE;
00789             return result;
00790           }            
00791         }
00792         pivot = 1.0f/pivot;
00793         for(unsigned int j=0;j<n;j++){
00794           MM._[i*column+j]   *= pivot;
00795           result._[i*column+j] *= pivot;
00796         }
00797         for(unsigned int k=0;k<n;k++){
00798           if(k!=i){
00799             const float mki = MM._[k*column+i];
00800             for(unsigned int j=0;j<n;j++){
00801                MM._[k*column+j]   -= MM._[i*column+j]   *mki;
00802                result._[k*column+j] -= result._[i*column+j] *mki;              
00803             }            
00804           }
00805         }
00806       }        
00807     }else{ // Moore-Penrose pseudo inverse
00808       if(row>column){ // (JtJ)^(-1)Jt
00809         Matrix MT,SQ,SQInv;
00810         Transpose(MT);
00811         MT.Mult(*this,SQ);
00812         SQ.Inverse(SQInv);
00813         SQInv.Mult(MT,result); 
00814       }else{ // Jt(JJt)^(-1)
00815         Matrix MT,SQ,SQInv;
00816         Transpose(MT);
00817         Mult(MT,SQ);
00818         SQ.Inverse(SQInv);
00819         MT.Mult(SQInv,result);         
00820       }
00821     }
00822     return result;    
00823   }
00824 
00825   inline Matrix& SwapRow(unsigned int j1, unsigned int j2){
00826     if((j1<row)&&(j2<row)){
00827       float tmp;
00828       for (unsigned int i = 0; i < column; i++){
00829         tmp            = _[j1*column+i];
00830         _[j1*column+i] = _[j2*column+i];
00831         _[j2*column+i] = tmp;        
00832       }        
00833     }
00834     return *this; 
00835   }
00836  
00837   inline Matrix& SwapColumn(unsigned int i1, unsigned int i2){
00838     if((i1<column)&&(i2<column)){
00839       float tmp;
00840       for (unsigned int j = 0; j < row; j++){
00841         tmp            = _[j*column+i1];
00842         _[j*column+i1] = _[j*column+i2];
00843         _[j*column+i2] = tmp;        
00844       }        
00845     }
00846     return *this; 
00847   }
00848   
00849   void Print() const
00850   {
00851     std::cout << "Matrix " <<row<<"x"<<column<<std::endl;;
00852     for (unsigned int j = 0; j < row; j++){
00853       for (unsigned int i = 0; i < column; i++)
00854         std::cout << _[j*column+i] <<" ";
00855       std::cout << std::endl;
00856     }
00857   }
00858   
00859   void QRDecomposition(Matrix & Q, Matrix & R){
00860     Matrix QR;
00861     QRDecomposition(Q,R,QR);
00862   }
00863   
00864   void QRDecomposition(Matrix & Q, Matrix & R, Matrix & QR){    
00865     if(row>=column){
00866       QR = *this;
00867     }else{
00868       Transpose(QR);
00869     }
00870     unsigned int m = QR.row;
00871     unsigned int n = QR.column;
00872     Vector RDiag(n);
00873         
00874     for(unsigned int k=0;k<n;k++){
00875       float nrm = 0.0f;
00876       for (unsigned int i = k; i < m; i++) {
00877         nrm = hypot_s(nrm, QR(i,k));
00878       }
00879       if (nrm != 0.0f) {
00880         if(QR(k,k)<0.0f){
00881           nrm = -nrm;
00882         }
00883         for (unsigned int i = k; i < m; i++) {
00884             QR(i,k) /= nrm;
00885         }
00886         QR(k,k)+=1.0f;
00887 
00888         for (unsigned int j = k + 1; j < n; j++) {
00889           float s = 0.0f;
00890           for (unsigned int i = k; i < m; i++) {
00891               s += QR(i,k) * QR(i,j);
00892           }
00893           s = -s / QR(k,k);
00894           for (unsigned int i = k; i < m; i++) {
00895               QR(i,j) += s * QR(i,k);
00896           }
00897         }
00898       }
00899       RDiag(k) = -nrm;
00900     }
00901     
00902     R.Resize(n,n);
00903     for(unsigned int i = 0; i < n; i++) {
00904       for(unsigned int j = 0; j < n; j++) {
00905         if(i<j){
00906           R(i,j) = QR(i,j); 
00907         }else if(i==j){
00908           R(i,j) = RDiag(i);
00909         }else{
00910           R(i,j) = 0.0f;
00911         }
00912       }
00913     }
00914 
00915     Q.Resize(m,n);
00916     for(int k= n-1;k>=0;k--){
00917       for(unsigned int i = 0; i < m; i++) {
00918         Q(i,k) = 0.0f;
00919       }
00920       Q(k,k)=1.0f;
00921       for(unsigned int j = k; j < n; j++) {
00922         if(QR(k,k)!=0.0f){
00923           float s = 0.0f;
00924           for(unsigned int i = k; i < m; i++) {
00925             s += QR(i,k) * Q(i,j);
00926           }
00927           s = -s / QR(k,k);
00928           for(unsigned int i = k; i < m; i++) {
00929             Q(i,j) = Q(i,j) + s*QR(i,k);
00930           }
00931         }
00932       }       
00933     }
00934   }
00935 
00936   
00937   Matrix& Tridiagonalize(Matrix &result,Matrix &trans){
00938     result.Resize(2,row);
00939     Matrix A(*this);
00940     
00941     int n = row;
00942     int l,k,j,i;
00943     float scale,hh,h,g,f;
00944     for(i=n-1;i>=1;i--){
00945       l = i-1;
00946       h = scale = 0.0f;
00947       if(l>0){
00948         for(k=0;k<=l;k++)
00949           scale += fabs(A._[i*column+k]);
00950         if(scale == 0.0f){
00951           result._[column+i] = A._[i*column+l];
00952         }else{
00953           for(k=0;k<=l;k++){
00954             A._[i*column+k] /= scale;
00955             h += A._[i*column+k]*A._[i*column+k];
00956           }
00957           f= A._[i*column+l];
00958           g=(f>=0.0f?-sqrt(h):sqrt(h));
00959           result._[column+i] = scale*g;
00960           h-=f*g;
00961           A._[i*column+l] = f-g;
00962           f=0.0f;
00963           for(j=0;j<=l;j++){
00964             A._[j*column+i] = A._[i*column+j] /h;
00965             g=0.0f;
00966             for(k=0;k<=j;k++)
00967               g+=  A._[j*column+k]*A._[i*column+k];
00968             for(k=j+1;k<=l;k++)
00969               g+=  A._[k*column+j]*A._[i*column+k];
00970             result._[column+j] = g/h;
00971             f+= result._[column+j]*A._[i*column+j];
00972           }
00973           hh = f/(h+h);
00974           for(j=0;j<=l;j++){
00975             f = A._[i*column+j];
00976             result._[column+j]=g=result._[column+j]-hh*f;
00977             for(k=0;k<=j;k++)
00978               A._[j*column+k] -= (f*result._[column+k]+g*A._[i*column+k]);            
00979           }             
00980         }
00981       }else{
00982         result._[column+i] = A._[i*column+l];        
00983       }
00984       result._[i]=h;  
00985     }
00986     result._[0]=0.0f;  
00987     result._[column+0]=0.0f;
00988     for(i=0;i<n;i++){
00989       l=i-1;
00990       if(result._[i]){
00991         for(j=0;j<=l;j++){
00992           g=0.0f;
00993           for(k=0;k<=l;k++)
00994             g+= A._[i*column+k]*A._[k*column+j]; 
00995           for(k=0;k<=l;k++)
00996             A._[k*column+j] -= g*A._[k*column+i]; 
00997         }  
00998       }
00999       result._[i] = A._[i*column+i];
01000       A._[i*column+i] = 1.0f;
01001       for(j=0;j<=l;j++) A._[j*column+i]=A._[i*column+j]=0.0f;
01002     }
01003     trans = A;
01004     return result;
01005   }
01006     
01007   Matrix& TriDiag(Matrix &tri){
01008     Resize(tri.ColumnSize(),tri.ColumnSize(),false);
01009     Zero();
01010     for(unsigned int i=0;i<column;i++){
01011       _[i*(column+1)] = tri._[i];
01012       if(i<column-1)
01013         _[i*(column+1)+1] = tri._[column+i+1];
01014       if(i>0)  
01015         _[i*(column+1)-1] = tri._[column+i];
01016     }
01017     return *this;
01018   }
01019   
01020   int TriEigen(Vector &eigenValues, Matrix& eigenVectors,int maxIter = 30){
01021     GetRow(0,eigenValues);
01022     Vector e;
01023     GetRow(1,e);
01024     
01025     const int n = column;
01026     int m,l,iter,i,k;
01027     float s,r,p,g,f,dd,c,b;
01028     
01029     for(i=1;i<n;i++) e._[i-1] = e._[i];
01030     e._[n-1] = 0.0f;
01031 
01032     for(l=0;l<n;l++){
01033       iter=0;
01034       do{
01035         for(m=l;m<=n-2;m++){
01036           dd = fabs(eigenValues._[m])+fabs(eigenValues._[m+1]);
01037           if((float)(fabs(e._[m])+dd) == dd) break;  
01038         }
01039         if(m!=l){
01040           if(iter++==maxIter) printf("Error: too many ierations...\n");
01041           g=(eigenValues._[l+1]-eigenValues._[l])/(2.0f*e[l]);
01042           r=hypot_s(g,1.0f);
01043           g=eigenValues._[m]-eigenValues._[l]+e._[l]/(g+SIGN2(r,g));
01044           s=c=1.0f;
01045           p=0.0f;
01046           for(i=m-1;i>=l;i--){
01047             f=s*e._[i];
01048             b=c*e._[i];
01049             e._[i+1] =(r=hypot_s(f,g));
01050             if(r==0.0f){
01051               eigenValues._[i+1]-=p;
01052               e._[m] = 0.0f;
01053               break;  
01054             }
01055             s=f/r;
01056             c=g/r;
01057             g=eigenValues._[i+1]-p;
01058             r=(eigenValues._[i]-g)*s+2.0f*c*b;
01059             eigenValues._[i+1]=g+(p=s*r);
01060             g=c*r-b;
01061             for(k=0;k<n;k++){
01062               f=eigenVectors._[k*n+i+1];
01063               eigenVectors._[k*n+i+1]=s*eigenVectors._[k*n+i]+c*f;
01064               eigenVectors._[k*n+i]=c*eigenVectors._[k*n+i]-s*f;                
01065             }            
01066           }
01067           if((r==0.0f)&&(i>=0)) continue;
01068           eigenValues._[l]-=p;
01069           e._[l] = g;
01070           e._[m] = 0.0f;
01071         }        
01072       }while(m!=l); 
01073     } 
01074        
01075     return iter;
01076   }
01077 
01078 
01079 
01080   
01081 
01082 
01083   Matrix& SortColumnAbs(Vector & values){
01084     const int k = (values.Size()<column?values.Size():column);
01085     float cmax;
01086     int maxId;
01087     for(int i=0;i<k-1;i++){
01088       cmax  = fabs(values._[i]);
01089       maxId = i;
01090       for(int j=i+1;j<k;j++){
01091         if(cmax<fabs(values._[j])){
01092           cmax = fabs(values._[j]);
01093           maxId = j;  
01094         }            
01095       }
01096       if(maxId!=i){
01097         float tmp       = values._[i];
01098         values._[i]     = values._[maxId];
01099         values._[maxId] = tmp;
01100         SwapColumn(i,maxId);
01101       }     
01102     }  
01103     return *this;
01104   }
01105 
01106   
01107 protected:
01108 
01109   inline void Release(){
01110     if(_!=NULL) delete [] _; 
01111     row    = 0;
01112     column = 0;
01113     _      = NULL;
01114   }  
01115 public:  
01116   inline virtual void Resize(unsigned int rowSize, unsigned int colSize, bool copy = true){
01117     if((row!=rowSize)||(column!=colSize)){
01118       if((rowSize)&&(colSize)){
01119         float *arr = new float[rowSize*colSize];
01120         if(copy){
01121           unsigned int mj = (row<rowSize?row:rowSize);
01122           unsigned int mi = (column<colSize?column:colSize);
01123           
01124           for (unsigned int j = 0; j < mj; j++){
01125             for (unsigned int i = 0; i < mi; i++)
01126               arr[j*colSize+i] = _[j*colSize+i];
01127             for (unsigned int i = mi; i < colSize; i++)
01128               arr[j*colSize+i] = 0.0f;
01129           }
01130           for (unsigned int j = mj; j < rowSize; j++){
01131             for (unsigned int i = 0; i < colSize; i++)
01132               arr[j*colSize+i] = 0.0f;            
01133           }
01134         }
01135         if(_!=NULL) delete [] _; 
01136         _      = arr;
01137         row    = rowSize;
01138         column = colSize;        
01139       }else{
01140         Release();
01141       }
01142     }
01143   }
01144 };
01145 
01146 
01147 #endif
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:25 2010 for Body_Schema_Learning by  doxygen 1.6.1