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
00027
00028 #ifndef CMatrixTemplate_H
00029 #define CMatrixTemplate_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <iostream>
00033 #include <iomanip>
00034
00035
00036 namespace mrpt
00037 {
00038 namespace math
00039 {
00052 template <class T>
00053 class CMatrixTemplate
00054 {
00055 protected:
00056 T **m_Val;
00057 size_t m_Rows, m_Cols;
00058
00061 void realloc(size_t row, size_t col, bool newElementsToZero = false)
00062 {
00063 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
00064 {
00065 size_t r;
00066 bool doZeroColumns = newElementsToZero && (col>m_Cols);
00067 size_t sizeZeroColumns = sizeof(T)*(col-m_Cols);
00068
00069
00070 for (r=row;r<m_Rows;r++)
00071 {
00072 free( m_Val[r] );
00073 }
00074
00075
00076 if (!row)
00077 { ::free(m_Val); m_Val=NULL; }
00078 else m_Val = static_cast<T**> (::realloc(m_Val, sizeof(T*) * row ) );
00079
00080
00081 size_t row_size = col * sizeof(T);
00082
00083
00084 for (r=0;r<row;r++)
00085 {
00086 if (r<m_Rows)
00087 {
00088
00089 m_Val[r] = static_cast<T*> (::realloc( m_Val[r], row_size ));
00090
00091 if (doZeroColumns)
00092 {
00093
00094 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
00095 }
00096 }
00097 else
00098 {
00099
00100 m_Val[r] = static_cast<T*> (::calloc( row_size, 1 ));
00101 }
00102 }
00103
00104
00105 m_Rows = row;
00106 m_Cols = col;
00107 }
00108 }
00109
00110 public:
00114 void extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0)
00115 {
00116 size_t i,n;
00117 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
00118 if (nCol>=m_Cols)
00119 THROW_EXCEPTION("extractCol: Column index out of bounds");
00120 #endif
00121
00122 n = m_Rows - startingRow;
00123 out.resize( n );
00124
00125 for (i=0;i<n;i++)
00126 out[i] = m_Val[i+startingRow][nCol];
00127 }
00128
00132 void extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0)
00133 {
00134 size_t i,n;
00135 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00136 if (nCol>=m_Cols)
00137 THROW_EXCEPTION("extractCol: Column index out of bounds");
00138 #endif
00139
00140 n = m_Rows - startingRow;
00141 out.setSize(n,1);
00142
00143 for (i=0;i<n;i++)
00144 out(i,0) = m_Val[i+startingRow][nCol];
00145 }
00146
00150 template <class F>
00151 void extractRow(size_t nRow, std::vector<F> &out, size_t startingCol = 0)
00152 {
00153 size_t i,n;
00154 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00155 if (nRow>=m_Rows)
00156 THROW_EXCEPTION("extractRow: Row index out of bounds");
00157 #endif
00158 n = m_Cols - startingCol ;
00159 out.resize( n );
00160
00161 for (i=0;i<n;i++)
00162 out[i] = static_cast<F> ( m_Val[nRow][i+startingCol] );
00163 }
00164
00168 template <class F>
00169 void extractRow(size_t nRow, CMatrixTemplate<F> &out, size_t startingCol = 0)
00170 {
00171 size_t i,n;
00172 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00173 if (nRow>=m_Rows)
00174 THROW_EXCEPTION("extractRow: Row index out of bounds");
00175 #endif
00176 n = m_Cols - startingCol ;
00177 out.setSize(1,n);
00178
00179 for (i=0;i<n;i++)
00180 out(0,i) = static_cast<F>( m_Val[nRow][i+startingCol] );
00181 }
00182
00185 CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
00186 {
00187 (*this) = m;
00188 }
00189
00190 CMatrixTemplate (size_t row = 3, size_t col = 3) : m_Val(NULL),m_Rows(0),m_Cols(0)
00191 {
00192 realloc(row,col);
00193 }
00194
00203 template <typename V, size_t N>
00204 CMatrixTemplate (size_t row, size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00205 {
00206 MRPT_COMPILE_TIME_ASSERT(N!=0)
00207 realloc(row,col);
00208
00209 if (m_Rows*m_Cols != N)
00210 {
00211 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00212 }
00213 size_t idx=0;
00214 for (size_t i=0; i < m_Rows; i++)
00215 for (size_t j=0; j < m_Cols; j++)
00216 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00217
00218 }
00219
00222 virtual ~CMatrixTemplate()
00223 {
00224 realloc(0,0);
00225 }
00226
00229 CMatrixTemplate& operator = (const CMatrixTemplate& m)
00230 {
00231 realloc( m.m_Rows, m.m_Cols );
00232
00233 for (size_t i=0; i < m_Rows; i++)
00234 for (size_t j=0; j < m_Cols; j++)
00235 m_Val[i][j] = m.m_Val[i][j];
00236
00237 return *this;
00238 }
00239
00250 template <typename V, size_t N>
00251 CMatrixTemplate& operator = (V (&theArray)[N] )
00252 {
00253 MRPT_COMPILE_TIME_ASSERT(N!=0)
00254
00255 if (m_Rows*m_Cols != N)
00256 {
00257 THROW_EXCEPTION(format("Mismatch between matrix size %"PRIuPTR"x%"PRIuPTR" and array of length %"PRIuPTR,m_Rows,m_Cols,N))
00258 }
00259 size_t idx=0;
00260 for (size_t i=0; i < m_Rows; i++)
00261 for (size_t j=0; j < m_Cols; j++)
00262 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00263
00264 return *this;
00265 }
00266
00267
00271 inline size_t getRowCount() const
00272 {
00273 return m_Rows;
00274 }
00275
00279 inline size_t getColCount() const
00280 {
00281 return m_Cols;
00282 }
00283
00287 inline size_t nr() const
00288 {
00289 return m_Rows;
00290 }
00291
00295 inline size_t nc() const
00296 {
00297 return m_Cols;
00298 }
00299
00300
00303 void setSize(size_t row, size_t col)
00304 {
00305 realloc(row,col);
00306 }
00307
00310 bool IsSquare ()
00311 {
00312 return ( m_Rows == m_Cols );
00313 }
00314
00317 inline T& operator () (size_t row, size_t col)
00318 {
00319 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00320 if (row >= m_Rows || col >= m_Cols)
00321 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range!",static_cast<unsigned long>(row),static_cast<unsigned long>(col)) );
00322 #endif
00323 return m_Val[row][col];
00324 }
00325
00328 inline T operator () (size_t row, size_t col) const
00329 {
00330 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00331 if (row >= m_Rows || col >= m_Cols)
00332 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range!",static_cast<unsigned long>(row),static_cast<unsigned long>(col)) );
00333 #endif
00334 return m_Val[row][col];
00335 }
00336
00340 inline T& operator () (size_t ith)
00341 {
00342 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00343 ASSERT_(m_Rows==1 || m_Cols==1);
00344 #endif
00345 if (m_Rows==1)
00346 {
00347
00348 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00349 if (ith >= m_Cols)
00350 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00351 #endif
00352 return m_Val[0][ith];
00353 }
00354 else
00355 {
00356
00357 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00358 if (ith >= m_Rows)
00359 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00360 #endif
00361 return m_Val[ith][0];
00362 }
00363 }
00364
00368 inline T operator () (size_t ith) const
00369 {
00370 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00371 ASSERT_(m_Rows==1 || m_Cols==1);
00372 #endif
00373 if (m_Rows==1)
00374 {
00375
00376 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00377 if (ith >= m_Cols)
00378 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00379 #endif
00380 return m_Val[0][ith];
00381 }
00382 else
00383 {
00384
00385 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00386 if (ith >= m_Rows)
00387 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00388 #endif
00389 return m_Val[ith][0];
00390 }
00391 }
00392
00395 inline void set_unsafe(size_t row, size_t col,const T &v)
00396 {
00397 m_Val[row][col] = v;
00398 }
00399
00402 inline T get_unsafe(size_t row, size_t col) const
00403 {
00404 return m_Val[row][col];
00405 }
00406
00409 inline T* get_unsafe_row(size_t row) const
00410 {
00411 return m_Val[row];
00412 }
00413
00418 void insertRow(size_t nRow, const std::vector<T> &in)
00419 {
00420 if (nRow>=m_Rows) THROW_EXCEPTION("insertRow: Row index out of bounds");
00421
00422 size_t n = in.size();
00423 ASSERT_(m_Cols>=in.size());
00424
00425 for (size_t i=0;i<n;i++)
00426 m_Val[nRow][i] = in[i];
00427 }
00428
00433 void appendRow(const std::vector<T> &in)
00434 {
00435 size_t i,n, row;
00436
00437 n = m_Cols;
00438 row = m_Rows;
00439 ASSERT_(in.size()==m_Cols);
00440
00441 realloc( row+1,n );
00442
00443 for (i=0;i<n;i++)
00444 m_Val[row][i] = in[i];
00445 }
00446
00451 void insertCol(size_t nCol, const std::vector<T> &in)
00452 {
00453 if (nCol>=m_Cols) THROW_EXCEPTION("insertCol: Row index out of bounds");
00454
00455 size_t n = in.size();
00456 ASSERT_( m_Rows >= in.size() );
00457
00458 for (size_t i=0;i<n;i++)
00459 m_Val[i][nCol] = in[i];
00460 }
00461
00470 template <class R>
00471 void insertMatrix(size_t nRow, size_t nCol, const CMatrixTemplate<R> &in)
00472 {
00473 size_t i,j,ncols,nrows;
00474
00475 nrows = in.m_Rows;
00476 ncols = in.m_Cols;
00477 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00478 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00479
00480 for (i=nRow;i<nRow+nrows;i++)
00481 for(j=nCol;j<nCol+ncols;j++)
00482 set_unsafe(i,j, static_cast<T> (in.get_unsafe(i-nRow,j-nCol) ) );
00483 }
00484
00493 void insertMatrixTranspose(size_t nRow, size_t nCol, const CMatrixTemplate<T> &in)
00494 {
00495 size_t i,j,ncols,nrows;
00496
00497 ncols = in.m_Rows;
00498 nrows = in.m_Cols;
00499 if ( (nRow+nrows > m_Rows) || (nCol+ncols >m_Cols) )
00500 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00501
00502 for (i=nRow;i<nRow+nrows;i++)
00503 for(j=nCol;j<nCol+ncols;j++)
00504 set_unsafe(i,j, in.get_unsafe(j-nCol,i-nRow) );
00505 }
00506
00507
00514 void insertMatrix(size_t nRow, size_t nCol, const std::vector<T> &in)
00515 {
00516 size_t j,ncols;
00517
00518 ncols = in.size();
00519 if ( (nRow+1 > m_Rows) || (nCol+ncols >m_Cols) )
00520 THROW_EXCEPTION("insertMatrix: Row or Col index out of bounds");
00521
00522 for(j = nCol ; j < nCol + ncols ; j++)
00523 set_unsafe(nRow,j, in[j-nCol] );
00524 }
00525
00530 void joinMatrix(const CMatrixTemplate<T> &left_up, const CMatrixTemplate<T> &right_up,
00531 const CMatrixTemplate<T> &left_down, const CMatrixTemplate<T> &right_down)
00532 {
00533 if ((left_up.getRowCount()!= right_up.getRowCount())||(left_up.getColCount()!=left_down.getColCount())||
00534 (left_down.getRowCount()!=right_down.getRowCount())||(right_up.getColCount()!=right_down.getColCount()))
00535 THROW_EXCEPTION("join_Matrix: Row or Col index out of bounds");
00536 setSize(left_up.getRowCount()+left_down.getRowCount(),left_up.getColCount()+right_up.getColCount());
00537 insertMatrix(0,0,left_up);
00538 insertMatrix(0,left_up.getColCount(),right_up);
00539 insertMatrix(left_up.getRowCount(),0,left_down);
00540 insertMatrix(left_up.getRowCount(),left_up.getColCount(),right_down);
00541 }
00542
00545 void fill( const T &val)
00546 {
00547 for (size_t r=0;r<m_Rows;r++)
00548 for (size_t c=0;c<m_Cols;c++)
00549 m_Val[r][c]= val;
00550 }
00551
00552
00553
00554 };
00555
00559 template <class T>
00560 std::ostream& operator << (std::ostream& ostrm, const CMatrixTemplate<T>& m)
00561 {
00562 ostrm << std::setprecision(4);
00563
00564 for (size_t i=0; i < m.getRowCount(); i++)
00565 {
00566 for (size_t j=0; j < m.getColCount(); j++)
00567 {
00568 ostrm << std::setw(13) << m(i,j);
00569 }
00570 ostrm << std::endl;
00571 }
00572 return ostrm;
00573 }
00574
00576 template <class T>
00577 size_t size( const CMatrixTemplate<T>& m, int dim )
00578 {
00579 if (dim==1)
00580 return m.getRowCount();
00581 else if (dim==2)
00582 return m.getColCount();
00583 else THROW_EXCEPTION_CUSTOM_MSG1("size: Matrix dimensions are 1 & 2 only (called with i=%i)",dim);
00584 }
00585
00586
00587 }
00588 }
00589
00590 #endif