00001 #include "SMatrix.h"
00002
00003
00004
00005 #undef min
00006 #undef max
00007
00008 #ifndef min
00009 #define min(a,b) ( a<b ? a : b )
00010 #endif
00011 #ifndef max
00012 #define max(a,b) ( a>b ? a : b )
00013 #endif
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #define checkNRows(M) (m_NRows==M.m_NRows)
00027 #define checkNCols(M) (m_NCols==M.m_NCols)
00028 #define checkDims(M) (checkNRows(M) && checkNCols(M))
00029
00030 SMatrix::SMatrix(const dword nrows, const dword ncols)
00031 {
00032 init(nrows, ncols);
00033 }
00034
00035 SMatrix::SMatrix(const dword nrows, const dword ncols, const double s)
00036 {
00037 init(nrows, ncols);
00038 operator=(s);
00039 }
00040
00041 void SMatrix::init(dword nrows, dword ncols)
00042 {
00043 m_NRows = nrows;
00044 m_NCols = ncols;
00045 if(nrows && ncols) m_Data = new double[ncols*nrows];
00046 else m_Data = 0;
00047 }
00048
00049 void SMatrix::free()
00050 {
00051 if(m_Data)
00052 delete m_Data;
00053 m_Data = 0;
00054 m_NRows = m_NCols = 0;
00055 }
00056
00057 SMatrix::~SMatrix()
00058 {
00059 free();
00060 }
00061
00062 void SMatrix::resize(dword nrows, dword ncols)
00063 {
00064 if((m_NRows != nrows) || (m_NCols != ncols))
00065 {
00066 free();
00067 init(nrows, ncols);
00068 }
00069 }
00070
00072 SMatrix::SMatrix(const SMatrix& m)
00073 {
00074 init(m.m_NRows, m.m_NCols);
00075 operator=(m);
00076 }
00077
00079 SMatrix::SMatrix(dword nrows, dword ncols, const float* data)
00080 {
00081 init(nrows, ncols);
00082 operator=(data);
00083 }
00084
00086 SMatrix::SMatrix(dword nrows, dword ncols, const double* data)
00087 {
00088 init(nrows, ncols);
00089 operator=(data);
00090 }
00091
00093 SMatrix::SMatrix(const vuColourN& col)
00094 {
00095 init(col.nComponents()-1,1);
00096 double *dat = m_Data;
00097 const float * src = col.getData();
00098 for(dword i=0; i<m_NRows; i++, dat++, src++)
00099 {
00100 (*dat) = (*src);
00101 }
00102 }
00103
00105 SMatrix::SMatrix(const vuMatrix& m)
00106 {
00107 init(4,4);
00108 double *dat=m_Data;
00109 for(dword i=0; i<4; i++)
00110 for(dword j=0;j<4;j++, dat++)
00111 {
00112 (*dat) = m[i][j];
00113 }
00114 }
00115
00117 SMatrix::SMatrix(const linalg::Matrix& mat)
00118 {
00119
00120 linalg::MatrixDA eth((linalg::Matrix&)mat);
00121
00122 init(eth.q_row_upb()-eth.q_row_lwb()+1,
00123 eth.q_col_upb()-eth.q_col_lwb()+1);
00124 double *dat = m_Data;
00125
00126 for(register int i=eth.q_row_lwb(); i<=eth.q_row_upb(); i++)
00127 for(register int j=eth.q_col_lwb(); j<=eth.q_col_upb(); j++, dat++)
00128 *dat = eth(i,j);
00129 }
00130
00132 SMatrix::SMatrix(const linalg::Vector& v)
00133 {
00134 init(v.q_upb()-v.q_lwb()+1, 1);
00135 double *dat = m_Data;
00136 for(register int i=v.q_lwb(); i<=v.q_upb(); i++, dat++)
00137 {
00138 *dat = v(i);
00139 }
00140
00141 }
00142
00144 SMatrix::SMatrix(const coool::DensMatrix<double>& m)
00145 {
00146 init(m.numOfRows(), m.numOfCols());
00147
00148 double *dat = m_Data;
00149 for(dword i=0; i<m_NRows; i++)
00150 {
00151 coool::Vector<double> row = m[i];
00152 for(dword j=0; j<m_NCols; j++, dat++)
00153 *dat = row[j];
00154 }
00155 }
00156
00157 SMatrix::SMatrix(const coool::Vector<double>& v)
00158 {
00159 init(v.size(),1);
00160 double *dat = m_Data;
00161 for(dword i=0; i<m_NRows; i++, dat++)
00162 {
00163 *dat = v[i];
00164 }
00165 }
00166
00167
00169 SMatrix& SMatrix::operator=(const SMatrix& m)
00170 {
00171 resize(m.m_NRows, m.m_NCols);
00172 double *dat = m_Data;
00173 double *src = m.m_Data;
00174 dword N = m_NRows*m_NCols;
00175 for(dword i=0; i<N; i++, dat++, src++)
00176 {
00177 (*dat) = (*src);
00178 }
00179 return *this;
00180 }
00181
00183 SMatrix& SMatrix::operator=(const double s)
00184 {
00185 double *dat = m_Data;
00186 dword N = m_NRows*m_NCols;
00187 for(dword i=0; i<N; i++, dat++)
00188 {
00189 (*dat) = s;
00190 }
00191 return *this;
00192 }
00193
00195 SMatrix& SMatrix::operator=(const float* fdata)
00196 {
00197 double *dat = m_Data;
00198 float *src = (float*)fdata;
00199 dword N = m_NRows*m_NCols;
00200 for(dword i=0; i<N; i++, dat++, src++)
00201 {
00202 (*dat) = (*src);
00203 }
00204 return *this;
00205 }
00206
00208 SMatrix& SMatrix::operator=(const double* fdata)
00209 {
00210 double *dat = m_Data;
00211 double *src = (double*)fdata;
00212 dword N = m_NRows*m_NCols;
00213 for(dword i=0; i<N; i++, dat++, src++)
00214 {
00215 (*dat) = (*src);
00216 }
00217 return *this;
00218 }
00219
00221 double* SMatrix::operator[](unsigned int index)
00222 {
00223 return &m_Data[index * m_NCols];
00224 }
00225
00226
00228 const double* SMatrix::operator[](unsigned int index) const
00229 {
00230 return &m_Data[index * m_NCols];
00231 }
00232
00233
00235
00237 double* SMatrix::getData(void)
00238 {
00239 return m_Data;
00240 }
00241
00242 double const* SMatrix::getData(void) const
00243 {
00244 return m_Data;
00245 }
00246
00248 SMatrix::operator linalg::Matrix() const
00249 {
00250 linalg::Matrix mat(1,m_NRows, 1,m_NCols);
00251 linalg::MatrixDA eth(mat);
00252 double *dat = m_Data;
00253 for(register int i=eth.q_row_lwb(); i<=eth.q_row_upb(); i++)
00254 for(register int j=eth.q_col_lwb(); j<=eth.q_col_upb(); j++, dat++)
00255 eth(i,j) = *dat;
00256 return mat;
00257 }
00258
00260 SMatrix::operator coool::DensMatrix<double>() const
00261 {
00262 coool::DensMatrix<double> dmat(m_NRows, m_NCols);
00263 double *dat = m_Data;
00264 for(dword i=0; i<m_NRows; i++)
00265 {
00266 for(dword j=0; j<m_NCols; j++, dat++)
00267 dmat[i][j] = *dat;
00268 }
00269 return dmat;
00270 }
00271
00272 double SMatrix::norm2() const
00273 {
00274 double *dat = m_Data;
00275 double res=0;
00276 dword N = m_NRows*m_NCols;
00277 for(dword i=0; i<N; i++, dat++)
00278 res += (*dat)*(*dat);
00279 return res;
00280 }
00281
00282 double SMatrix::norm() const
00283 {
00284 return sqrt(norm2());
00285 }
00286
00288 SMatrix& SMatrix::makeIdentity(void)
00289 {
00290 makeZero();
00291 double *dat = m_Data;
00292 dword N = min(m_NRows, m_NCols);
00293 for(dword i=0; i<N; i++, dat+=m_NCols+1)
00294 {
00295 (*dat) = 1.0f;
00296 }
00297 return *this;
00298 }
00299
00300 SMatrix& SMatrix::makeZero(void)
00301 {
00302 operator=(0.0f);
00303 return *this;
00304 }
00305
00307 SMatrix& SMatrix::makeDiag(const SVector& diag)
00308 {
00309 makeZero();
00310 double *dat = m_Data;
00311 double const* src = diag.getData();
00312 dword N = min(m_NRows, m_NCols);
00313 for(dword i=0; i<N; i++, src++, dat+=m_NCols+1)
00314 {
00315 (*dat) = (*src);
00316 }
00317 return *this;
00318 }
00319
00321 SMatrix& SMatrix::makeToeplitz(const SVector& v, bool symmetric)
00322 {
00323 if(v.getSize() < m_NCols)
00324 throw "SMatrix::makeToeplitz Error: dimensions don't agree.";
00325 if(!symmetric || m_NRows<m_NCols) makeZero();
00326 dword N = min(m_NRows, m_NCols);
00327 double *dat = m_Data;
00328
00329 for(dword j=0; j<N; j++)
00330 {
00331 const double *src = v.getData();
00332 dat+=j;
00333 for(dword i=j; i<m_NRows; i++, src++, dat++)
00334 {
00335 (*dat) = (*src);
00336 }
00337 }
00338
00339 if(symmetric) {
00340 dat = m_Data+m_NCols;
00341 for(dword j=1; j<N; j++)
00342 {
00343 const double *src = v.getData()+1;
00344 for(dword i=0; i<j; i++, src++, dat--)
00345 {
00346 (*dat) = (*src);
00347 }
00348 dat+=j+m_NCols+1;
00349 }
00350 }
00351
00352 return *this;
00353 }
00354
00356 SMatrix& SMatrix::insert(const SMatrix& m, dword i, dword j)
00357 {
00358 if(m.m_NRows+i > m_NRows || m.m_NCols+j>m_NCols)
00359 throw "SMatrix::insert operand dimensions too big.";
00360 double *dst = m_Data + m_NCols*i + j;
00361 const double *src = m.m_Data;
00362 dword dstinc = m_NCols-m.m_NCols;
00363 for(dword a=0; a<m.m_NRows; a++, dst+=dstinc)
00364 for(dword b=0; b<m.m_NCols; b++, src++, dst++)
00365 (*dst) = (*src);
00366 return *this;
00367 }
00368
00370 SMatrix SMatrix::horizCat(const SMatrix& m) const
00371 {
00372 if(!checkNRows(m))
00373 throw "SMatrix::horizCat matrix dimensions must agree.";
00374 SMatrix ret(m_NRows, m_NCols+m.m_NCols);
00375 ret.insert(*this);
00376 ret.insert(m,0,m_NCols);
00377
00378 return ret;
00379 }
00380
00381
00383 SMatrix SMatrix::vertCat(const SMatrix& m) const
00384 {
00385 if(!checkNCols(m))
00386 throw "SMatrix::vertCat matrix dimensions must agree.";
00387
00388 SMatrix ret(m_NRows+m.m_NRows, m_NCols);
00389 ret.insert(*this);
00390 ret.insert(m,m_NRows,0);
00391
00392 return ret;
00393 }
00394
00396 SMatrix SMatrix::operator+(const SMatrix& m) const
00397 {
00398 if(checkDims(m)) {
00399 SMatrix res(*this);
00400 return res += m;
00401 } else throw "SMatrix::operator+ matrix dimensions don't agree.";
00402 }
00403
00405 SMatrix SMatrix::operator-(const SMatrix& m) const
00406 {
00407 if(checkDims(m)) {
00408 SMatrix res(*this);
00409 return res -= m;
00410 } else throw "SMatrix::operator- matrix dimensions don't agree.";
00411 }
00412
00414 SMatrix SMatrix::operator*(const SMatrix& m) const
00415 {
00416 if(m_NCols==m.m_NRows) {
00417 SMatrix res(m_NRows, m.m_NCols, 0.0f);
00418 for(dword h=0; h<m.m_NCols; h++)
00419 for(dword i=0; i<m_NRows; i++)
00420 {
00421 double &r= res[i][h];
00422 double *me = &m_Data[m_NCols * i];
00423 double *it = &m.m_Data[h];
00424 for(dword j=0; j<m_NCols; j++, me++, it += m.m_NCols)
00425 {
00426 r += (*me) * (*it);
00427 }
00428 }
00429 return res;
00430 } else throw "SMatrix::operator* matrix dimensions don't agree.";
00431 }
00432
00433
00435 SMatrix SMatrix::operator*(double s) const
00436 {
00437 SMatrix res(*this);
00438 return res *= s;
00439 }
00440
00442 SMatrix operator*(const double s, const SMatrix& m)
00443 {
00444 return m*s;
00445 }
00446
00448 SMatrix& SMatrix::operator+=(const SMatrix& m)
00449 {
00450 if(checkDims(m)) {
00451 double *dat = m_Data;
00452 double *src = m.m_Data;
00453 dword N = m_NRows*m_NCols;
00454 for(dword i=0; i<N; i++, dat++, src++)
00455 {
00456 (*dat) += (*src);
00457 }
00458
00459 return *this;
00460 } else throw "SMatrix::operator+ matrix dimensions don't agree.";
00461 }
00462
00464 SMatrix& SMatrix::operator-=(const SMatrix& m)
00465 {
00466 if(checkDims(m)) {
00467 double *dat = m_Data;
00468 double *src = m.m_Data;
00469 dword N = m_NRows*m_NCols;
00470 for(dword i=0; i<N; i++, dat++, src++)
00471 {
00472 (*dat) -= (*src);
00473 }
00474
00475 return *this;
00476 } else throw "SMatrix::operator- matrix dimensions don't agree.";
00477 }
00478
00480 SMatrix& SMatrix::operator*=(const SMatrix& m)
00481 {
00482 if(checkDims(m) && (m_NCols==m_NRows)) {
00483 SMatrix tmp(*this);
00484 for(dword h=0; h<m_NCols; h++)
00485 for(dword i=0; i<m_NRows; i++)
00486 {
00487 double &r= m_Data[m_NCols * i + h];
00488 r = 0;
00489 double *a = &m_Data[m_NCols * i];
00490 double *b = &m.m_Data[h];
00491 for(dword j=0; j<m_NCols; j++, a++, b += m_NCols)
00492 {
00493 r += (*a) * (*b);
00494 }
00495 }
00496 return *this;
00497 } else throw "SMatrix::operator*= matrix dimensions don't agree.";
00498 }
00499
00501 SMatrix& SMatrix::operator*=(double s)
00502 {
00503 double *dat = m_Data;
00504 dword N = m_NRows*m_NCols;
00505 for(dword i=0; i<N; i++, dat++)
00506 {
00507 (*dat) *= s;
00508 }
00509 return *this;
00510 }
00511
00512 ostream& operator<<(ostream& ofp, const SMatrix& A)
00513 {
00514 double *dat = A.m_Data;
00515 for (dword i = 0; i < A.m_NRows; i++)
00516 {
00517 for (dword j = 0; j < A.m_NCols; j++, dat++)
00518 ofp << (*dat) << " ";
00519
00520 ofp << endl;
00521 }
00522 return ofp;
00523 }
00524
00525 istream& operator>>(istream& ifp, SMatrix& A)
00526 {
00527 double *dat = A.m_Data;
00528 dword N = A.m_NRows* A.m_NCols;
00529 for (dword j = 0; j < N; j++, dat++)
00530 ifp >> (*dat);
00531
00532 return ifp;
00533 }
00534
00535
00536
00537
00538 SVector::operator coool::Vector<double>() const
00539 {
00540 coool::Vector<double> v(m_NRows);
00541 for(dword i=0; i<m_NRows;i++)
00542 {
00543 v[i] = m_Data[i];
00544 }
00545 return v;
00546 };
00547
00548 SVector::operator linalg::Vector() const
00549 {
00550 linalg::Vector v(1,m_NRows);
00551 double *dat = m_Data;
00552 for(register int i=v.q_lwb(); i<=v.q_upb(); i++, dat++)
00553 {
00554 v(i) = *dat;
00555 }
00556
00557 return v;
00558 }