generic_matrix.hpp

/*
 *    This file is part of CasADi.
 *
 *    CasADi -- A symbolic framework for dynamic optimization.
 *    Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl,
 *                            KU Leuven. All rights reserved.
 *    Copyright (C) 2011-2014 Greg Horn
 *
 *    CasADi 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.
 *
 *    CasADi 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 CasADi; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */
 
 
#ifndef CASADI_GENERIC_MATRIX_HPP
#define CASADI_GENERIC_MATRIX_HPP
 
#include "slice.hpp"
#include "submatrix.hpp"
#include "nonzeros.hpp"
#include "sparsity.hpp"
#include "calculus.hpp"
#include "sparsity_interface.hpp"
#include "generic_type.hpp"
 
namespace casadi {
  struct CASADI_EXPORT GenericMatrixCommon {};
 
  template<typename MatType>
  class GenericMatrix
    : public GenericMatrixCommon,
      public SWIG_IF_ELSE(SparsityInterfaceCommon, SparsityInterface<MatType>) {
    using SparsityInterface<MatType>::self;
  public:
 
    casadi_int nnz() const;
 
    casadi_int nnz_lower() const;
 
    casadi_int nnz_upper() const;
 
    casadi_int nnz_diag() const;
 
    casadi_int numel() const;
 
    casadi_int size1() const;
 
    casadi_int rows() const {return size1();}
 
    casadi_int size2() const;
 
    casadi_int columns() const {return size2();}
 
    std::string dim(bool with_nz=false) const;
 
    std::pair<casadi_int, casadi_int> size() const;
 
    casadi_int size(casadi_int axis) const;
 
    bool is_empty(bool both=false) const { return sparsity().is_empty(both);}
 
    bool is_dense() const { return sparsity().is_dense();}
 
    bool is_scalar(bool scalar_and_dense=false) const;
 
    bool is_square() const { return sparsity().is_square();}
 
    bool is_vector() const { return sparsity().is_vector();}
 
    bool is_row() const { return sparsity().is_row();}
 
    bool is_column() const { return sparsity().is_column();}
 
    bool is_triu() const { return sparsity().is_triu();}
 
    bool is_tril() const { return sparsity().is_tril();}
 
 
    std::vector<casadi_int> get_row() const { return sparsity().get_row(); }
    std::vector<casadi_int> get_colind() const { return sparsity().get_colind(); }
#ifndef SWIG
    const casadi_int* row() const { return sparsity().row(); }
    const casadi_int* colind() const { return sparsity().colind(); }
#endif
    casadi_int row(casadi_int el) const { return sparsity().row(el); }
    casadi_int colind(casadi_int col) const { return sparsity().colind(col); }
 
    SWIG_CONSTREF(Sparsity) sparsity() const;
 
#ifndef SWIG
 
    static MatType interp1d(const std::vector<double>& x, const MatType &v,
         const std::vector<double>& xq, const std::string& mode, bool equidistant);
    static casadi_int sprank(const MatType &x) { return Sparsity::sprank(x.sparsity());}
    static casadi_int norm_0_mul(const MatType &x, const MatType &y) {
      return Sparsity::norm_0_mul(x.sparsity(), y.sparsity());
    }
    static MatType tril(const MatType &x, bool includeDiagonal=true) {
      return project(x, Sparsity::tril(x.sparsity(), includeDiagonal));
    }
    static MatType triu(const MatType &x, bool includeDiagonal=true) {
      return project(x, Sparsity::triu(x.sparsity(), includeDiagonal));
    }
    static MatType sumsqr(const MatType &x) { return dot(x, x);}
    static MatType linspace(const MatType &a, const MatType &b, casadi_int nsteps);
    static MatType cross(const MatType &a, const MatType &b, casadi_int dim=-1);
    static MatType skew(const MatType &a);
    static MatType inv_skew(const MatType &a);
    static MatType tril2symm(const MatType &x);
    static MatType triu2symm(const MatType &x);
    static MatType repsum(const MatType &x, casadi_int n, casadi_int m=1);
    static MatType diff(const MatType &x, casadi_int n=1, casadi_int axis=-1);
 
    static bool is_linear(const MatType &expr, const MatType &var);
    static bool is_quadratic(const MatType &expr, const MatType &var);
    static void quadratic_coeff(const MatType &expr, const MatType &var,
        MatType& A, MatType& b, MatType& c, bool check);
    static void linear_coeff(const MatType &expr, const MatType &var,
        MatType& A, MatType& b, bool check);
 
    template<typename K>
    const MatType nz(const K& k) const {
      MatType ret;
      self().get_nz(ret, false, k);
      return ret;
    }
 
    template<typename K>
    NonZeros<MatType, K> nz(const K& k) {
      return NonZeros<MatType, K>(self(), k);
    }
 
    template<typename RR>
    const MatType operator()(const RR& rr) const {
      MatType ret;
      self().get(ret, false, rr);
      return ret;
    }
 
    template<typename RR, typename CC>
    const MatType operator()(const RR& rr, const CC& cc) const {
      MatType ret;
      self().get(ret, false, rr, cc);
      return ret;
    }
 
    template<typename RR>
    SubIndex<MatType, RR> operator()(const RR& rr) {
      return SubIndex<MatType, RR>(self(), rr);
    }
 
    template<typename RR, typename CC>
    SubMatrix<MatType, RR, CC> operator()(const RR& rr, const CC& cc) {
      return SubMatrix<MatType, RR, CC>(self(), rr, cc);
    }
#endif // SWIG
 
#if !defined(SWIG) || defined(DOXYGEN)
     inline friend MatType interp1d(const std::vector<double>& x, const MatType&v,
         const std::vector<double>& xq, const std::string& mode, bool equidistant=false) {
       return MatType::interp1d(x, v, xq, mode, equidistant);
     }
 
    inline friend MatType mpower(const MatType& x, const MatType& n) {
      return MatType::mpower(x, n);
    }
 
    inline friend MatType soc(const MatType& x, const MatType& y) {
      return MatType::soc(x, y);
    }
 
    inline friend MatType
      einstein(const MatType &A, const MatType &B, const MatType &C,
        const std::vector<casadi_int>& dim_a, const std::vector<casadi_int>& dim_b,
        const std::vector<casadi_int>& dim_c,
        const std::vector<casadi_int>& a, const std::vector<casadi_int>& b,
        const std::vector<casadi_int>& c) {
      return MatType::einstein(A, B, C, dim_a, dim_b, dim_c, a, b, c);
    }
 
    inline friend MatType
      einstein(const MatType &A, const MatType &B,
        const std::vector<casadi_int>& dim_a, const std::vector<casadi_int>& dim_b,
        const std::vector<casadi_int>& dim_c,
        const std::vector<casadi_int>& a, const std::vector<casadi_int>& b,
        const std::vector<casadi_int>& c) {
      return MatType::einstein(A, B, dim_a, dim_b, dim_c, a, b, c);
    }
 
    inline friend MatType mrdivide(const MatType& x, const MatType& n) {
      return MatType::mrdivide(x, n);
    }
 
    inline friend MatType mldivide(const MatType& x, const MatType& n) {
      return MatType::mldivide(x, n);
    }
 
    inline friend std::vector<MatType> symvar(const MatType& x) {
      return MatType::symvar(x);
    }
 
 
    inline friend MatType bilin(const MatType &A, const MatType &x, const MatType &y) {
      return MatType::bilin(A, x, y);
    }
    inline friend MatType bilin(const MatType &A, const MatType &x) {
      return MatType::bilin(A, x, x);
    }
    static MatType bilin(const MatType& A, const MatType& x, const MatType& y);
 
 
    inline friend MatType rank1(const MatType &A, const MatType &alpha,
                                const MatType &x, const MatType &y) {
      return MatType::rank1(A, alpha, x, y);
    }
    static MatType rank1(const MatType& A, const MatType& alpha,
                         const MatType& x, const MatType& y);
 
    inline friend MatType sumsqr(const MatType &x) {
      return MatType::sumsqr(x);
    }
 
    inline friend MatType logsumexp(const MatType& x) {
      return MatType::logsumexp(x);
    }
    inline friend MatType logsumexp(const MatType& x, const MatType& margin) {
      MatType alpha = log(x.size1()) / margin;
      return MatType::logsumexp(alpha*x)/alpha;
    }
    static MatType logsumexp(const MatType& x);
 
    inline friend MatType linspace(const MatType &a, const MatType &b, casadi_int nsteps) {
      return MatType::linspace(a, b, nsteps);
    }
 
    inline friend MatType cross(const MatType &a, const MatType &b, casadi_int dim = -1) {
      return MatType::cross(a, b, dim);
    }
 
    inline friend MatType skew(const MatType &a) {
      return MatType::skew(a);
    }
 
    inline friend MatType inv_skew(const MatType &a) {
      return MatType::inv_skew(a);
    }
 
    inline friend MatType det(const MatType& A) { return MatType::det(A);}
 
    inline friend MatType inv_minor(const MatType& A) { return MatType::inv_minor(A);}
 
    inline friend MatType inv(const MatType& A) {
        return MatType::inv(A);
    }
 
    inline friend MatType inv(const MatType& A,
      const std::string& lsolver,
      const Dict& options=Dict()) {
        return MatType::inv(A, lsolver, options);
    }
 
    inline friend MatType trace(const MatType& x) { return MatType::trace(x);}
 
    inline friend MatType tril2symm(const MatType &a) { return MatType::tril2symm(a);}
 
    inline friend MatType triu2symm(const MatType &a) { return MatType::triu2symm(a);}
 
    inline friend MatType norm_fro(const MatType &x) { return MatType::norm_fro(x);}
 
    inline friend MatType norm_2(const MatType &x) { return MatType::norm_2(x);}
 
    inline friend MatType norm_1(const MatType &x) { return MatType::norm_1(x);}
 
    inline friend MatType norm_inf(const MatType &x) { return MatType::norm_inf(x);}
 
    inline friend MatType diff(const MatType &x, casadi_int n=1, casadi_int axis=-1) {
      return MatType::diff(x, n, axis);
    }
 
    inline friend MatType cumsum(const MatType &x, casadi_int axis=-1) {
      return MatType::cumsum(x, axis);
    }
 
    inline friend MatType dot(const MatType &x, const MatType &y) {
      return MatType::dot(x, y);
    }
 
    inline friend MatType nullspace(const MatType& A) {
      return MatType::nullspace(A);
    }
 
    inline friend MatType polyval(const MatType& p, const MatType& x) {
      return MatType::polyval(p, x);
    }
 
    inline friend MatType diag(const MatType &A) {
      return MatType::diag(A);
    }
 
    inline friend MatType unite(const MatType& A, const MatType& B) {
      return MatType::unite(A, B);
    }
 
    inline friend MatType densify(const MatType& x) {
      return MatType::densify(x);
    }
 
    inline friend MatType densify(const MatType& x, const MatType& val) {
      return MatType::densify(x, val);
    }
 
    inline friend MatType project(const MatType& A, const Sparsity& sp,
                                  bool intersect=false) {
      return MatType::project(A, sp, intersect);
    }
 
    inline friend MatType if_else(const MatType &cond, const MatType &if_true,
                                  const MatType &if_false, bool short_circuit=false) {
      return MatType::if_else(cond, if_true, if_false, short_circuit);
    }
 
    inline friend MatType conditional(const MatType& ind, const std::vector<MatType> &x,
                                      const MatType &x_default, bool short_circuit=false) {
      return MatType::conditional(ind, x, x_default, short_circuit);
    }
 
    inline friend bool depends_on(const MatType& f, const MatType &arg) {
      return MatType::depends_on(f, arg);
    }
 
    friend inline MatType substitute(const MatType& ex, const MatType& v,
                                     const MatType& vdef) {
      return MatType::substitute(ex, v, vdef);
    }
 
    friend inline std::vector<MatType>
      substitute(const std::vector<MatType>& ex, const std::vector<MatType>& v,
                 const std::vector<MatType>& vdef) {
      return MatType::substitute(ex, v, vdef);
    }
 
    inline friend void
      substitute_inplace(const std::vector<MatType>& v,
                        std::vector<MatType>& inout_vdef,
                        std::vector<MatType>& inout_ex, bool reverse=false) {
      return MatType::substitute_inplace(v, inout_vdef, inout_ex, reverse);
    }
 
    inline friend MatType cse(const MatType& e) {
      return MatType::cse({e}).at(0);
    }
 
 
    inline friend std::vector<MatType> cse(const std::vector<MatType>& e) {
      return MatType::cse(e);
    }
 
    friend inline MatType solve(const MatType& A, const MatType& b) {
      return MatType::solve(A, b);
    }
 
    friend inline MatType solve(const MatType& A, const MatType& b,
                                const std::string& lsolver,
                                const Dict& dict = Dict()) {
      return MatType::solve(A, b, lsolver, dict);
    }
 
    friend inline MatType linearize(const MatType& f, const MatType& x, const MatType& x0,
        const Dict& opts=Dict()) {
      return MatType::linearize(f, x, x0, opts);
    }
 
    friend inline MatType pinv(const MatType& A) {
      return MatType::pinv(A);
    }
 
    friend inline MatType pinv(const MatType& A, const std::string& lsolver,
                               const Dict& dict = Dict()) {
      return MatType::pinv(A, lsolver, dict);
    }
 
 
    friend inline MatType expm_const(const MatType& A, const MatType& t) {
      return MatType::expm_const(A, t);
    }
 
    friend inline MatType expm(const MatType& A) {
      return MatType::expm(A);
    }
 
    inline friend MatType jacobian(const MatType &ex, const MatType &arg,
                                   const Dict& opts = Dict()) {
      return MatType::jacobian(ex, arg, opts);
    }
    inline friend MatType gradient(const MatType &ex, const MatType &arg, const Dict& opts=Dict()) {
      return MatType::gradient(ex, arg, opts);
    }
    inline friend MatType tangent(const MatType &ex, const MatType &arg, const Dict& opts=Dict()) {
      return MatType::tangent(ex, arg, opts);
    }
 
    friend inline MatType jtimes(const MatType &ex, const MatType &arg,
                                 const MatType &v, bool tr=false, const Dict& opts=Dict()) {
      return MatType::jtimes(ex, arg, v, tr, opts);
    }
 
    friend inline std::vector<std::vector<MatType> >
    forward(const std::vector<MatType> &ex, const std::vector<MatType> &arg,
            const std::vector<std::vector<MatType> > &v,
            const Dict& opts = Dict()) {
      return MatType::forward(ex, arg, v, opts);
    }
 
    friend inline std::vector<std::vector<MatType> >
    reverse(const std::vector<MatType> &ex, const std::vector<MatType> &arg,
            const std::vector<std::vector<MatType> > &v,
            const Dict& opts = Dict()) {
      return MatType::reverse(ex, arg, v, opts);
    }
 
 
    inline friend MatType hessian(const MatType &ex, const MatType &arg,
        const Dict& opts = Dict()) {
      return MatType::hessian(ex, arg, opts);
    }
    inline friend MatType hessian(const MatType &ex, const MatType &arg, MatType& output_g,
        const Dict& opts = Dict()) {
      return MatType::hessian(ex, arg, output_g, opts);
    }
 
    inline friend std::vector<bool> which_depends(const MatType &expr, const MatType &var,
        casadi_int order, bool tr) {
      return MatType::which_depends(expr, var, order, tr);
    }
 
    inline friend Sparsity jacobian_sparsity(const MatType &f, const MatType &x) {
      return MatType::jacobian_sparsity(f, x);
    }
 
    inline friend bool is_linear(const MatType &expr, const MatType &var) {
      return MatType::is_linear(expr, var);
    }
 
    inline friend bool is_quadratic(const MatType &expr, const MatType &var) {
      return MatType::is_quadratic(expr, var);
    }
 
    inline friend void quadratic_coeff(const MatType &expr, const MatType &var,
        MatType& A, MatType& b, MatType& c, bool check=true) {
      MatType::quadratic_coeff(expr, var, A, b, c, check);
    }
 
    inline friend void linear_coeff(const MatType &expr, const MatType &var,
        MatType& A, MatType& b, bool check=true) {
      MatType::linear_coeff(expr, var, A, b, check);
    }
 
    inline friend casadi_int n_nodes(const MatType& A) {
      return MatType::n_nodes(A);
    }
 
    friend inline MatType simplify(const MatType &x) {
      return MatType::simplify(x);
    }
 
    inline friend std::string
      print_operator(const MatType& xb, const std::vector<std::string>& args) {
      return MatType::print_operator(xb, args);
    }
 
    inline friend void extract(std::vector<MatType>& ex,
        std::vector<MatType>& v,
        std::vector<MatType>& vdef,
        const Dict& opts = Dict()) {
      MatType::extract(ex, v, vdef, opts);
    }
 
    inline friend void shared(std::vector<MatType>& ex,
        std::vector<MatType>& v,
        std::vector<MatType>& vdef,
        const std::string& v_prefix="v_",
        const std::string& v_suffix="") {
      return MatType::shared(ex, v, vdef, v_prefix, v_suffix);
    }
 
    inline friend MatType repsum(const MatType &A, casadi_int n, casadi_int m=1) {
      return MatType::repsum(A, n, m);
    }
 
 
    friend inline MatType mmin(const MatType& x) {
      return MatType::mmin(x);
    }
 
 
    friend inline MatType mmax(const MatType& x) {
      return MatType::mmax(x);
    }
 
    static MatType jtimes(const MatType &ex, const MatType &arg,
                          const MatType &v, bool tr=false, const Dict& opts=Dict());
    static MatType gradient(const MatType &ex, const MatType &arg, const Dict& opts=Dict());
    static MatType tangent(const MatType &ex, const MatType &arg, const Dict& opts=Dict());
    static MatType linearize(const MatType& f, const MatType& x, const MatType& x0,
      const Dict& opts=Dict());
    static MatType mpower(const MatType &x, const MatType &y);
    static MatType soc(const MatType &x, const MatType &y);
 
#endif // SWIG
 
 
    static MatType sym(const std::string& name, casadi_int nrow=1, casadi_int ncol=1) {
      return sym(name, Sparsity::dense(nrow, ncol));
    }
 
    static MatType sym(const std::string& name, const std::pair<casadi_int, casadi_int> &rc) {
      return sym(name, rc.first, rc.second);
    }
 
    static MatType sym(const std::string& name, const Sparsity& sp) {
      return MatType::_sym(name, sp);
    }
 
    static std::vector<MatType > sym(const std::string& name, const Sparsity& sp, casadi_int p);
 
    static std::vector<MatType > sym(const std::string& name, casadi_int nrow,
        casadi_int ncol, casadi_int p) {
      return sym(name, Sparsity::dense(nrow, ncol), p);
    }
 
    static std::vector<std::vector<MatType> >
      sym(const std::string& name, const Sparsity& sp, casadi_int p, casadi_int r);
 
    static std::vector<std::vector<MatType> >
      sym(const std::string& name, casadi_int nrow, casadi_int ncol, casadi_int p, casadi_int r) {
      return sym(name, Sparsity::dense(nrow, ncol), p, r);
    }
 
 
    static MatType zeros(casadi_int nrow=1, casadi_int ncol=1) {
      return zeros(Sparsity::dense(nrow, ncol));
    }
    static MatType zeros(const Sparsity& sp) { return MatType(sp, 0, false);}
    static MatType zeros(const std::pair<casadi_int, casadi_int>& rc) {
      return zeros(rc.first, rc.second);
    }
 
 
    static MatType ones(casadi_int nrow=1, casadi_int ncol=1) {
      return ones(Sparsity::dense(nrow, ncol));
    }
    static MatType ones(const Sparsity& sp) { return MatType(sp, 1, false);}
    static MatType ones(const std::pair<casadi_int, casadi_int>& rc) {
      return ones(rc.first, rc.second);
    }
  };
 
  // Throw informative error message
  #define CASADI_THROW_ERROR(FNAME, WHAT) \
  throw CasadiException("Error in " + MatType::type_name() \
    + "::" FNAME " at " + CASADI_WHERE + ":\n" + std::string(WHAT));
 
#ifndef SWIG
  // Implementations
  template<typename MatType>
  const Sparsity& GenericMatrix<MatType>::sparsity() const {
    return self().sparsity();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::nnz() const {
    return sparsity().nnz();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::nnz_lower() const {
    return sparsity().nnz_lower();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::nnz_upper() const {
    return sparsity().nnz_upper();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::nnz_diag() const {
    return sparsity().nnz_diag();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::numel() const {
    return sparsity().numel();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::size1() const {
    return sparsity().size1();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::size2() const {
    return sparsity().size2();
  }
 
  template<typename MatType>
  std::pair<casadi_int, casadi_int> GenericMatrix<MatType>::size() const {
    return sparsity().size();
  }
 
  template<typename MatType>
  casadi_int GenericMatrix<MatType>::size(casadi_int axis) const {
    return sparsity().size(axis);
  }
 
  template<typename MatType>
  std::string GenericMatrix<MatType>::dim(bool with_nz) const {
    return sparsity().dim(with_nz);
  }
 
  template<typename MatType>
  bool GenericMatrix<MatType>::is_scalar(bool scalar_and_dense) const {
    return sparsity().is_scalar(scalar_and_dense);
  }
 
#endif // SWIG
 
  template<typename MatType>
  std::vector<MatType> GenericMatrix<MatType>::sym(const std::string& name,
                                                   const Sparsity& sp, casadi_int p) {
    std::vector<MatType> ret(p);
    std::stringstream ss;
    for (casadi_int k=0; k<p; ++k) {
      ss.str("");
      ss << name << k;
      ret[k] = sym(ss.str(), sp);
    }
    return ret;
  }
 
  template<typename MatType>
  std::vector<std::vector<MatType> > GenericMatrix<MatType>::sym(const std::string& name,
                                                                  const Sparsity& sp, casadi_int p,
                                                                  casadi_int r) {
    std::vector<std::vector<MatType> > ret(r);
    for (casadi_int k=0; k<r; ++k) {
      std::stringstream ss;
      ss << name << "_" << k;
      ret[k] = sym(ss.str(), sp, p);
    }
    return ret;
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::linspace(const MatType& a, const MatType& b, casadi_int nsteps) {
    std::vector<MatType> ret(nsteps);
    ret[0] = a;
    MatType step = (b-a)/static_cast<MatType>(nsteps-1);
 
    for (casadi_int i=1; i<nsteps-1; ++i)
      ret[i] = a + i * step;
 
    ret[nsteps-1] = b;
    return vertcat(ret);
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::cross(const MatType& a, const MatType& b, casadi_int dim) {
    casadi_assert(a.size1()==b.size1() && a.size2()==b.size2(),
      "cross(a, b): Inconsistent dimensions. Dimension of a ("
      + a.dim() + " ) must equal that of b (" + b.dim() + ").");
 
    casadi_assert(a.size1()==3 || a.size2()==3,
      "cross(a, b): One of the dimensions of a should have length 3, but got "
        + a.dim() + ".");
    casadi_assert(dim==-1 || dim==1 || dim==2,
      "cross(a, b, dim): Dim must be 1, 2 or -1 (automatic).");
 
    std::vector<MatType> ret(3);
 
    bool t = a.size1()==3;
 
    if (dim==1) t = true;
    if (dim==2) t = false;
 
    MatType a1 = t ? a(0, Slice()) : a(Slice(), 0);
    MatType a2 = t ? a(1, Slice()) : a(Slice(), 1);
    MatType a3 = t ? a(2, Slice()) : a(Slice(), 2);
 
    MatType b1 = t ? b(0, Slice()) : b(Slice(), 0);
    MatType b2 = t ? b(1, Slice()) : b(Slice(), 1);
    MatType b3 = t ? b(2, Slice()) : b(Slice(), 2);
 
    ret[0] = a2*b3-a3*b2;
    ret[1] = a3*b1-a1*b3;
    ret[2] = a1*b2-a2*b1;
 
    return t ? vertcat(ret) : horzcat(ret);
  }
 
  double CASADI_EXPORT index_interp1d(const std::vector<double>& x, double xq,
    bool equidistant=false);
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::interp1d(const std::vector<double>& x, const MatType& v,
      const std::vector<double>& xq, const std::string& mode, bool equidistant) {
 
    bool mode_floor = mode == "floor";
    bool mode_ceil = mode == "ceil";
 
    casadi_assert_dev(is_increasing(x));
 
    casadi_assert(x.size()==v.size1(),
      "interp1d(x, v, xq): dimensions mismatch. v expected to have " + str(x.size()) + " rows,"
      " but got " + str(v.size1()) + " instead.");
 
    // Need at least two elements
    casadi_assert(x.size()>=2, "interp1d(x, v, xq): x must be at least length 2.");
 
    // Vectors to compose a sparse matrix
    std::vector<double> val;
    std::vector<casadi_int> colind(1, 0);
    std::vector<casadi_int> row;
 
    // Number of nonzeros in to-be composed matrix
    casadi_int nnz = 0;
    for (casadi_int i=0;i<xq.size();++i) {
      // Obtain index corresponding to xq[i]
      double ind = index_interp1d(x, xq[i], equidistant);
 
      if (mode_floor) ind = floor(ind);
      if (mode_ceil) ind = ceil(ind);
 
      // Split into integer and fractional part
      double int_partd;
      double frac_part = modf(ind, &int_partd);
      casadi_int int_part = static_cast<casadi_int>(int_partd);
 
      if (frac_part==0) {
         // Create a single entry
         val.push_back(1);
         row.push_back(int_part);
         nnz+=1;
         colind.push_back(nnz);
       } else {
         // Create a double entry
         val.push_back(1-frac_part);
         val.push_back(frac_part);
         row.push_back(int_part);
         row.push_back(int_part+1);
         nnz+=2;
         colind.push_back(nnz);
       }
     }
 
     // Construct sparsity for composed matrix
     Sparsity sp(x.size(), xq.size() , colind, row);
 
     return MatType::mtimes(MatType(sp, val).T(), v);
 
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::skew(const MatType& a) {
    casadi_assert(a.is_vector() && (a.size1()==3 || a.size2()==3),
      "skew(a): Expecting 3-vector, got " + a.dim() + ".");
 
    MatType x = a(0);
    MatType y = a(1);
    MatType z = a(2);
    return blockcat(std::vector< std::vector<MatType> >({{0, -z, y}, {z, 0, -x}, {-y, x, 0}}));
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::inv_skew(const MatType& a) {
    casadi_assert(a.size1()==3 && a.size2()==3,
      "inv_skew(a): Expecting 3-by-3 matrix, got " + a.dim() + ".");
 
    return 0.5*vertcat(std::vector<MatType>({a(2, 1)-a(1, 2), a(0, 2)-a(2, 0), a(1, 0)-a(0, 1)}));
  }
 
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::tril2symm(const MatType& x) {
    casadi_assert(x.is_square(),
      "Shape error in tril2symm. Expecting square shape but got " + x.dim());
    casadi_assert(x.nnz_upper()-x.nnz_diag()==0,
      "Sparsity error in tril2symm. Found above-diagonal entries in argument: " + x.dim());
    return x +  x.T() - diag(diag(x));
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::repsum(const MatType& x, casadi_int n, casadi_int m) {
    casadi_assert_dev(x.size1() % n==0);
    casadi_assert_dev(x.size2() % m==0);
    std::vector< std::vector< MatType> > s =
      blocksplit(x, x.size1()/n, x.size2()/m);
    MatType sum = 0;
    for (casadi_int i=0;i<s.size();++i) {
      for (casadi_int j=0;j<s[i].size();++j) {
        sum = sum + s[i][j];
      }
    }
    return sum;
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::triu2symm(const MatType& x) {
    casadi_assert(x.is_square(),
      "Shape error in triu2symm. Expecting square shape but got " + x.dim());
    casadi_assert(x.nnz_lower()-x.nnz_diag()==0,
      "Sparsity error in triu2symm. Found below-diagonal entries in argument: " + x.dim());
    return x + x.T() - diag(diag(x));
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::bilin(const MatType& A, const MatType& x,
                                        const MatType& y) {
    // Check/correct x
    casadi_assert_dev(x.is_vector());
    if (!x.is_column()) return bilin(A, x.T(), y);
    if (!x.is_dense()) return bilin(A, densify(x), y);
 
    // Check/correct y
    casadi_assert_dev(y.is_vector());
    if (!y.is_column()) return bilin(A, x, y.T());
    if (!y.is_dense()) return bilin(A, x, densify(y));
 
    // Assert dimensions
    casadi_assert(x.size1()==A.size1() && y.size1()==A.size2(),
      "Dimension mismatch. Got x.size1() = " + str(x.size1())
      + " and y.size1() = " + str(y.size1()) + " but A.size() = " + str(A.size()));
 
    // Call the class specific method
    return MatType::_bilin(A, x, y);
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::rank1(const MatType& A, const MatType& alpha,
                                        const MatType& x, const MatType& y) {
    // Check/correct x
    casadi_assert_dev(x.is_vector());
    if (!x.is_column()) return rank1(A, alpha, x.T(), y);
    if (!x.is_dense()) return rank1(A, alpha, densify(x), y);
 
    // Check/correct y
    casadi_assert_dev(y.is_vector());
    if (!y.is_column()) return rank1(A, alpha, x, y.T());
    if (!y.is_dense()) return rank1(A, alpha, x, densify(y));
 
    // Check alpha, quick return
    casadi_assert_dev(alpha.is_scalar());
    if (!alpha.is_dense()) return A;
 
    // Assert dimensions
    casadi_assert(x.size1()==A.size1() && y.size1()==A.size2(),
      "Dimension mismatch. Got x.size1() = " + str(x.size1())
        + " and y.size1() = " + str(y.size1())
        + " but A.size() = " + str(A.size()));
 
    // Call the class specific method
    return MatType::_rank1(A, alpha, x, y);
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::logsumexp(const MatType& x) {
    casadi_assert(x.is_dense(), "Argument must be dense");
    casadi_assert(x.is_column(), "Argument must be column vector");
    // Call the class specific method
    return MatType::_logsumexp(x);
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::jtimes(const MatType &ex, const MatType &arg,
                                         const MatType &v, bool tr, const Dict& opts) {
    try {
      // Assert consistent input dimensions
      if (tr) {
        casadi_assert(v.size1() == ex.size1() && v.size2() % ex.size2() == 0,
                      "'v' has inconsistent dimensions: "
                      " v " + v.dim(false) + ", ex " + ex.dim(false) + ".");
      } else {
        casadi_assert(v.size1() == arg.size1() && v.size2() % arg.size2() == 0,
                      "'v' has inconsistent dimensions: "
                      " v " + v.dim(false) + ", arg " + arg.dim(false) + ".");
      }
 
      // Quick return if no seeds
      if (v.is_empty()) return MatType(tr ? arg.size1() : ex.size1(), 0);
 
      // Split up the seed into its components
      std::vector<MatType> w = horzsplit(v, tr ? ex.size2() : arg.size2());
 
      // Seeds as a vector of vectors
      std::vector<std::vector<MatType> > ww(w.size());
      for (casadi_int i=0; i<w.size(); ++i) ww[i] = {w[i]};
 
      // Calculate directional derivatives
      if (tr) {
        ww = reverse({ex}, {arg}, ww, opts);
      } else {
        ww = forward({ex}, {arg}, ww, opts);
      }
 
      // Get results
      for (casadi_int i=0; i<w.size(); ++i) w[i] = ww[i][0];
      return horzcat(w);
    } catch (std::exception& e) {
      CASADI_THROW_ERROR("jtimes", e.what());
    }
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::gradient(const MatType &ex, const MatType &arg,
      const Dict& opts) {
    try {
      casadi_assert(ex.is_scalar(),
                    "'gradient' only defined for scalar outputs: Use 'jacobian' instead.");
      return project(jtimes(ex, arg, MatType::ones(ex.sparsity()), true, opts), arg.sparsity());
    } catch (std::exception& e) {
      CASADI_THROW_ERROR("gradient", e.what());
    }
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::tangent(const MatType &ex, const MatType &arg,
      const Dict& opts) {
    try {
      casadi_assert(arg.is_scalar(),
                    "'tangent' only defined for scalar inputs: Use 'jacobian' instead.");
      return project(jtimes(ex, arg, MatType::ones(arg.sparsity()), false, opts), ex.sparsity());
    } catch (std::exception& e) {
      CASADI_THROW_ERROR("tangent", e.what());
    }
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::
  linearize(const MatType& f, const MatType& x, const MatType& x0, const Dict& opts) {
    MatType x_lin = MatType::sym("x_lin", x.sparsity());
    // mismatching dimensions
    if (x0.size() != x.size()) {
      // Scalar x0 is ok
      if (x0.sparsity().is_scalar()) {
        return linearize(f, x, MatType(x.sparsity(), x0));
      }
      casadi_error("Dimension mismatch in 'linearize'");
    }
    return substitute(f + jtimes(f, x, x_lin, false, opts),
      MatType::vertcat({x_lin, x}), MatType::vertcat({x, x0}));
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::mpower(const MatType& a,
                                            const MatType& b) {
    if (a.is_scalar() && b.is_scalar()) return pow(a, b);
    casadi_assert(a.is_square() && b.is_constant() && b.is_scalar(),
      "Not Implemented");
    double bv = static_cast<double>(b);
    casadi_int N = static_cast<casadi_int>(bv);
    casadi_assert(bv-static_cast<double>(N)==0, "mpower only defined for integer powers.");
    casadi_assert(bv==N, "Not Implemented");
    if (N<0) return inv(mpower(a, -N));
    if (N==0) return MatType::eye(a.size1());
    if (N==1) return a;
    if (N % 2 == 0) {
      MatType h = mpower(a, N/2); // NOLINT
      return MatType::mtimes(h, h);
    } else {
      return MatType::mtimes(mpower(a, N-1), a);
    }
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::soc(const MatType& x,
                                            const MatType& y) {
    casadi_assert(y.is_scalar(), "y needs to be scalar. Got " + y.dim() + ".");
    casadi_assert(x.is_vector(), "x needs to be a vector. Got " + x.dim() + ".");
 
    MatType x_col = x.is_column() ? x : x.T();
 
    x_col = x_col.nz(Slice()); // NOLINT
 
    casadi_int n = x_col.numel();
    return blockcat(y*MatType::eye(n), x_col, x_col.T(), y);
  }
 
  template<typename MatType>
  bool GenericMatrix<MatType>::is_linear(const MatType &expr, const MatType &var) {
    return !any(MatType::which_depends(expr, var, 2, true));
  }
 
  template<typename MatType>
  bool GenericMatrix<MatType>::is_quadratic(const MatType &expr, const MatType &var) {
    return is_linear(gradient(expr, var), var);
  }
 
  template<typename MatType>
  void GenericMatrix<MatType>::quadratic_coeff(const MatType &expr, const MatType &var,
          MatType& A, MatType& b, MatType& c, bool check) {
    casadi_assert(expr.is_scalar(), "'quadratic_coeff' only defined for scalar expressions.");
    A = hessian(expr, var);
    b = substitute(jacobian(expr, var), var, 0).T();
    if (check)
      casadi_assert(!depends_on(A, var), "'quadratic_coeff' called on non-quadratic expression.");
    c = substitute(expr, var, 0);
  }
 
  template<typename MatType>
  void GenericMatrix<MatType>::linear_coeff(const MatType &expr, const MatType &var,
          MatType& A, MatType& b, bool check) {
    casadi_assert(expr.is_vector(), "'linear_coeff' only defined for vector expressions.");
    if (check)
      casadi_assert(is_linear(expr, var), "'linear_coeff' called on non-linear expression.");
    A = substitute(jacobian(expr, var), var, 0);
    b = vec(substitute(expr, var, 0));
  }
 
  template<typename MatType>
  MatType GenericMatrix<MatType>::diff(const MatType& x, casadi_int n, casadi_int axis) {
    casadi_assert(axis==-1 || axis==0 || axis==1, "Axis argument invalid");
    casadi_assert(n>=1, "n argument invalid");
 
    MatType ret = x;
    for (casadi_int i=0;i<n;++i) {
      // Matlab's special case
      if (axis==-1 && ret.is_scalar()) return MatType();
 
      casadi_int local_axis = (axis==-1) ? ret.is_row() : axis;
      if (local_axis==0) {
        if (ret.size1()<=1) {
          ret = MatType::zeros(0, ret.size2());
        } else {
          ret = ret(Slice(1, ret.size1()), Slice())-ret(Slice(0, ret.size1()-1), Slice());
        }
      } else {
        if (ret.size2()<=1) {
          ret = MatType::zeros(ret.size1(), 0);
        } else {
          ret = ret(Slice(), Slice(1, ret.size2()))-ret(Slice(), Slice(0, ret.size2()-1));
        }
      }
    }
    return ret;
  }
 
#undef CASADI_THROW_ERROR
 
} // namespace casadi
 
#endif // CASADI_GENERIC_MATRIX_HPP