root/library/bdm/math/functions.h

//
// C++ Interface: itpp_ext
//
// Description:
//
//
// Author: smidl <smidl@utia.cas.cz>, (C) 2008
//
// Copyright: See COPYING file that comes with this distribution
//
//
#ifndef FN_H
#define FN_H

#include "../bdmerror.h"
#include "../base/bdmbase.h"

namespace bdm {

//! class representing function \f$f(x) = a\f$, here \c rv is empty
class constfn : public fnc {
    //! value of the function
    vec val;

public:
    //vec eval() {return val;};
    //! inherited
    vec eval ( const vec &cond ) {
        return val;
    };
    //!Default constructor
    constfn ( const vec &val0 ) : fnc(), val ( val0 ) {
        dimy = val.length();
        dimc = 0;
    };
};

//! Class representing function \f$f(x) = Ax+B\f$
class linfn: public fnc {
  public:
    //! Identification of \f$x\f$
    RV rv;
    //! Matrix A
    mat A;
    //! vector B
    vec B;
public :
    vec eval ( const vec &cond ) {
        bdm_assert_debug ( cond.length() == A.cols(), "linfn::eval Wrong cond." );
        return A * cond + B;
    }

//              linfn evalsome ( ivec &rvind );
    //!default constructor
    linfn ( ) : fnc(), A ( ), B () { };
    linfn ( const mat &A0, const vec &B0 ) : fnc(), A (A0 ), B (B0) { };
    //! Set values of \c A and \c B
    void set_parameters ( const mat &A0 , const vec &B0 ) {
        A = A0;
        B = B0;
    };
    void from_setting(const Setting &set) {
        UI::get(A,set,"A",UI::compulsory);
        UI::get(B,set,"B",UI::compulsory);
    }
    void validate() {
        dimy = A.rows();
        dimc = A.cols();
    }

};
UIREGISTER(linfn);


//! Class representing function \f$ f(x) = [x' 1] Q [x' 1]' \f$ with Q in choleski decomposition
class quadraticfn: public fnc {
public :
    //! Identification of \f$x\f$
    RV rv;
    //! Matrix Q
    chmat Q;

        //!default constructor
    quadraticfn ( ) : fnc(), Q ( ) { };
    quadraticfn ( const mat &Q0) : fnc(), Q (Q0 ) { };
    
    vec eval ( const vec &cond ) {
        bdm_assert_debug ( cond.length() == Q.cols(), "quadraticfc::eval Wrong cond." );
        return vec_1( Q.qform(concat(cond,1.0)) );
    }

    void from_setting(const Setting &set) {
        //UI::get(Q,set,"Q",UI::compulsory);
    }
    void validate() {
        dimy = 1;
        dimc = Q.cols()-1;
    }

};
UIREGISTER(quadraticfn);

/*!
\brief Class representing a differentiable function of two variables \f$f(x,u)\f$.

Function of two variables.

TODO:
1) Technically, it could have a common parent (e.g. \c fnc ) with other functions. For now, we keep it as it is.
2) It could be generalized into multivariate form, (which was original meaning of \c fnc ).
*/
class diffbifn: public fnc {
protected:
    //! Indentifier of the first rv.
    RV rvx;
    //! Indentifier of the second rv.
    RV rvu;
    //! cache for rvx.count()
    int dimx;
    //! cache for rvu.count()
    int dimu;
public:
    //! Evaluates \f$f(x0,u0)\f$ (VS: Do we really need common eval? )
    vec eval ( const vec &cond ) {
        bdm_assert_debug ( cond.length() == ( dimx + dimu ), "linfn::eval Wrong cond." );
        if ( dimu > 0 ) {
            return eval ( cond ( 0, dimx - 1 ), cond ( dimx, dimx + dimu - 1 ) );//-1 = end (in matlab)
        } else {
            return eval ( cond ( 0, dimx - 1 ), vec ( 0 ) );//-1 = end (in matlab)
        }

    }

    //! Evaluates \f$f(x0,u0)\f$
    virtual vec eval ( const vec &x0, const vec &u0 ) {
        return zeros ( dimy );
    };
    //! Evaluates \f$A=\frac{d}{dx}f(x,u)|_{x0,u0}\f$ and writes result into \c A . @param full denotes that even unchanged entries are to be rewritten. When, false only the changed elements are computed. @param x0 numeric value of \f$x\f$, @param u0 numeric value of \f$u\f$ @param A a place where the result will be stored.
    virtual void dfdx_cond ( const vec &x0, const vec &u0, mat &A , bool full = true ) {};
    //! Evaluates \f$A=\frac{d}{du}f(x,u)|_{x0,u0}\f$ and writes result into \c A . @param full denotes that even unchanged entries are to be rewritten. When, false only the changed elements are computed.    @param x0 numeric value of \f$x\f$, @param u0 numeric value of \f$u\f$ @param A a place where the result will be stored.
    virtual void dfdu_cond ( const vec &x0, const vec &u0, mat &A, bool full = true ) {};
    //!Default constructor (dimy is not set!)
    diffbifn () : fnc() {};
    //! access function
    int _dimx() const {
        return dimx;
    }
    //! access function
    int _dimu() const {
        return dimu;
    }
    void validate(){dimc=dimx+dimu;};
};

//! Class representing function \f$f(x,u) = Ax+Bu\f$
//TODO can be generalized into multilinear form!
class bilinfn: public diffbifn {
    mat A;
    mat B;
public :
    //!\name Constructors
    //!@{

    bilinfn () : diffbifn (), A(), B() { }

    bilinfn ( const mat &A0, const mat &B0 ) {
        set_parameters ( A0, B0 );
    }

    //! Alternative initialization
    void set_parameters ( const mat &A0, const mat &B0 ) {
        bdm_assert ( A0.rows() == B0.rows(), "bilinfn matrices must have the same number of rows" );
        A = A0;
        B = B0;
        dimy = A.rows();
        dimx = A.cols();
        dimu = B.cols();
    }
    //!@}

    //!\name Mathematical operations
    //!@{
    inline vec eval ( const  vec &x0, const vec &u0 ) {
        bdm_assert_debug ( x0.length() == dimx, "bilinfn::eval Wrong xcond." );
        bdm_assert_debug ( u0.length() == dimu, "bilinfn::eval Wrong ucond." );
        return dimu>0 ?  A*x0 + B*u0 : A*x0;
    }

    void dfdx_cond ( const vec &x0, const vec &u0, mat &F, bool full ) {
        bdm_assert_debug ( ( F.cols() == A.cols() ) && ( F.rows() == A.rows() ), "Allocated F is not compatible." );
        if ( full ) F = A;      //else : nothing has changed no need to regenerate
    }

    void dfdu_cond ( const vec &x0, const vec &u0, mat &F,  bool full = true ) {
        bdm_assert_debug ( ( F.cols() == B.cols() ) && ( F.rows() == B.rows() ), "Allocated F is not compatible." );
        if ( full ) F = B;      //else : nothing has changed no need to regenerate
    }
    void from_setting(const Setting &set) {
                diffbifn::from_setting(set);
                UI::get(A,set,"A",UI::compulsory);
                UI::get(B,set,"B",UI::compulsory);
        }
        void validate() {
                dimy = A.rows();
                dimx = A.cols();
                dimu = B.cols();
                dimc =dimx+dimu;
        }
        //!@}
};
UIREGISTER(bilinfn);

} //namespace
#endif // FN_H