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.hpp>
00034 #include <acado/matrix_vector/acado_mat_file.hpp>
00035 #include <iomanip>
00036 
00037 using namespace std;
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 template<typename T>
00042 GenericMatrix<T>::GenericMatrix(        unsigned _nRows,
00043                                                                         unsigned _nCols,
00044                                                                         std::vector< std::vector< T > >& _values
00045                                                                         )
00046         : Base(_nRows, _nCols)
00047 {
00048         ASSERT( _values.size() > 0 );
00049 
00050         unsigned nRows = _values.size();
00051         unsigned nCols = _values[ 0 ].size();
00052 
00053         for (unsigned r = 0; r < nRows; ++r)
00054         {
00055                 ASSERT( _values[ r ].size() == nCols );
00056 
00057                 std::copy(_values[ r ].begin(), _values[ r ].end(), Base::data() + r * nCols);
00058         }
00059 }
00060 
00061 template<typename T>
00062 GenericMatrix<T>& GenericMatrix<T>::appendRows( const GenericMatrix& _arg
00063 )
00064 {
00065         if (getDim() == 0)
00066         {
00067                 Base::_set( _arg );
00068                 return *this;
00069         }
00070 
00071         ASSERT(Base::cols() == _arg.cols());
00072 
00073         unsigned oldRows = Base::rows();
00074         unsigned argRows = _arg.rows();
00075 
00076         Base::conservativeResize(oldRows + argRows, Base::cols());
00077         Base::block(oldRows, 0, argRows, Base::cols()) = _arg;
00078 
00079         return *this;
00080 }
00081 
00082 template<typename T>
00083 GenericMatrix<T>& GenericMatrix<T>::appendCols( const GenericMatrix& _arg
00084 )
00085 {
00086         if (getDim() == 0)
00087         {
00088                 Base::_set( _arg );
00089                 return *this;
00090         }
00091 
00092         ASSERT(Base::rows() == _arg.rows());
00093 
00094         unsigned oldCols = Base::cols();
00095         unsigned argCols = _arg.cols();
00096 
00097         Base::conservativeResize(Base::rows(), oldCols + argCols);
00098         Base::block(0, oldCols, Base::rows(), argCols) = _arg;
00099 
00100         return *this;
00101 }
00102 
00103 template<typename T>
00104 GenericVector< T > GenericMatrix< T >::sumCol() const
00105 {
00106         GenericVector< T > foo(Base::rows());
00107         for (unsigned r = 0; r < Base::rows(); ++r)
00108                 foo( r ) = Base::row( r ).sum();
00109 
00110         return foo;
00111 }
00112 
00113 template<typename T>
00114 GenericVector< T > GenericMatrix<T>::sumRow() const
00115 {
00116         GenericVector< T > foo(Base::cols());
00117         for (unsigned c = 0; c < Base::cols(); ++c)
00118                 foo( c ) = Base::col( c ).sum();
00119 
00120         return foo;
00121 }
00122 
00123 template<typename T>
00124 GenericMatrix<T>& GenericMatrix<T>::makeVector( )
00125 {
00126         Base::_set(GenericVector< T >(Base::cols() * Base::rows(), Base::data()));
00127         return *this;
00128 }
00129 
00130 template<typename T>
00131 GenericVector< T > GenericMatrix< T >::getDiag( ) const
00132 { return Base::diagonal(); }
00133 
00134 template<typename T>
00135 bool GenericMatrix<T>::isSquare() const
00136 { return Base::rows() == Base::cols(); }
00137 
00138 template<typename T>
00139 bool GenericMatrix<T>::isSymmetric( ) const
00140 { return Base::operator==(Base::transpose()); }
00141 
00142 template<>
00143 bool GenericMatrix<double>::isSymmetric( ) const
00144 {
00145         if ( isSquare( ) == BT_FALSE )
00146                 return BT_FALSE;
00147 
00148         for (unsigned r = 0; r < getNumRows(); ++r)
00149                 for (unsigned c = r + 1; c < getNumRows(); ++c)
00150                         if (acadoIsEqual(Base::operator()(r, c), Base::operator()(c, r)) == false)
00151                                 return false;
00152         return true;
00153 }
00154 
00155 template<typename T>
00156 returnValue GenericMatrix<T>::symmetrize( )
00157 {
00158         if ( isSquare( ) == false )
00159                 return ACADOERROR( RET_MATRIX_NOT_SQUARE );
00160 
00161         for (unsigned i = 0; i < getNumRows(); ++i)
00162                 for (unsigned j = i + 1; j < getNumRows(); ++j)
00163                 {
00164                         T m = (Base::operator()(i, j) + Base::operator()(j, i)) / T( 2 );
00165                         Base::operator()(i, j) = m;
00166                         Base::operator()(j, i) = m;
00167                 }
00168 
00169         return SUCCESSFUL_RETURN;
00170 }
00171 
00172 template<typename T>
00173 bool GenericMatrix< T >::isPositiveSemiDefinite( ) const
00174 {
00175         Eigen::LDLT< Base > foo( Base::size() );
00176         foo.compute( *this );
00177         if (foo.info() == Eigen::Success && foo.isPositive() == true)
00178                 return true;
00179 
00180         return false;
00181 }
00182 
00183 template<typename T>
00184 bool GenericMatrix< T >::isPositiveDefinite( ) const
00185 {
00186         if (this->llt().info() == Eigen::Success)
00187                 return true;
00188 
00189         return false;
00190 }
00191 
00192 template<typename T>
00193 GenericMatrix< T > GenericMatrix< T >::absolute() const
00194 { return Base::cwiseAbs(); }
00195 
00196 template<typename T>
00197 GenericMatrix< T > GenericMatrix< T >::positive() const
00198 {
00199         GenericMatrix foo = *this;
00200         unsigned dim = foo.rows() * foo.cols();
00201         for (unsigned el = 0; el < dim; ++el)
00202         {
00203                 T bar = foo.data()[ el ];
00204                 if (bar < T( 0 ))
00205                         foo.data()[ el ] = T( 0 );
00206         }
00207 
00208         return foo;
00209 }
00210 
00211 template<typename T>
00212 GenericMatrix<T> GenericMatrix<T>::negative() const
00213 {
00214         GenericMatrix foo = *this;
00215         unsigned dim = foo.rows() * foo.cols();
00216         for (unsigned el = 0; el < dim; ++el)
00217         {
00218                 T bar = foo.data()[ el ];
00219                 if (bar > T( 0 ))
00220                         foo.data()[ el ] = T( 0 );
00221         }
00222 
00223         return foo;
00224 }
00225 
00226 template<typename T>
00227 T GenericMatrix< T >::getNorm( ) const
00228 { return Base::norm(); }
00229 
00230 template<typename T>
00231 T GenericMatrix< T >::getTrace( ) const
00232 { return Base::trace(); }
00233 
00234 template<typename T>
00235 T GenericMatrix< T >::getConditionNumber() const
00236 {
00237         ASSERT(1 == 0);
00238 
00239         return T( 0 );
00240 }
00241 
00242 template<>
00243 double GenericMatrix< double >::getConditionNumber() const
00244 {
00245         ASSERT(isSquare() == true);
00246 
00247         Eigen::JacobiSVD<Base, Eigen::NoQRPreconditioner> foo( *this );
00248         const GenericVector< double > V = foo.singularValues();
00249 
00250         return (V( 0 ) / V(V.size() - 1));
00251 }
00252 
00253 template<typename T>
00254 returnValue GenericMatrix< T >::print(  std::ostream& _stream,
00255                                                                                 const std::string& _name,
00256                                                                                 const std::string& _startString,
00257                                                                                 const std::string& _endString,
00258                                                                                 uint _width,
00259                                                                                 uint _precision,
00260                                                                                 const std::string& _colSeparator,
00261                                                                                 const std::string& _rowSeparator
00262                                                                                 ) const
00263 {
00264         if (_name.size())
00265                 _stream << _name << " = ";
00266 
00267         _stream << _startString;
00268 
00269         for (unsigned r = 0; r < Base::rows(); ++r)
00270         {
00271                 for (unsigned c = 0; c < Base::cols(); ++c)
00272                 {
00273                         _stream << Base::operator()(r, c);
00274 
00275                         if (c < (Base::cols() - 1))
00276                                 _stream << _colSeparator;
00277                 }
00278 
00279                 if (r < (Base::rows() - 1))
00280                         _stream << _rowSeparator;
00281         }
00282 
00283         _stream << _endString;
00284 
00285         return SUCCESSFUL_RETURN;
00286 }
00287 
00288 template<>
00289 returnValue GenericMatrix< double >::print(     std::ostream& _stream,
00290                                                                                         const std::string& _name,
00291                                                                                         const std::string& _startString,
00292                                                                                         const std::string& _endString,
00293                                                                                         uint _width,
00294                                                                                         uint _precision,
00295                                                                                         const std::string& _colSeparator,
00296                                                                                         const std::string& _rowSeparator
00297                                                                                         ) const
00298 {
00299         IoFormatter iof( _stream );
00300 
00301         if (_name.size())
00302                 _stream << _name << " = ";
00303 
00304         _stream << _startString;
00305 
00306         iof.set(_precision,
00307                         _width,
00308                         _precision > 0 ? ios::scientific | ios::right : ios::fixed);
00309 
00310         for (unsigned r = 0; r < Base::rows(); ++r)
00311         {
00312                 for (unsigned c = 0; c < Base::cols(); ++c)
00313                 {
00314                         _stream << Base::operator()(r, c);
00315 
00316                         if (c < (Base::cols() - 1))
00317                                 _stream << _colSeparator;
00318                 }
00319 
00320                 if (r < (Base::rows() - 1))
00321                         _stream << _rowSeparator;
00322         }
00323 
00324         _stream << _endString;
00325 
00326         iof.reset();
00327 
00328         return SUCCESSFUL_RETURN;
00329 }
00330 
00331 template<typename T>
00332 returnValue GenericMatrix< T >::print(  std::ostream& _stream,
00333                                                                                 const std::string& _name,
00334                                                                                 PrintScheme _printScheme
00335                                                                                 ) const
00336 {
00337         MatFile< T >* matFile;
00338 
00339         switch ( _printScheme )
00340         {
00341         case PS_MATLAB_BINARY:
00342                 matFile = new MatFile<T>();
00343 
00344                 matFile->write(_stream, *this, _name.c_str());
00345 
00346                 delete matFile;
00347 
00348                 return SUCCESSFUL_RETURN;
00349 
00350         default:
00351                 char* startString = 0;
00352                 char* endString = 0;
00353                 uint width = 0;
00354                 uint precision = 0;
00355                 char* colSeparator = 0;
00356                 char* rowSeparator = 0;
00357 
00358                 returnValue ret = getGlobalStringDefinitions(_printScheme, &startString,
00359                                 &endString, width, precision, &colSeparator, &rowSeparator);
00360                 if (ret != SUCCESSFUL_RETURN)
00361                         return ret;
00362 
00363                 returnValue status = print(_stream, _name, startString, endString, width,
00364                                 precision, colSeparator, rowSeparator);
00365 
00366                 if ( startString != 0 )   delete[] startString;
00367                 if ( endString != 0 )     delete[] endString;
00368                 if ( colSeparator != 0 )  delete[] colSeparator;
00369                 if ( rowSeparator != 0 )  delete[] rowSeparator;
00370 
00371                 return status;
00372         }
00373 }
00374 
00375 template<typename T>
00376 returnValue GenericMatrix<T>::print(    const std::string& _filename,
00377                                                                                 const std::string& _name,
00378                                                                                 const std::string& _startString,
00379                                                                                 const std::string& _endString,
00380                                                                                 uint _width,
00381                                                                                 uint _precision,
00382                                                                                 const std::string& _colSeparator,
00383                                                                                 const std::string& _rowSeparator
00384                                                                                 ) const
00385 {
00386         ofstream stream( _filename.c_str() );
00387 
00388         if ( stream.is_open() )
00389                 return print(stream, _name, _startString, _endString, _width, _precision,
00390                                 _colSeparator, _rowSeparator);
00391         else
00392                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00393 
00394         stream.close();
00395 
00396         return SUCCESSFUL_RETURN;
00397 }
00398 
00399 template<typename T>
00400 returnValue GenericMatrix< T >::print(  const std::string& _filename,
00401                                                                                 const std::string& _name,
00402                                                                                 PrintScheme _printScheme
00403                                                                                 ) const
00404 {
00405         ofstream stream( _filename.c_str() );
00406         returnValue status;
00407 
00408         if ( stream.is_open() )
00409                 status = print(stream, _name, _printScheme);
00410         else
00411                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00412 
00413         stream.close();
00414 
00415         return status;
00416 }
00417 
00418 template<typename T>
00419 returnValue GenericMatrix< T >::read( std::istream& _stream )
00420 {
00421         vector< vector< T > > tmp;
00422         _stream >> tmp;
00423 
00424         if (tmp.size() == 0)
00425         {
00426                 Base::_set(GenericMatrix< T >(0, 0));
00427                 return SUCCESSFUL_RETURN;
00428         }
00429 
00430         // Sanity check
00431         unsigned nc = tmp[ 0 ].size();
00432         unsigned nr = tmp.size();
00433         for (unsigned r = 0; r < nr; ++r)
00434                 if (tmp[ r ].size() != nc)
00435                         return ACADOERROR( RET_INVALID_ARGUMENTS );
00436 
00437         Base::_set(GenericMatrix< T >(nr, nc, tmp));
00438 
00439         return SUCCESSFUL_RETURN;
00440 }
00441 
00442 template<typename T>
00443 returnValue GenericMatrix< T >::read(const std::string& _filename)
00444 {
00445         ifstream stream( _filename.c_str() );
00446         returnValue status;
00447 
00448         if (stream.is_open())
00449                 status = read( stream );
00450         else
00451                 return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00452 
00453         stream.close();
00454 
00455         return status;
00456 }
00457 
00458 //
00459 // Explicit instantiation of templates.
00460 //
00461 template class GenericMatrix<double>;
00462 template class GenericMatrix<int>;
00463 template class GenericMatrix<bool>;
00464 
00465 CLOSE_NAMESPACE_ACADO


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