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 <mrpt/system/memory.h>
00033 #include <mrpt/system/datetime.h>
00034
00035 #include <mrpt/math/math_frwds.h>
00036 #include <mrpt/math/matrix_adaptors.h>
00037 #include <mrpt/math/CArray.h>
00038
00039 namespace mrpt
00040 {
00041 namespace math
00042 {
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 template <class T>
00058 class CMatrixTemplate
00059 {
00060 public:
00061
00062 typedef T value_type;
00063 typedef T& reference;
00064 typedef const T& const_reference;
00065 typedef std::size_t size_type;
00066 typedef std::ptrdiff_t difference_type;
00067
00068
00069 protected:
00070 T **m_Val;
00071 size_t m_Rows, m_Cols;
00072
00073
00074
00075 void realloc(size_t row, size_t col, bool newElementsToZero = false)
00076 {
00077 if (row!=m_Rows || col!=m_Cols || m_Val==NULL)
00078 {
00079 size_t r;
00080 bool doZeroColumns = newElementsToZero && (col>m_Cols);
00081 size_t sizeZeroColumns = sizeof(T)*(col-m_Cols);
00082
00083
00084 for (r=row;r<m_Rows;r++)
00085 mrpt::system::os::aligned_free( m_Val[r] );
00086
00087
00088 if (!row)
00089 { mrpt::system::os::aligned_free(m_Val); m_Val=NULL; }
00090 else m_Val = static_cast<T**> (mrpt::system::os::aligned_realloc(m_Val, sizeof(T*) * row, 16 ) );
00091
00092
00093 size_t row_size = col * sizeof(T);
00094
00095
00096 for (r=0;r<row;r++)
00097 {
00098 if (r<m_Rows)
00099 {
00100
00101 m_Val[r] = static_cast<T*> (mrpt::system::os::aligned_realloc( m_Val[r], row_size, 16));
00102
00103 if (doZeroColumns)
00104 {
00105
00106 ::memset(&m_Val[r][m_Cols],0,sizeZeroColumns);
00107 }
00108 }
00109 else
00110 {
00111
00112 m_Val[r] = static_cast<T*> ( mrpt::system::os::aligned_calloc( row_size, 16 ));
00113 }
00114 }
00115
00116 m_Rows = row;
00117 m_Cols = col;
00118 }
00119 }
00120
00121 public:
00122
00123
00124
00125 template<size_t N> inline void ASSERT_ENOUGHROOM(size_t r,size_t c) const {
00126 #if defined(_DEBUG)||(MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00127 ASSERT_((r>=N)&&(r+N<getRowCount())&&(c>=N)&&(c+N<getColCount()));
00128 #endif
00129 }
00130
00131 void fillAll(const T &val) {
00132 for (size_t r=0;r<m_Rows;r++)
00133 for (size_t c=0;c<m_Cols;c++)
00134 m_Val[r][c]=val;
00135 }
00136
00137
00138 inline void swap(CMatrixTemplate<T> &o)
00139 {
00140 std::swap(m_Val, o.m_Val );
00141 std::swap(m_Rows, o.m_Rows );
00142 std::swap(m_Cols, o.m_Cols );
00143 }
00144
00145
00146 CMatrixTemplate (const CMatrixTemplate& m) : m_Val(NULL),m_Rows(0),m_Cols(0)
00147 {
00148 (*this) = m;
00149 }
00150
00151 CMatrixTemplate (size_t row = 1, size_t col = 1) : m_Val(NULL),m_Rows(0),m_Cols(0)
00152 {
00153 realloc(row,col);
00154 }
00155
00156
00157
00158 CMatrixTemplate (const CMatrixTemplate& m, const size_t cropRowCount, const size_t cropColCount) : m_Val(NULL),m_Rows(0),m_Cols(0)
00159 {
00160 ASSERT_(m.m_Rows>=cropRowCount)
00161 ASSERT_(m.m_Cols>=cropColCount)
00162 realloc( cropRowCount, cropColCount );
00163 for (size_t i=0; i < m_Rows; i++)
00164 for (size_t j=0; j < m_Cols; j++)
00165 m_Val[i][j] = m.m_Val[i][j];
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176 template <typename V, size_t N>
00177 CMatrixTemplate (size_t row, size_t col, V (&theArray)[N] ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00178 {
00179 MRPT_COMPILE_TIME_ASSERT(N!=0)
00180 realloc(row,col);
00181 if (m_Rows*m_Cols != N) THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",static_cast<long unsigned>(m_Rows),static_cast<long unsigned>(m_Cols),static_cast<long unsigned>(N)))
00182 size_t idx=0;
00183 for (size_t i=0; i < m_Rows; i++)
00184 for (size_t j=0; j < m_Cols; j++)
00185 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00186 }
00187
00188
00189
00190 template <typename V>
00191 CMatrixTemplate(size_t row, size_t col, const V &theVector ) : m_Val(NULL),m_Rows(0),m_Cols(0)
00192 {
00193 const size_t N = theVector.size();
00194 realloc(row,col);
00195 if (m_Rows*m_Cols != N) THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",static_cast<long unsigned>(m_Rows),static_cast<long unsigned>(m_Cols),static_cast<long unsigned>(N)))
00196 typename V::const_iterator it = theVector.begin();
00197 for (size_t i=0; i < m_Rows; i++)
00198 for (size_t j=0; j < m_Cols; j++)
00199 m_Val[i][j] = static_cast<T>( *(it++) );
00200 }
00201
00202
00203 virtual ~CMatrixTemplate() { realloc(0,0); }
00204
00205
00206 CMatrixTemplate& operator = (const CMatrixTemplate& m)
00207 {
00208 realloc( m.m_Rows, m.m_Cols );
00209 for (size_t i=0; i < m_Rows; i++)
00210 for (size_t j=0; j < m_Cols; j++)
00211 m_Val[i][j] = m.m_Val[i][j];
00212 return *this;
00213 }
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225 template <typename V, size_t N>
00226 CMatrixTemplate& operator = (V (&theArray)[N] )
00227 {
00228 MRPT_COMPILE_TIME_ASSERT(N!=0)
00229 if (m_Rows*m_Cols != N)
00230 {
00231 THROW_EXCEPTION(format("Mismatch between matrix size %lu x %lu and array of length %lu",m_Rows,m_Cols,N))
00232 }
00233 size_t idx=0;
00234 for (size_t i=0; i < m_Rows; i++)
00235 for (size_t j=0; j < m_Cols; j++)
00236 m_Val[i][j] = static_cast<T>(theArray[idx++]);
00237 return *this;
00238 }
00239
00240
00241
00242
00243 inline size_t getRowCount() const { return m_Rows; }
00244
00245
00246
00247
00248 inline size_t getColCount() const { return m_Cols; }
00249
00250
00251 inline CMatrixTemplateSize size() const
00252 {
00253 CMatrixTemplateSize dims;
00254 dims[0]=m_Rows;
00255 dims[1]=m_Cols;
00256 return dims;
00257 }
00258
00259
00260 void setSize(size_t row, size_t col,bool zeroNewElements=false)
00261 {
00262 realloc(row,col,zeroNewElements);
00263 }
00264
00265
00266 inline void resize(const CMatrixTemplateSize &siz,bool zeroNewElements=false)
00267 {
00268 setSize(siz[0],siz[1],zeroNewElements);
00269 }
00270
00271
00272
00273 inline T& operator () (size_t row, size_t col)
00274 {
00275 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00276 if (row >= m_Rows || col >= m_Cols)
00277 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00278 #endif
00279 return m_Val[row][col];
00280 }
00281
00282
00283
00284 inline const T &operator () (size_t row, size_t col) const
00285 {
00286 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00287 if (row >= m_Rows || col >= m_Cols)
00288 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00289 #endif
00290 return m_Val[row][col];
00291 }
00292
00293
00294
00295
00296 inline T& operator () (size_t ith)
00297 {
00298 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00299 ASSERT_(m_Rows==1 || m_Cols==1);
00300 #endif
00301 if (m_Rows==1)
00302 {
00303
00304 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00305 if (ith >= m_Cols)
00306 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00307 #endif
00308 return m_Val[0][ith];
00309 }
00310 else
00311 {
00312
00313 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00314 if (ith >= m_Rows)
00315 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00316 #endif
00317 return m_Val[ith][0];
00318 }
00319 }
00320
00321
00322
00323
00324 inline T operator () (size_t ith) const
00325 {
00326 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00327 ASSERT_(m_Rows==1 || m_Cols==1);
00328 #endif
00329 if (m_Rows==1)
00330 {
00331
00332 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00333 if (ith >= m_Cols)
00334 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00335 #endif
00336 return m_Val[0][ith];
00337 }
00338 else
00339 {
00340
00341 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00342 if (ith >= m_Rows)
00343 THROW_EXCEPTION_CUSTOM_MSG1( "Index %u out of range!",static_cast<unsigned>(ith) );
00344 #endif
00345 return m_Val[ith][0];
00346 }
00347 }
00348
00349
00350
00351 inline void set_unsafe(size_t row, size_t col,const T &v)
00352 {
00353 #ifdef _DEBUG
00354 if (row >= m_Rows || col >= m_Cols)
00355 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00356 #endif
00357 m_Val[row][col] = v;
00358 }
00359
00360
00361
00362 inline const T &get_unsafe(size_t row, size_t col) const
00363 {
00364 #ifdef _DEBUG
00365 if (row >= m_Rows || col >= m_Cols)
00366 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00367 #endif
00368 return m_Val[row][col];
00369 }
00370
00371
00372
00373 inline T &get_unsafe(size_t row,size_t col)
00374 {
00375 #ifdef _DEBUG
00376 if (row >= m_Rows || col >= m_Cols)
00377 THROW_EXCEPTION( format("Indexes (%lu,%lu) out of range. Matrix is %lux%lu",static_cast<unsigned long>(row),static_cast<unsigned long>(col),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00378 #endif
00379 return m_Val[row][col];
00380 }
00381
00382
00383
00384 inline T* get_unsafe_row(size_t row)
00385 {
00386 #ifdef _DEBUG
00387 if (row >= m_Rows)
00388 THROW_EXCEPTION( format("Row index %"PRIuPTR" out of range. Matrix is %"PRIuPTR"x%"PRIuPTR,static_cast<unsigned long>(row),static_cast<unsigned long>(m_Rows),static_cast<unsigned long>(m_Cols)) );
00389 #endif
00390 return m_Val[row];
00391 }
00392
00393
00394
00395 inline const T* get_unsafe_row(size_t row) const {
00396 return m_Val[row];
00397 }
00398
00399
00400
00401 inline CMatrixTemplate<T> operator() (const size_t row1,const size_t row2,const size_t col1,const size_t col2) const {
00402 CMatrixTemplate<T> val(0,0);
00403 extractSubmatrix(row1,row2,col1,col2,val);
00404 return val;
00405 }
00406
00407
00408
00409
00410 void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,CMatrixTemplate<T> &out) const
00411 {
00412 size_t nrows=row2-row1+1;
00413 size_t ncols=col2-col1+1;
00414 if (nrows<=0||ncols<=0) {
00415 out.realloc(0,0);
00416 return;
00417 }
00418 if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00419 out.realloc(nrows,ncols);
00420 for (size_t i=0;i<nrows;i++) for (size_t j=0;j<ncols;j++) out.m_Val[i][j]=m_Val[i+row1][j+col1];
00421 }
00422
00423 template <class Derived>
00424 void extractSubmatrix(const size_t row1,const size_t row2,const size_t col1,const size_t col2,Eigen::MatrixBase<Derived> &out) const
00425 {
00426 size_t nrows=row2-row1+1;
00427 size_t ncols=col2-col1+1;
00428 if (nrows<=0||ncols<=0) {
00429 out = typename Eigen::MatrixBase<Derived>::PlainObject();
00430 return;
00431 }
00432 if (row1<0||row2>=m_Rows||col1<0||col2>=m_Cols) THROW_EXCEPTION("Indices out of range!");
00433 out.resize(nrows,ncols);
00434 for (size_t i=0;i<nrows;i++) for (size_t j=0;j<ncols;j++) out.coeffRef(i,j)=m_Val[i+row1][j+col1];
00435 }
00436
00437
00438
00439
00440
00441
00442
00443 inline void extractRows(size_t firstRow,size_t lastRow,CMatrixTemplate<T> &out) const {
00444 out.setSize(lastRow-firstRow+1,m_Cols);
00445 detail::extractMatrix(*this,firstRow,0,out);
00446 }
00447
00448
00449
00450
00451
00452
00453 inline void extractColumns(size_t firstCol,size_t lastCol,CMatrixTemplate<T> &out) const {
00454 out.setSize(m_Rows,lastCol-firstCol+1);
00455 detail::extractMatrix(*this,0,firstCol,out);
00456 }
00457
00458
00459
00460
00461 void extractCol(size_t nCol, std::vector<T> &out, int startingRow = 0) const
00462 {
00463 size_t i,n;
00464 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00465 if (nCol>=m_Cols)
00466 THROW_EXCEPTION("extractCol: Column index out of bounds");
00467 #endif
00468
00469 n = m_Rows - startingRow;
00470 out.resize( n );
00471
00472 for (i=0;i<n;i++)
00473 out[i] = m_Val[i+startingRow][nCol];
00474 }
00475
00476
00477
00478
00479 void extractCol(size_t nCol, CMatrixTemplate<T> &out, int startingRow = 0) const
00480 {
00481 size_t i,n;
00482 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG_MATRICES)
00483 if (nCol>=m_Cols)
00484 THROW_EXCEPTION("extractCol: Column index out of bounds");
00485 #endif
00486
00487 n = m_Rows - startingRow;
00488 out.setSize(n,1);
00489
00490 for (i=0;i<n;i++)
00491 out(i,0) = m_Val[i+startingRow][nCol];
00492 }
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507 void appendRow(const std::vector<T> &in)
00508 {
00509 size_t i,n, row;
00510
00511 n = m_Cols;
00512 row = m_Rows;
00513
00514 if (m_Cols==0 || m_Rows==0)
00515 {
00516 ASSERT_(!in.empty());
00517 n=m_Cols=in.size();
00518 }
00519 else
00520 {
00521 ASSERT_(in.size()==m_Cols);
00522 }
00523
00524 realloc( row+1,n );
00525
00526 for (i=0;i<n;i++)
00527 m_Val[row][i] = in[i];
00528 }
00529
00530
00531
00532
00533
00534
00535
00536 void appendCol(const std::vector<T> &in) {
00537 size_t r=m_Rows,c=m_Cols;
00538 if (m_Cols==0||m_Rows==0) {
00539 ASSERT_(!in.empty());
00540 r=in.size();
00541 c=0;
00542 } else ASSERT_(in.size()==m_Rows);
00543 realloc(r,c+1);
00544 for (size_t i=0;i<m_Rows;i++) m_Val[i][m_Cols-1]=in[i];
00545 }
00546
00547
00548
00549
00550
00551 void insertCol(size_t nCol, const std::vector<T> &in)
00552 {
00553 if (nCol>=m_Cols) THROW_EXCEPTION("insertCol: Row index out of bounds");
00554
00555 size_t n = in.size();
00556 ASSERT_( m_Rows >= in.size() );
00557
00558 for (size_t i=0;i<n;i++)
00559 m_Val[i][nCol] = in[i];
00560 }
00561
00562
00563
00564 void getAsVector(std::vector<T> &out) const {
00565 out.clear();
00566 out.reserve(m_Rows*m_Cols);
00567 for (size_t i=0;i<m_Rows;i++) out.insert(out.end(),&(m_Val[i][0]),&(m_Val[i][m_Cols]));
00568 }
00569
00570 };
00571
00572
00573 }
00574 }
00575
00576
00577 #endif