jyoung/phonelibs/acado/include/acado/matrix_vector/matrix.hpp

/*
 *    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 <memory>

#include <acado/matrix_vector/vector.hpp>

BEGIN_NAMESPACE_ACADO

/** A generic matrix class based on Eigen's matrix class. */
template<typename T>
class GenericMatrix : public Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign>
{
public:

	/** \internal
	 *  \name Eigen compatibility layer
	 *  @{
	 */

	/** Handy typedef for the base matrix class. */
	typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign> Base;


	/** Constructor from any other Eigen::MatrixBase derived class. */
	template<typename OtherDerived>
	inline GenericMatrix(const Eigen::MatrixBase<OtherDerived>& other) : Base( other ) {}

	/** Constructor from any other Eigen::ReturnByValue derived class. */
	template<typename OtherDerived>
	inline GenericMatrix(const Eigen::ReturnByValue<OtherDerived>& other) : Base( other ) {}

	/** Constructor from any other Eigen::EigenBase derived class. */
	template<typename OtherDerived>
	inline GenericMatrix(const Eigen::EigenBase<OtherDerived>& 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<T>(_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<typename T>
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<typename T>
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<typename T>
GenericMatrix< T > ones(	unsigned _nRows,
							unsigned _nCols = 1
							)
{ return GenericMatrix< T >(_nRows, _nCols).setOnes(); }

/** Create a square matrix with all T( 0 ) elements. */
template<typename T>
GenericMatrix< T > zeros(	unsigned _nRows,
							unsigned _nCols = 1
							)
{ return GenericMatrix< T >(_nRows, _nCols).setZero(); }

/** Create an identity matrix. */
template<typename T>
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<typename T>
struct traits< ACADO::GenericMatrix< T > >
	: traits<Matrix<T, Dynamic, Dynamic, Eigen::RowMajor | Eigen::AutoAlign> >
{};

} // namespace internal
} // namespace Eigen
/** \endinternal */

#endif  // ACADO_TOOLKIT_MATRIX_HPP