matrix_LTI.cpp
Go to the documentation of this file.
00001 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
00002 
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017 //
00018 
00019 #include "../config.h"
00020 #ifdef __MATRIXWRAPPER_LTI__
00021 
00022 #include <iostream>
00023 #include "matrix_LTI.h"
00024 
00025 #include <ltilib/ltiMatrixInversion.h>
00026 #include <ltilib/ltiMatrixDecomposition.h>
00027 #include <ltilib/ltiSymmetricMatrixInversion.h>
00028 
00029 
00030 
00032 // CLASS MATRIX  //
00034 // Passing the constructor arguments...
00035 MyMatrix::Matrix() : ltiMatrix() {}
00036 MyMatrix::Matrix(int num_rows, int num_cols) : ltiMatrix(num_rows,
00037                                                          num_cols,
00038                                                          0.0){}
00039 // Destructor
00040 MyMatrix::~Matrix(){}
00041 
00042 // Copy constructor
00043 MyMatrix::Matrix(const MyMatrix& a) : ltiMatrix(a){}
00044 
00045 // This is a bad solution, but necessary if the base class is
00046 // ill-designed
00047 MyMatrix::Matrix(const ltiMatrix & a) : ltiMatrix(a){}
00048 
00049 // Number of Rows/Cols
00050 unsigned int MyMatrix::rows() const { return ((ltiMatrix)(*this)).rows();}
00051 unsigned int MyMatrix::columns() const { return ((ltiMatrix)(*this)).columns();}
00052 
00053 
00054 MyRowVector MyMatrix::rowCopy(unsigned int r) const
00055 {
00056   ltiMatrix temp = (ltiMatrix) *this;
00057   return (MyRowVector) temp.getRowCopy(r-1);
00058 }
00059 
00060 MyColumnVector MyMatrix::columnCopy(unsigned int c) const
00061 {
00062   ltiMatrix temp = (ltiMatrix) *this;
00063   return (MyColumnVector) temp.getColumnCopy(c-1);
00064 }
00065 
00066 double& MyMatrix::operator()(unsigned int a, unsigned int b)
00067 {
00068   //ltiMatrix & op1 = (*this);
00069   return this->at(a-1,b-1);
00070 }
00071 
00072 const double MyMatrix::operator()(unsigned int a, unsigned int b) const
00073 {
00074   //ltiMatrix  op1(*this);
00075   return this->at(a-1,b-1);
00076 }
00077 
00078 const bool MyMatrix::operator==(const MyMatrix& a) const
00079 {
00080   const ltiMatrix& op1 = *this;
00081   const ltiMatrix& op2 = a;
00082   return (op1 == op2);
00083 }
00084 
00085 
00086 // MATRIX - SCALAR operators
00087 MyMatrix& MyMatrix::operator+= (double a)
00088 {
00089   ltiMatrix & op1 = (*this);
00090   op1 += a;
00091   return (MyMatrix&) op1;
00092 }
00093 
00094 MyMatrix& MyMatrix::operator-= (double a)
00095 {
00096   ltiMatrix & op1 = (*this);
00097   op1 -= a;
00098   return (MyMatrix&) op1;
00099 }
00100 
00101 MyMatrix& MyMatrix::operator*= (double a)
00102 {
00103   ltiMatrix & op1 = (*this);
00104   op1 *= a;
00105   return (MyMatrix&) op1;
00106 }
00107 
00108 MyMatrix MyMatrix::operator+ (double a) const
00109 {
00110   ltiMatrix op1(*this);
00111   op1 += a;
00112   return (MyMatrix) op1;
00113 }
00114 
00115 MyMatrix MyMatrix::operator- (double a) const
00116 {
00117   ltiMatrix op1(*this);
00118   op1 -= a;
00119   return (MyMatrix) op1;
00120 }
00121 
00122 MyMatrix MyMatrix::operator* (double a) const
00123 {
00124   ltiMatrix op1(*this);
00125   op1 *= a;
00126   return (MyMatrix) op1;
00127 }
00128 
00129 
00130 MyMatrix& MyMatrix::operator/= (double a)
00131 {
00132   ltiMatrix & op1 = (*this);
00133   op1 /= a;
00134   return (MyMatrix&) op1;
00135 }
00136 
00137 
00138 // MATRIX - MATRIX Operators
00139 MyMatrix MyMatrix::operator- (const MyMatrix& a) const
00140 {
00141   ltiMatrix op1 = (*this);
00142   return (MyMatrix) (op1.subtract(a));
00143 }
00144 
00145 MyMatrix MyMatrix::operator+ (const MyMatrix& a) const
00146 {
00147   ltiMatrix op1 = (*this);
00148   return (MyMatrix) (op1.add(a));
00149 }
00150 
00151 MyMatrix MyMatrix::operator* (const MyMatrix& a) const
00152 {
00153   ltiMatrix op1 = (*this);
00154   return (MyMatrix) (op1.multiply(a));
00155 }
00156 
00157 MyMatrix MyMatrix::operator/ (double b) const
00158 {
00159   ltiMatrix op1(*this);
00160   op1 /= b;
00161   return (MyMatrix) op1;
00162 }
00163 
00164 
00165 MyMatrix & MyMatrix::operator+= (const MyMatrix& a)
00166 {
00167   ltiMatrix & op1 = (*this);
00168   op1 += a;
00169   return (MyMatrix &) op1;
00170 }
00171 
00172 MyMatrix & MyMatrix::operator-= (const MyMatrix& a)
00173 {
00174   ltiMatrix & op1 = (*this);
00175   op1 -= a;
00176   return (MyMatrix &) op1;
00177 }
00178 
00179 
00180 // MATRIX - VECTOR Operators
00181 MyColumnVector MyMatrix::operator* (const MyColumnVector &b) const
00182 {
00183   const ltiMatrix& op1 = *this;
00184   ltiColumnVector op2(b);
00185   return (MyColumnVector) op1.multiply(op2);
00186 }
00187 
00188 // Set all elements equal to a
00189 MyMatrix &MyMatrix::operator=(const double a)
00190 {
00191   ltiMatrix & op1 = (*this);
00192   op1.fill(a,0,0,(*this).rows(),(*this).columns());
00193   return (MyMatrix&) op1;
00194 }
00195 
00196 
00197 
00198 MyMatrix MyMatrix::transpose() const
00199 {
00200   ltiMatrix base(*this);
00201   base.transpose();
00202   return (MyMatrix) base;
00203 }
00204 
00205 double MyMatrix::determinant() const
00206 {
00207   ltiMatrix & base = (ltiMatrix &) *this;
00208   lti::matrix<double> tmp;
00209   tmp.resize(base.size());
00210   for (int i=0;i<tmp.rows();i++)
00211     for (int j=0;j<tmp.columns();j++) {
00212       tmp.at(i,j)=(double)(base.at(i,j));
00213     }
00214   lti::luDecomposition<double> lu;
00215   return lu.det(tmp);
00216 }
00217 
00218 
00219 MyMatrix MyMatrix::inverse() const
00220 {
00221   lti::matrixInversion<double> inv;
00222   ltiMatrix base(*this);
00223   inv.apply(base);
00224   return (MyMatrix) base;
00225 }
00226 
00227 
00228 int
00229 MyMatrix::convertToSymmetricMatrix(MySymmetricMatrix& sym)
00230 {
00231   // test if matrix is square matrix
00232   assert(this->rows() == this->columns() );
00233 
00234   // if necessairy, resize sym
00235   // only check cols or rows. Symmetric matrix is square.
00236   if ( sym.rows() != this->rows() )
00237     sym.resize(this->rows());
00238 
00239   // copy elements
00240   for ( unsigned int i=0; i<this->rows(); i++ )
00241     for ( unsigned int j=0; j<=i; j++ )
00242     {
00243             sym[i][j] = (*this)[i][j];
00244             sym[j][i] = (*this)[i][j];
00245     }
00246   return 0;
00247 }
00248 
00249 // get sub matrix
00250 MyMatrix MyMatrix::sub(int i_start, int i_end, int j_start , int j_end) const
00251 {
00252   ltiMatrix m(*this,i_start-1,i_end-1, j_start-1,j_end-1);
00253   return (MyMatrix) m;
00254 }
00255 
00256 
00257 void
00258 MyMatrix::resize(unsigned int i, unsigned int j, bool copy, bool initialize)
00259 {
00260   ltiMatrix& base = (ltiMatrix &) *this;
00261   base.resize(i,j, copy, initialize);
00262 }
00263 
00265 // CLASS SYMMETRIC MATRIX  //
00267 
00268 MySymmetricMatrix::SymmetricMatrix() : ltiSymmetricMatrix() {}
00269 MySymmetricMatrix::SymmetricMatrix(int n) : ltiSymmetricMatrix(n,n) {}
00270 
00271 // Copy constructor
00272 MySymmetricMatrix::SymmetricMatrix(const SymmetricMatrix& a) : ltiSymmetricMatrix(a){}
00273 MySymmetricMatrix::SymmetricMatrix(const ltiSymmetricMatrix & a) : ltiSymmetricMatrix(a){}
00274 
00275 // Destructor
00276 MySymmetricMatrix::~SymmetricMatrix(){}
00277 
00278 // Ask Number of Rows and Columns
00279 unsigned int MySymmetricMatrix::rows() const { return (((ltiSymmetricMatrix)(*this)).rows());}
00280 unsigned int MySymmetricMatrix::columns() const { return (((ltiSymmetricMatrix)(*this)).rows());}
00281 
00282 MySymmetricMatrix MySymmetricMatrix::transpose() const
00283 {return (*this);}
00284 
00285 MySymmetricMatrix MySymmetricMatrix::inverse() const
00286 {
00287   lti::matrixInversion<double> inv;
00288   ltiSymmetricMatrix base(*this);
00289   inv.apply(base);
00290   return (MySymmetricMatrix) base;
00291 }
00292 
00293 double MySymmetricMatrix::determinant() const
00294 {
00295   ltiSymmetricMatrix & base = (ltiSymmetricMatrix &) *this;
00296   lti::matrix<double> tmp;
00297   tmp.resize(base.size());
00298   for (int i=0;i<tmp.rows();i++)
00299     for (int j=0;j<tmp.columns();j++) {
00300       tmp.at(i,j)=(double)(base.at(i,j));
00301     }
00302   lti::luDecomposition<double> lu;
00303   return lu.det(tmp);
00304 }
00305 
00306 
00307 double& MySymmetricMatrix::operator()(unsigned int a, unsigned int b)
00308 {
00309   ltiSymmetricMatrix & op1 = (*this);
00310   // only fill in lower triangle
00311   if (a < b)
00312     return op1.at(b-1,a-1);
00313   else
00314     return op1.at(a-1,b-1);
00315 }
00316 const double MySymmetricMatrix::operator()(unsigned int a, unsigned int b) const
00317 {
00318   ltiSymmetricMatrix op1(*this);
00319   // only fill in lower triangle
00320   if (a < b)
00321     return op1.at(b-1,a-1);
00322   else
00323     return op1.at(a-1,b-1);
00324 }
00325 
00326 
00327 const bool MySymmetricMatrix::operator==(const MySymmetricMatrix& a) const
00328 {
00329   const ltiSymmetricMatrix& op1 = *this;
00330   const ltiSymmetricMatrix& op2 = a;
00331   return (op1 == op2);
00332 }
00333 
00334 // MATRIX - SCALAR operators
00335 MySymmetricMatrix& MySymmetricMatrix::operator=(const double a)
00336 {
00337   ltiSymmetricMatrix temp = (ltiSymmetricMatrix) *this;
00338   temp.fill(a,0,0,temp.rows(),temp.columns());
00339   *this = (MySymmetricMatrix) temp;
00340 
00341   return *this;
00342 }
00343 
00344 // MATRIX - SCALAR operators
00345 MySymmetricMatrix&
00346 MySymmetricMatrix::operator +=(double a)
00347 {
00348   ltiSymmetricMatrix & op1 = (*this);
00349   op1 += a;
00350   return (MySymmetricMatrix&) op1;
00351 }
00352 
00353 MySymmetricMatrix&
00354 MySymmetricMatrix::operator -=(double a)
00355 {
00356   ltiSymmetricMatrix & op1 = (*this);
00357   op1 -= a;
00358   return (MySymmetricMatrix&) op1;
00359 }
00360 
00361 MySymmetricMatrix&
00362 MySymmetricMatrix::operator *=(double b)
00363 {
00364   ltiSymmetricMatrix & op1 = (*this);
00365   op1 *= b;
00366   return (MySymmetricMatrix&) op1;
00367 }
00368 
00369 MySymmetricMatrix&
00370 MySymmetricMatrix::operator /=(double b)
00371 {
00372   ltiSymmetricMatrix & op1 = (*this);
00373   op1 /= b;
00374   return (MySymmetricMatrix&) op1;
00375 }
00376 
00377 MySymmetricMatrix
00378 MySymmetricMatrix::operator+ (double a) const
00379 {
00380   ltiSymmetricMatrix op1(*this);
00381   op1 += a;
00382   return (MySymmetricMatrix) op1;
00383 }
00384 
00385 MySymmetricMatrix
00386 MySymmetricMatrix::operator- (double a) const
00387 {
00388   ltiSymmetricMatrix op1(*this);
00389   op1 -= a;
00390   return (MySymmetricMatrix) op1;
00391 }
00392 
00393 MySymmetricMatrix MySymmetricMatrix::operator* (double a) const
00394 {
00395   ltiSymmetricMatrix op1(*this);
00396   op1 *= a;
00397   return (MySymmetricMatrix) op1;
00398 }
00399 
00400 MySymmetricMatrix MySymmetricMatrix::operator/ (double b) const
00401 {
00402   ltiSymmetricMatrix op1(*this);
00403   op1 /= b;
00404   return (MySymmetricMatrix) op1;
00405 }
00406 
00407 
00408 
00409 
00410 
00411 // SYMMETRICMATRIX - MATRIX operators
00412 MyMatrix& MySymmetricMatrix::operator +=(const MyMatrix& a)
00413 {
00414   ltiSymmetricMatrix & op1 = (*this);
00415   const ltiMatrix & op2 = a;
00416   op1 += op2;
00417   return (MyMatrix &) op1;
00418 }
00419 
00420 MyMatrix&
00421 MySymmetricMatrix::operator -=(const MyMatrix& a)
00422 {
00423   ltiSymmetricMatrix & op1 = (*this);
00424   const ltiMatrix & op2 = a;
00425   op1 -= op2;
00426   return (MyMatrix &) op1;
00427 }
00428 
00429 
00430 MyMatrix
00431 MySymmetricMatrix::operator+ (const MyMatrix &a) const
00432 {
00433   ltiMatrix op1(*this);
00434   return (MyMatrix) (op1.add(a));
00435 }
00436 
00437 MyMatrix
00438 MySymmetricMatrix::operator- (const MyMatrix &a) const
00439 {
00440 
00441   ltiMatrix op1(*this);
00442   return (MyMatrix) (op1.subtract(a));
00443 }
00444 
00445 MyMatrix
00446 MySymmetricMatrix::operator* (const MyMatrix &a) const
00447 {
00448   ltiMatrix op1(*this);
00449   return (MyMatrix) (op1.multiply(a));
00450 }
00451 
00452 MyMatrix&
00453 MyMatrix::operator =(const MySymmetricMatrix& a)
00454 {
00455   *this =(MyMatrix) a;
00456 
00457   return *this;
00458 }
00459 
00460 MySymmetricMatrix&
00461 MySymmetricMatrix::operator +=(const MySymmetricMatrix& a)
00462 {
00463   ltiSymmetricMatrix & op1 = (*this);
00464   const ltiSymmetricMatrix & op2 = a;
00465   op1 += op2;
00466   return (MySymmetricMatrix &) op1;
00467 }
00468 
00469 MySymmetricMatrix&
00470 MySymmetricMatrix::operator -=(const MySymmetricMatrix& a)
00471 {
00472   ltiSymmetricMatrix & op1 = (*this);
00473   const ltiSymmetricMatrix & op2 = a;
00474   op1 -= op2;
00475   return (MySymmetricMatrix &) op1;
00476 }
00477 
00478 
00479 MySymmetricMatrix
00480 MySymmetricMatrix::operator+ (const MySymmetricMatrix &a) const
00481 {
00482   ltiSymmetricMatrix op1 = (*this);
00483   op1 += a;
00484   return (MySymmetricMatrix &) op1;
00485 }
00486 
00487 MySymmetricMatrix
00488 MySymmetricMatrix::operator- (const MySymmetricMatrix &a) const
00489 {
00490   ltiSymmetricMatrix op1 = (*this);
00491   op1 -= a;
00492   return (MySymmetricMatrix &) op1;
00493 }
00494 
00495 MyMatrix
00496 MySymmetricMatrix::operator* (const MySymmetricMatrix &a) const
00497 {
00498   ltiSymmetricMatrix op1 = (*this);
00499   return (MyMatrix) (op1.multiply(a));
00500 }
00501 
00502 
00503 
00504 
00505 
00506 
00507 MyColumnVector MySymmetricMatrix::operator* (const MyColumnVector &b) const
00508 {
00509   const ltiSymmetricMatrix& op1 = (const ltiSymmetricMatrix&) *this;
00510   ltiColumnVector op2 = b;
00511   return (MyColumnVector) op1.multiply(op2);
00512 }
00513 
00514 void MySymmetricMatrix::multiply (const MyColumnVector &b, MyColumnVector &result) const
00515 {
00516   const ltiSymmetricMatrix& op1 = (const ltiSymmetricMatrix&) *this;
00517   ltiColumnVector op2 = b;
00518   result = (MyColumnVector) op1.multiply(op2);
00519 }
00520 
00521 MyMatrix MySymmetricMatrix::sub(int i_start, int i_end, int j_start , int j_end) const
00522 {
00523   // first copy all elements from lower triangle to upper triangle
00524   unsigned int r = this->rows();
00525   unsigned int c = this->columns();
00526   ltiMatrix copy = *this;
00527   for (unsigned int i=0; i<r; i++)
00528     for (unsigned int j=0; j<=i; j++)
00529       copy.at(j,i) = copy.at(i,j);
00530   ltiMatrix m(copy,i_start-1,i_end-1, j_start-1,j_end-1);
00531   return (MyMatrix) m;
00532 }
00533 
00534 
00535 void
00536 MySymmetricMatrix::resize(unsigned int i, bool copy, bool initialize)
00537 {
00538   ltiSymmetricMatrix& base = (ltiSymmetricMatrix &) *this;
00539   base.resize(i, i, copy, initialize);
00540 }
00541 
00542 
00543 #endif


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Fri Aug 28 2015 10:10:21