block_matrix.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00033 #include <acado/matrix_vector/matrix_vector.hpp>
00034 
00035 using namespace std;
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 //
00040 // PUBLIC MEMBER FUNCTIONS:
00041 //
00042 
00043 BlockMatrix::BlockMatrix( )
00044 {
00045         nRows = nCols = 0;
00046 }
00047 
00048 BlockMatrix::BlockMatrix( uint _nRows, uint _nCols )
00049 {
00050         init(_nRows, _nCols);
00051 }
00052 
00053 
00054 BlockMatrix::BlockMatrix(       const DMatrix& value
00055                                                         )
00056 {
00057         init(1, 1);
00058 
00059         setDense(0, 0, value);
00060 }
00061 
00062 BlockMatrix::~BlockMatrix( )
00063 {}
00064 
00065 returnValue BlockMatrix::init( uint _nRows, uint _nCols )
00066 {
00067         nRows = _nRows;
00068         nCols = _nCols;
00069 
00070         vector< DMatrix > foo(nCols, DMatrix());
00071         for (unsigned row = 0; row < nRows; ++row)
00072                 elements.push_back( foo );
00073 
00074         vector< SubBlockMatrixType > bar(nCols, SBMT_ZERO);
00075         for (unsigned row = 0; row < nRows; ++row)
00076                 types.push_back( bar );
00077 
00078         return SUCCESSFUL_RETURN;
00079 }
00080 
00081 
00082 BlockMatrix BlockMatrix::operator+( const BlockMatrix& arg ) const{
00083 
00084         ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
00085 
00086         uint i,j;
00087 
00088         BlockMatrix tmp( arg );
00089 
00090         for( i=0; i < getNumRows(); i++ ){
00091         for( j=0; j < getNumCols(); j++ ){
00092 
00093             if( tmp.types[i][j] == SBMT_ZERO ){
00094 
00095                 tmp.types    [i][j] = types    [i][j];
00096                 tmp.elements [i][j] = elements [i][j];
00097             }
00098             else{
00099                 if( types[i][j] != SBMT_ZERO ){
00100 
00101                    tmp.types    [i][j]  = SBMT_DENSE     ;
00102                    tmp.elements [i][j] += elements [i][j];
00103                 }
00104             }
00105         }
00106     }
00107 
00108         return tmp;
00109 }
00110 
00111 
00112 BlockMatrix& BlockMatrix::operator+=( const BlockMatrix& arg ){
00113 
00114         ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
00115 
00116         uint i, j;
00117 
00118         for( i = 0; i < getNumRows(); i++ ){
00119         for( j = 0; j < getNumCols(); j++ ){
00120 
00121             if( types[i][j] == SBMT_ZERO ){
00122 
00123                 types    [i][j] = arg.types    [i][j];
00124                 elements [i][j] = arg.elements [i][j];
00125             }
00126             else{
00127                 if( arg.types[i][j] != SBMT_ZERO ){
00128 
00129                    types    [i][j]  = SBMT_DENSE         ;
00130                    elements [i][j] += arg.elements [i][j];
00131                 }
00132             }
00133         }
00134     }
00135         return *this;
00136 }
00137 
00138 
00139 BlockMatrix BlockMatrix::operator-( const BlockMatrix& arg ) const{
00140 
00141     ASSERT( ( getNumRows( ) == arg.getNumRows( ) ) && ( getNumCols( ) == arg.getNumCols( ) ) );
00142 
00143     uint i,j;
00144 
00145     BlockMatrix tmp( getNumRows(), getNumCols() );
00146 
00147     for( i=0; i < getNumRows(); i++ ){
00148         for( j=0; j < getNumCols(); j++ ){
00149 
00150             if( arg.types[i][j] == SBMT_ZERO ){
00151 
00152                 tmp.types    [i][j] = types    [i][j];
00153                 tmp.elements [i][j] = elements [i][j];
00154             }
00155             else{
00156                 if( types[i][j] != SBMT_ZERO ){
00157 
00158                    tmp.types    [i][j]  = SBMT_DENSE;
00159                    tmp.elements [i][j]  = elements [i][j] - arg.elements[i][j];
00160                 }
00161                 else{
00162 
00163                    DMatrix nn(arg.elements[i][j].getNumRows(),arg.elements[i][j].getNumCols());
00164                    nn.setZero();
00165 
00166                    tmp.types    [i][j]  = SBMT_DENSE;
00167                    tmp.elements [i][j]  = nn - arg.elements[i][j];
00168                 }
00169             }
00170         }
00171     }
00172 
00173     return tmp;
00174 }
00175 
00176 BlockMatrix BlockMatrix::operator*=( double scalar ){
00177 
00178         uint i,j;
00179 
00180         for( i=0; i < getNumRows(); i++ ){
00181         for( j=0; j < getNumCols(); j++ ){
00182             if( types[i][j] != SBMT_ZERO ){
00183                 types   [i][j]  = SBMT_DENSE;
00184                 elements[i][j] *= scalar    ;
00185             }
00186         }
00187     }
00188         return *this;
00189 }
00190 
00191 
00192 BlockMatrix BlockMatrix::operator*( const BlockMatrix& arg ) const{
00193 
00194     ASSERT( getNumCols( ) == arg.getNumRows( ) );
00195 
00196     uint i,j,k;
00197 
00198     uint newNumRows = getNumRows( );
00199     uint newNumCols = arg.getNumCols( );
00200     BlockMatrix result( newNumRows,newNumCols );
00201 
00202     for( i=0; i<newNumRows; ++i ){
00203         for( k=0; k<getNumCols( ); ++k ){
00204 
00205             switch( types[i][k] ){
00206 
00207                 case SBMT_DENSE:
00208 
00209                     for( j=0; j<newNumCols; ++j ){
00210 
00211                         if( arg.types[k][j] == SBMT_DENSE ){
00212 
00213                             if( result.types[i][j] != SBMT_ZERO )
00214                                   result.elements[i][j] += elements[i][k] * arg.elements[k][j];
00215                             else  result.elements[i][j]  = elements[i][k] * arg.elements[k][j];
00216                         }
00217 
00218                         if( arg.types[k][j] == SBMT_ONE ){
00219 
00220                             if( result.types[i][j] != SBMT_ZERO )
00221                                   result.elements[i][j] += elements[i][k];
00222                             else  result.elements[i][j]  = elements[i][k];
00223                         }
00224 
00225                         if( arg.types[k][j] != SBMT_ZERO )
00226                             result.types[i][j]  = SBMT_DENSE;
00227                     }
00228                     break;
00229 
00230 
00231                 case SBMT_ONE:
00232 
00233                     for( j=0; j<newNumCols; ++j ){
00234 
00235                          if( arg.types[k][j] == SBMT_DENSE ){
00236 
00237                              if( result.types[i][j] != SBMT_ZERO )
00238                                    result.elements[i][j] += arg.elements[k][j];
00239                              else  result.elements[i][j]  = arg.elements[k][j];
00240 
00241                              result.types[i][j]  = SBMT_DENSE;
00242                          }
00243 
00244                          if( arg.types[k][j] == SBMT_ONE ){
00245 
00246                              if( result.types[i][j] == SBMT_ZERO ){
00247                                    result.elements[i][j]  = elements[i][k];
00248                                    result.types   [i][j]  = SBMT_ONE      ;
00249                              }
00250                              else{
00251                                    result.elements[i][j] += elements[i][k];
00252                                    result.types   [i][j]  = SBMT_DENSE    ;
00253                              }
00254                          }
00255                      }
00256                      break;
00257 
00258                 case SBMT_ZERO:
00259 
00260                      break;
00261 
00262                 default:
00263                      break;
00264             }
00265         }
00266     }
00267 
00268     return result;
00269 }
00270 
00271 
00272 BlockMatrix BlockMatrix::operator^( const BlockMatrix& arg ) const{
00273 
00274         ASSERT( getNumRows( ) == arg.getNumRows( ) );
00275 
00276         uint i,j,k;
00277 
00278         uint newNumRows = getNumCols( );
00279         uint newNumCols = arg.getNumCols( );
00280         BlockMatrix result( newNumRows,newNumCols );
00281 
00282     for( i=0; i<newNumRows; ++i ){
00283         for( k=0; k<getNumRows( ); ++k ){
00284 
00285             switch( types[k][i] ){
00286 
00287                 case SBMT_DENSE:
00288 
00289                     for( j=0; j<newNumCols; ++j ){
00290 
00291                         if( arg.types[k][j] == SBMT_DENSE ){
00292                             if( result.types[i][j] != SBMT_ZERO )
00293                                   result.elements[i][j] += elements[k][i].transpose() * arg.elements[k][j];
00294                             else  result.elements[i][j]  = elements[k][i].transpose() * arg.elements[k][j];
00295                         }
00296 
00297                         if( arg.types[k][j] == SBMT_ONE ){
00298                             if( result.types[i][j] != SBMT_ZERO )
00299                                   result.elements[i][j] += elements[k][i].transpose();
00300                             else  result.elements[i][j]  = elements[k][i].transpose();
00301                         }
00302 
00303                         if( arg.types[k][j] != SBMT_ZERO )
00304                              result.types[i][j]  = SBMT_DENSE;
00305                     }
00306                     break;
00307 
00308 
00309                 case SBMT_ONE:
00310 
00311                     for( j=0; j<newNumCols; ++j ){
00312 
00313                         if( arg.types[k][j] == SBMT_DENSE ){
00314                             if( result.types[i][j] != SBMT_ZERO )
00315                                   result.elements[i][j] += arg.elements[k][j];
00316                             else  result.elements[i][j]  = arg.elements[k][j];
00317                             result.types[i][j]  = SBMT_DENSE;
00318                         }
00319 
00320                         if( arg.types[k][j] == SBMT_ONE ){
00321 
00322                             if( result.types[i][j] == SBMT_ZERO ){
00323                                   result.elements[i][j]  = elements[k][i];
00324                                   result.types   [i][j]  = SBMT_ONE      ;
00325                             }
00326                             else{
00327                                   result.elements[i][j] += elements[k][i];
00328                                   result.types   [i][j]  = SBMT_DENSE    ;
00329                             }
00330                         }
00331                     }
00332                     break;
00333 
00334                 case SBMT_ZERO:
00335                      break;
00336 
00337                 default:
00338                      break;
00339             }
00340         }
00341     }
00342         return result;
00343 }
00344 
00345 
00346 BlockMatrix BlockMatrix::transpose() const{
00347 
00348      BlockMatrix result( getNumCols(), getNumRows() );
00349 
00350      uint i,j;
00351 
00352      for( i = 0; i < getNumRows(); i++ ){
00353          for( j = 0; j < getNumCols(); j++ ){
00354              result.elements[j][i] = elements[i][j].transpose();
00355              result.types   [j][i] = types   [i][j]            ;
00356          }
00357      }
00358 
00359      return result;
00360 }
00361 
00362 
00363 BlockMatrix BlockMatrix::getAbsolute() const{
00364 
00365     uint run1, run2;
00366     BlockMatrix result( nRows, nCols );
00367 
00368     for( run1 = 0; run1 < nRows; run1++ ){
00369         for( run2 = 0; run2 < nCols; run2++ ){
00370 
00371             if( types[run1][run2] == SBMT_ONE )
00372                 result.setIdentity( run1, run2, elements[run1][run2].getNumRows() );
00373 
00374             if( types[run1][run2] == SBMT_DENSE )
00375                 result.setDense( run1, run2, elements[run1][run2].absolute() );
00376         }
00377     }
00378 
00379     return result;
00380 }
00381 
00382 
00383 BlockMatrix BlockMatrix::getPositive() const{
00384 
00385     uint run1, run2;
00386     BlockMatrix result( nRows, nCols );
00387 
00388     for( run1 = 0; run1 < nRows; run1++ ){
00389         for( run2 = 0; run2 < nCols; run2++ ){
00390 
00391             if( types[run1][run2] == SBMT_ONE )
00392                 result.setIdentity( run1, run2, elements[run1][run2].getNumRows() );
00393 
00394             if( types[run1][run2] == SBMT_DENSE )
00395                 result.setDense( run1, run2, elements[run1][run2].positive() );
00396         }
00397     }
00398 
00399     return result;
00400 }
00401 
00402 
00403 BlockMatrix BlockMatrix::getNegative() const{
00404 
00405     uint run1, run2;
00406     BlockMatrix result( nRows, nCols );
00407 
00408     for( run1 = 0; run1 < nRows; run1++ ){
00409         for( run2 = 0; run2 < nCols; run2++ ){
00410 
00411             if( types[run1][run2] == SBMT_DENSE )
00412                 result.setDense( run1, run2, elements[run1][run2].negative() );
00413         }
00414     }
00415 
00416     return result;
00417 }
00418 
00419 
00420 returnValue BlockMatrix::print( std::ostream& stream) const
00421 {
00422         stream << "Printing Block DMatrix:" << endl << endl;
00423 
00424         for (unsigned i = 0; i < getNumRows(); ++i)
00425         {
00426                 stream << "Row " << i << endl;
00427                 for (unsigned j = 0; j < getNumCols(); ++j)
00428                 {
00429             if(nRows * nCols)
00430             {
00431                 if( types[i][j] == SBMT_DENSE ) stream << elements[i][j] << endl;
00432                 if( types[i][j] == SBMT_ONE   ) stream << "ONE " << endl;
00433                 if( types[i][j] == SBMT_ZERO  ) stream << "ZERO " <<  endl;
00434             }
00435             else
00436                 stream << "ZERO " << endl;
00437             stream << endl;
00438                 }
00439                 stream << endl << endl;
00440         }
00441 
00442         return SUCCESSFUL_RETURN;
00443 }
00444 
00445 
00446 returnValue BlockMatrix::setDense( uint rowIdx, uint colIdx, const DMatrix& value ){
00447 
00448         ASSERT( rowIdx < getNumRows( ) );
00449         ASSERT( colIdx < getNumCols( ) );
00450 
00451     elements[rowIdx][colIdx] = value     ;
00452     types   [rowIdx][colIdx] = SBMT_DENSE;
00453 
00454     return SUCCESSFUL_RETURN;
00455 }
00456 
00457 
00458 returnValue BlockMatrix::addDense( uint rowIdx, uint colIdx, const DMatrix& value ){
00459 
00460         ASSERT( rowIdx < getNumRows( ) );
00461         ASSERT( colIdx < getNumCols( ) );
00462 
00463     if( types[rowIdx][colIdx] == SBMT_DENSE || types[rowIdx][colIdx] == SBMT_ONE ){
00464         types[rowIdx][colIdx] = SBMT_DENSE;
00465         elements[rowIdx][colIdx] += value;
00466         return SUCCESSFUL_RETURN;
00467     }
00468 
00469     return setDense( rowIdx, colIdx, value );
00470 }
00471 
00472 
00473 returnValue BlockMatrix::getSubBlock( uint rowIdx, uint colIdx,
00474                                       DMatrix &value, uint nR, uint nC )  const{
00475 
00476 
00477     ASSERT( rowIdx < getNumRows( ) );
00478     ASSERT( colIdx < getNumCols( ) );
00479 
00480 
00481     if( types[rowIdx][colIdx] != SBMT_ZERO ){
00482 
00483         ASSERT( nR == elements[rowIdx][colIdx].getNumRows( ) );
00484         ASSERT( nC == elements[rowIdx][colIdx].getNumCols( ) );
00485 
00486         value = elements[rowIdx][colIdx];
00487     }
00488     else{
00489         value.resize(nR, nC);
00490         value.setZero();
00491     }
00492 
00493     return SUCCESSFUL_RETURN;
00494 }
00495 
00496 CLOSE_NAMESPACE_ACADO
00497 
00498 /*
00499  *      end of file
00500  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:47