/* * This file is part of ACADO Toolkit. * * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization. * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau, * Milan Vukov, Rien Quirynen, KU Leuven. * Developed within the Optimization in Engineering Center (OPTEC) * under supervision of Moritz Diehl. All rights reserved. * * ACADO Toolkit is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public * License as published by the Free Software Foundation; either * version 3 of the License, or (at your option) any later version. * * ACADO Toolkit is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public * License along with ACADO Toolkit; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * */ /** * \file include/acado/matrix_vector/matrix.hpp * \author Hans Joachim Ferreau, Boris Houska, Milan Vukov * \date 2008 - 2013 * \note The code in the class GenericMatrix is not 100% compatible with the * original code developed by B. Houska and H.J. Ferreau. */ #ifndef ACADO_TOOLKIT_MATRIX_HPP #define ACADO_TOOLKIT_MATRIX_HPP #include #include BEGIN_NAMESPACE_ACADO /** A generic matrix class based on Eigen's matrix class. */ template class GenericMatrix : public Eigen::Matrix { public: /** \internal * \name Eigen compatibility layer * @{ */ /** Handy typedef for the base matrix class. */ typedef Eigen::Matrix Base; /** Constructor from any other Eigen::MatrixBase derived class. */ template inline GenericMatrix(const Eigen::MatrixBase& other) : Base( other ) {} /** Constructor from any other Eigen::ReturnByValue derived class. */ template inline GenericMatrix(const Eigen::ReturnByValue& other) : Base( other ) {} /** Constructor from any other Eigen::EigenBase derived class. */ template inline GenericMatrix(const Eigen::EigenBase& other) : Base( other ) {} /** @} * \endinternal */ /** \name Constructors. */ /** @{ */ /** Default ctor */ GenericMatrix() : Base() {} /** Ctor with scalar initializtion. */ GenericMatrix( const T& _value ) : Base(1, 1) { Base::data()[ 0 ] = _value; } /** Ctor that accepts matrix dimensions. */ GenericMatrix( unsigned _nRows, unsigned _nCols ) : Base(_nRows, _nCols) { Base::setZero(); } /** Ctor that accepts matrix dimensions and initialization data in C-like array. */ GenericMatrix( unsigned _nRows, unsigned _nCols, const T* const _values ) : Base(_nRows, _nCols) { std::copy(_values, _values + _nRows * _nCols, Base::data()); } /** Ctor that accepts matrix dimensions and initialization data in STL 1D vector. */ GenericMatrix( unsigned _nRows, unsigned _nCols, std::vector< T >& _values) : Base(_nRows, _nCols) { std::copy(_values.begin(), _values.end(), Base::data()); } /** Ctor that accepts matrix dimensions and initialization data in STL 2D array. */ GenericMatrix( unsigned _nRows, unsigned _nCols, std::vector< std::vector< T > >& _values ); /** @} */ /** Destructor. */ virtual ~GenericMatrix() {} /** Equality operator. */ bool operator==(const GenericMatrix& arg) const { if (Base::rows() != arg.rows() || Base::cols() != arg.cols()) return false; return Base::isApprox(arg, EQUALITY_EPS); } /** Inequality operator. */ bool operator!=(const GenericMatrix& arg) const { return (operator==( arg ) == false); } /** Initialization routine. */ void init( unsigned _nRows = 0, unsigned _nCols = 0 ) { Base::_set(GenericMatrix(_nRows, _nCols)); } /** Set all elements constant. */ void setAll( const T& _value) { Base::setConstant( _value ); } /** Appends rows at the end of the matrix. */ GenericMatrix& appendRows( const GenericMatrix& _arg ); /** Appends columns at the end of the matrix. */ GenericMatrix& appendCols( const GenericMatrix& _arg ); /** Computes the column-wise sum the DMatrix * * Example: * \code * a | b * c | d * \endcode * * returns [a+b;c+d] */ GenericVector< T > sumCol() const; /** Computes the row-wise sum the DMatrix * * Example: * * \code * a | b * c | d * \endcode * * returns [a+c|b+d] */ GenericVector< T > sumRow() const; /** Reshapes a matrix into a column vector. */ GenericMatrix& makeVector( ); /** Returns total number of elements of the matrix object. */ unsigned getDim( ) const { return (Base::rows() * Base::cols()); } /** Returns number of rows of the matrix object. */ unsigned getNumRows( ) const { return Base::rows(); } /** Returns number of columns of the matrix object. */ unsigned getNumCols( ) const { return Base::cols(); } /** Returns whether the vector is empty. */ bool isEmpty( ) const { return Base::rows() == 0 || Base::cols() == 0; } /** Returns a given row of the matrix object. */ GenericVector< T > getRow( unsigned _idx ) const { ASSERT( _idx < Base::rows() ); return Base::row( _idx ).transpose(); } /** Returns a given column of the matrix object. */ GenericVector< T > getCol( unsigned _idx ) const { ASSERT( _idx < Base::cols() ); return Base::col( _idx ); } /** Assigns new values to a given row of the matrix object. */ GenericMatrix& setRow( unsigned _idx, const GenericVector< T >& _values ) { ASSERT(Base::cols() == _values.rows() && _idx < Base::rows()); Base::row( _idx ) = _values.transpose(); return *this; } /** Assigns new values to a given column of the matrix object. */ GenericMatrix& setCol( unsigned _idx, const GenericVector< T >& _arg ) { ASSERT(Base::rows() == _arg.rows() && _idx < Base::cols()); Base::col( _idx ) = _arg; return *this; } /** Returns given rows of the matrix object. */ GenericMatrix getRows( unsigned _start, unsigned _end ) const { if (_start >= Base::rows() || _end >= Base::rows() || _start > _end) return GenericMatrix(); return Base::block(_start, 0, _end - _start + 1, Base::cols()); } /** Returns given columns of the matrix object. */ GenericMatrix getCols( unsigned _start, unsigned _end ) const { if (_start >= Base::cols() || _end >= Base::cols() || _start > _end) return GenericMatrix(); return Base::block(0, _start, Base::rows(), _end - _start + 1); } /** Returns a vector containing the diagonal elements of a square matrix. */ GenericVector< T > getDiag( ) const; /** Is the matrix square? */ bool isSquare() const; /** Tests if object is a symmetric matrix. */ bool isSymmetric( ) const; /** Make the matrix symmetric. */ returnValue symmetrize( ); /** Tests if object is a positive semi-definite matrix. * \note This test involves a Cholesky decomposition. */ bool isPositiveSemiDefinite( ) const; /** Tests if object is a (strictly) positive definite matrix. * \note This test involves a Cholesky decomposition. */ bool isPositiveDefinite( ) const; /** Returns the a matrix whose components are the absolute * values of the components of this object. */ GenericMatrix absolute() const; /** Returns the a matrix whose components are equal to the components of * this object, if they are positive or zero, but zero otherwise. */ GenericMatrix positive() const; /** Returns the a matrix whose components are equal to the components of * this object, if they are negative or zero, but zero otherwise. */ GenericMatrix negative() const; /** Returns maximum element. */ T getMax( ) const { return Base::maxCoeff(); } /** Returns minimum element. */ T getMin( ) const { return Base::minCoeff(); } /** Returns mean value of all elements. */ T getMean( ) const { return Base::mean(); } /** Return a new vector with absolute elements. */ GenericMatrix< T > getAbsolute() const { return Base::cwiseAbs(); } /** Returns Frobenius norm of the matrix. */ T getNorm( ) const; /** Returns trace of the matrix. */ T getTrace( ) const; /** Returns condition number of the square matrix based on SVD. * \note Works for square matrices, only. */ T getConditionNumber( ) const; /** Prints object to given file. Various settings can * be specified defining its output format. * * @param[in] _stream Output stream for printing. * @param[in] _name Name label to be printed before the numerical values. * @param[in] _startString Prefix before printing the numerical values. * @param[in] _endString Suffix after printing the numerical values. * @param[in] _width Total number of digits per single numerical value. * @param[in] _precision Number of decimals per single numerical value. * @param[in] _colSeparator Separator between the columns of the numerical values. * @param[in] _rowSeparator Separator between the rows of the numerical values. * * \return SUCCESSFUL_RETURN, \n * RET_FILE_CAN_NOT_BE_OPENED, \n * RET_UNKNOWN_BUG */ virtual returnValue print( std::ostream& _stream = std::cout, const std::string& _name = DEFAULT_LABEL, const std::string& _startString = DEFAULT_START_STRING, const std::string& _endString = DEFAULT_END_STRING, uint _width = DEFAULT_WIDTH, uint _precision = DEFAULT_PRECISION, const std::string& _colSeparator = DEFAULT_COL_SEPARATOR, const std::string& _rowSeparator = DEFAULT_ROW_SEPARATOR ) const; /** Prints object to given file. Various settings can * be specified defining its output format. * * @param[in] _stream Output stream for printing. * @param[in] _name Name label to be printed before the numerical values. * @param[in] _printScheme Print scheme defining the output format of the information. * * \return SUCCESSFUL_RETURN, \n * RET_FILE_CAN_NOT_BE_OPENED, \n * RET_UNKNOWN_BUG */ virtual returnValue print( std::ostream& stream, const std::string& name, PrintScheme printScheme ) const; /** Prints object to file with given name. Various settings can * be specified defining its output format. * * @param[in] _filename Filename for printing. * @param[in] _name Name label to be printed before the numerical values. * @param[in] _startString Prefix before printing the numerical values. * @param[in] _endString Suffix after printing the numerical values. * @param[in] _width Total number of digits per single numerical value. * @param[in] _precision Number of decimals per single numerical value. * @param[in] _colSeparator Separator between the columns of the numerical values. * @param[in] _rowSeparator Separator between the rows of the numerical values. * * \return SUCCESSFUL_RETURN, \n * RET_FILE_CAN_NOT_BE_OPENED, \n * RET_UNKNOWN_BUG */ virtual returnValue print( const std::string& _filename, const std::string& _name = DEFAULT_LABEL, const std::string& _startString = DEFAULT_START_STRING, const std::string& _endString = DEFAULT_END_STRING, uint _width = DEFAULT_WIDTH, uint _precision = DEFAULT_PRECISION, const std::string& _colSeparator = DEFAULT_COL_SEPARATOR, const std::string& _rowSeparator = DEFAULT_ROW_SEPARATOR ) const; /** Prints object to given file. Various settings can * be specified defining its output format. * * @param[in] _filename Filename for printing. * @param[in] _name Name label to be printed before the numerical values. * @param[in] _printScheme Print scheme defining the output format of the information. * * \return SUCCESSFUL_RETURN, \n * RET_FILE_CAN_NOT_BE_OPENED, \n * RET_UNKNOWN_BUG */ virtual returnValue print( const std::string& _filename, const std::string& _name, PrintScheme _printScheme ) const; /** Read matrix data from an input stream. */ virtual returnValue read( std::istream& _stream ); /** Read data from an input file. */ virtual returnValue read( const std::string& _filename ); }; /** Prints the matrix into a stream. */ template std::ostream& operator<<( std::ostream& _stream, const GenericMatrix< T >& _arg ) { if (_arg.print( _stream ) != SUCCESSFUL_RETURN) ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Cannot write to output stream."); return _stream; } /** Read a matrix from an input stream. */ template std::istream& operator>>( std::istream& _stream, GenericMatrix< T >& _arg ) { if (_arg.read( _stream ) != SUCCESSFUL_RETURN ) ACADOERRORTEXT(RET_INVALID_ARGUMENTS, "Cannot read from input stream."); return _stream; } /** Create a square matrix with all T( 1 ) elements. */ template GenericMatrix< T > ones( unsigned _nRows, unsigned _nCols = 1 ) { return GenericMatrix< T >(_nRows, _nCols).setOnes(); } /** Create a square matrix with all T( 0 ) elements. */ template GenericMatrix< T > zeros( unsigned _nRows, unsigned _nCols = 1 ) { return GenericMatrix< T >(_nRows, _nCols).setZero(); } /** Create an identity matrix. */ template GenericMatrix< T > eye( unsigned _dim ) { return GenericMatrix< T >(_dim, _dim).setIdentity(); } /** Type definition of the matrix of doubles. */ typedef GenericMatrix< double > DMatrix; /** Type definition of the matrix of integers. */ typedef GenericMatrix< int > IMatrix; /** Type definition of the matrix of booleans. */ typedef GenericMatrix< bool > BMatrix; /** Shared pointer to a matrix of doubles. */ typedef std::shared_ptr< GenericMatrix< double > > DMatrixPtr; static DMatrix emptyMatrix; static const DMatrix emptyConstMatrix; CLOSE_NAMESPACE_ACADO /** \internal */ namespace Eigen { namespace internal { template struct traits< ACADO::GenericMatrix< T > > : traits > {}; } // namespace internal } // namespace Eigen /** \endinternal */ #endif // ACADO_TOOLKIT_MATRIX_HPP