sparsity.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
 *    Copyright (C) 2005-2013 Timothy A. Davis
 *
 *    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_SPARSITY_HPP
#define CASADI_SPARSITY_HPP
 
#include "shared_object.hpp"
#include "printable.hpp"
#include "casadi_common.hpp"
#include "sparsity_interface.hpp"
#include "generic_type.hpp"
#include <vector>
#include <list>
#include <limits>
#include <unordered_map>
#ifdef CASADI_WITH_THREAD
#ifdef CASADI_WITH_THREAD_MINGW
#include <mingw.mutex.h>
#else // CASADI_WITH_THREAD_MINGW
#include <mutex>
#endif // CASADI_WITH_THREAD_MINGW
#endif //CASADI_WITH_THREAD
 
namespace casadi {
  // Forward declaration
  class SparsityInternal;
  class SerializingStream;
  class DeserializingStream;
 
  #ifndef SWIG
    struct CASADI_EXPORT SparsityStruct {
      casadi_int nrow;
      casadi_int ncol;
      const casadi_int* colind;
      const casadi_int* row;
    };
  #endif // SWIG
 
  class CASADI_EXPORT Sparsity
    : public SharedObject,
      public SWIG_IF_ELSE(SparsityInterfaceCommon, SparsityInterface<Sparsity>),
      public SWIG_IF_ELSE(PrintableCommon, Printable<Sparsity>) {
  public:
 
    explicit Sparsity(casadi_int dummy=0);
 
    Sparsity(casadi_int nrow, casadi_int ncol);
 
    Sparsity(casadi_int nrow, casadi_int ncol,
             const std::vector<casadi_int>& colind, const std::vector<casadi_int>& row,
             bool order_rows=false);
 
    explicit Sparsity(const std::pair<casadi_int, casadi_int>& rc);
 
#ifndef SWIG
    Sparsity(casadi_int nrow, casadi_int ncol, const casadi_int* colind, const casadi_int* row,
            bool order_rows=false);
 
    static Sparsity create(SparsityInternal *node);
 
    typedef SparsityInterface<Sparsity> B;
 
    using B::horzsplit;
    using B::diagsplit;
    using B::vertsplit;
    using B::mtimes;
 
    SparsityInternal* get() const;
#endif
 
    static Sparsity scalar(bool dense_scalar=true)
    { return dense_scalar ? dense(1, 1) : Sparsity(1, 1); }
 
    static Sparsity dense(casadi_int nrow, casadi_int ncol=1);
    static Sparsity dense(const std::pair<casadi_int, casadi_int> &rc) {
      return dense(rc.first, rc.second);
    }
 
    static Sparsity unit(casadi_int n, casadi_int el);
 
    static Sparsity upper(casadi_int n);
 
    static Sparsity lower(casadi_int n);
 
    static Sparsity diag(casadi_int nrow) { return diag(nrow, nrow);}
    static Sparsity diag(casadi_int nrow, casadi_int ncol);
    static Sparsity diag(const std::pair<casadi_int, casadi_int> &rc) {
      return diag(rc.first, rc.second);
    }
 
    static Sparsity band(casadi_int n, casadi_int p);
 
    static Sparsity banded(casadi_int n, casadi_int p);
 
    static Sparsity rowcol(const std::vector<casadi_int>& row,
                           const std::vector<casadi_int>& col,
                           casadi_int nrow, casadi_int ncol);
 
 
    static Sparsity triplet(casadi_int nrow, casadi_int ncol,
                            const std::vector<casadi_int>& row, const std::vector<casadi_int>& col,
                            std::vector<casadi_int>& SWIG_OUTPUT(mapping), bool invert_mapping);
    static Sparsity triplet(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& row,
                            const std::vector<casadi_int>& col);
 
    static Sparsity nonzeros(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& nz,
                              bool ind1=SWIG_IND1);
 
    static Sparsity compressed(const std::vector<casadi_int>& v, bool order_rows=false);
#ifndef SWIG
    static Sparsity compressed(const casadi_int* v, bool order_rows=false);
#endif // SWIG
 
    static Sparsity permutation(const std::vector<casadi_int>& p, bool invert=false);
 
    const std::vector<casadi_int> permutation_vector(bool invert=false) const;
 
    Sparsity get_diag(std::vector<casadi_int>& SWIG_OUTPUT(mapping)) const;
 
    std::vector<casadi_int> compress(bool canonical=true) const;
 
#ifndef SWIG
    const SparsityInternal* operator->() const;
 
    const SparsityInternal& operator*() const;
#endif // SWIG
    bool is_equal(const Sparsity& y) const;
    bool is_equal(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& colind,
                 const std::vector<casadi_int>& row) const;
#ifndef SWIG
    bool is_equal(casadi_int nrow, casadi_int ncol,
                  const casadi_int* colind, const casadi_int* row) const;
#endif // SWIG
 
    bool operator==(const Sparsity& y) const { return is_equal(y);}
 
    bool operator!=(const Sparsity& y) const {return !is_equal(y);}
 
    bool is_stacked(const Sparsity& y, casadi_int n) const;
 
#ifndef SWIG
    operator const casadi_int*() const;
 
    operator const std::vector<casadi_int>&() const;
 
    operator SparsityStruct() const;
#endif // SWIG
 
 
    casadi_int size1() const;
 
    casadi_int rows() const {return size1();}
 
    casadi_int size2() const;
 
    casadi_int columns() const {return size2();}
 
    casadi_int numel() const;
 
    double density() const;
 
    bool is_empty(bool both=false) const;
 
    casadi_int nnz() const;
 
    casadi_int nnz_upper(bool strictly=false) const;
 
    casadi_int nnz_lower(bool strictly=false) const;
 
    casadi_int nnz_diag() const;
 
    casadi_int bw_upper() const;
 
    casadi_int bw_lower() const;
 
    std::pair<casadi_int, casadi_int> size() const;
 
    casadi_int size(casadi_int axis) const;
 
    Dict info() const;
 
    void to_file(const std::string& filename, const std::string& format_hint="") const;
 
    static Sparsity from_file(const std::string& filename, const std::string& format_hint="");
 
#ifndef SWIG
    void serialize(std::ostream &stream) const;
#endif
 
    std::string serialize() const;
 
    static Sparsity deserialize(std::istream& stream);
 
    static Sparsity deserialize(const std::string& s);
 
    void serialize(SerializingStream& s) const;
 
    static Sparsity deserialize(DeserializingStream& s);
 
#ifndef SWIG
    const casadi_int* row() const;
 
    const casadi_int* colind() const;
#endif
 
    std::vector<casadi_int> get_row() const;
 
    std::vector<casadi_int> get_colind() const;
 
    casadi_int colind(casadi_int cc) const;
 
    casadi_int row(casadi_int el) const;
 
    std::vector<casadi_int> get_col() const;
 
    void resize(casadi_int nrow, casadi_int ncol);
 
    casadi_int add_nz(casadi_int rr, casadi_int cc);
 
    casadi_int get_nz(casadi_int rr, casadi_int cc) const;
 
    bool has_nz(casadi_int rr, casadi_int cc) const;
 
    std::vector<casadi_int> get_nz(const std::vector<casadi_int>& rr,
                                    const std::vector<casadi_int>& cc) const;
 
    void get_nz(std::vector<casadi_int>& SWIG_INOUT(indices)) const;
 
    std::vector<casadi_int> get_lower() const;
 
    std::vector<casadi_int> get_upper() const;
 
    void get_ccs(std::vector<casadi_int>& SWIG_OUTPUT(colind),
                std::vector<casadi_int>& SWIG_OUTPUT(row)) const;
 
    void get_crs(std::vector<casadi_int>& SWIG_OUTPUT(rowind),
                std::vector<casadi_int>& SWIG_OUTPUT(col)) const;
 
    void get_triplet(std::vector<casadi_int>& SWIG_OUTPUT(row),
                    std::vector<casadi_int>& SWIG_OUTPUT(col)) const;
 
    Sparsity sub(const std::vector<casadi_int>& rr,
                 const std::vector<casadi_int>& cc,
                 std::vector<casadi_int>& SWIG_OUTPUT(mapping), bool ind1=false) const;
 
    Sparsity sub(const std::vector<casadi_int>& rr, const Sparsity& sp,
                 std::vector<casadi_int>& SWIG_OUTPUT(mapping), bool ind1=false) const;
 
    Sparsity T() const;
 
    Sparsity transpose(std::vector<casadi_int>& SWIG_OUTPUT(mapping),
                        bool invert_mapping=false) const;
 
    bool is_transpose(const Sparsity& y) const;
 
    bool is_reshape(const Sparsity& y) const;
 
    bool is_compactible(std::vector<casadi_int>& SWIG_OUTPUT(row),
                        std::vector<casadi_int>& SWIG_OUTPUT(col)) const;
 
 
#ifndef SWIG
    Sparsity combine(const Sparsity& y, bool f0x_is_zero, bool function0_is_zero,
                            std::vector<unsigned char>& mapping) const;
#endif // SWIG
    Sparsity combine(const Sparsity& y, bool f0x_is_zero, bool function0_is_zero) const;
 
 
#ifndef SWIG
    Sparsity unite(const Sparsity& y, std::vector<unsigned char>& mapping) const;
#endif // SWIG
    Sparsity unite(const Sparsity& y) const;
    Sparsity operator+(const Sparsity& b) const;
 
 
#ifndef SWIG
    Sparsity intersect(const Sparsity& y,
                       std::vector<unsigned char>& mapping) const;
#endif // SWIG
    Sparsity intersect(const Sparsity& y) const;
    Sparsity operator*(const Sparsity& b) const;
 
    bool is_subset(const Sparsity& rhs) const;
 
    Sparsity sparsity_cast_mod(const Sparsity& X, const Sparsity& Y) const;
 
    Sparsity pattern_inverse() const;
 
#ifndef SWIG
    static void mul_sparsityF(const bvec_t* x, const Sparsity& x_sp,
                              const bvec_t* y, const Sparsity& y_sp,
                              bvec_t* z, const Sparsity& z_sp,
                              bvec_t* w);
 
    static void mul_activityF(const bvec_t* x, const Sparsity& x_sp,
                              const bvec_t* y, const Sparsity& y_sp,
                              bvec_t* z, const Sparsity& z_sp,
                              bvec_t* w);
 
    static void mul_sparsityR(bvec_t* x, const Sparsity& x_sp,
                              bvec_t* y, const Sparsity& y_sp,
                              bvec_t* z, const Sparsity& z_sp,
                              bvec_t* w);
 
 
    static Sparsity horzcat(const std::vector<Sparsity> & sp);
    static Sparsity vertcat(const std::vector<Sparsity> & sp);
    static Sparsity blockcat(const std::vector< std::vector< Sparsity > > &v);
    static Sparsity diagcat(const std::vector< Sparsity > &v);
    static std::vector<Sparsity>
      horzsplit(const Sparsity& x, const std::vector<casadi_int>& offset);
    static std::vector<Sparsity>
      vertsplit(const Sparsity& x, const std::vector<casadi_int>& offset);
    static std::vector<Sparsity>
      diagsplit(const Sparsity& x,
                const std::vector<casadi_int>& offset1,
                const std::vector<casadi_int>& offset2);
    static Sparsity mtimes(const Sparsity& x, const Sparsity& y,
                           const std::string& blas = "reference");
    static Sparsity mac(const Sparsity& x, const Sparsity& y, const Sparsity& z,
                        const std::string& /*blas*/ = "reference") { return z;}
    static Sparsity reshape(const Sparsity& x, casadi_int nrow, casadi_int ncol);
    static Sparsity reshape(const Sparsity& x, const Sparsity& sp);
    static Sparsity sparsity_cast(const Sparsity& x, const Sparsity& sp);
    static casadi_int sprank(const Sparsity& x);
    static casadi_int norm_0_mul(const Sparsity& x, const Sparsity& A);
    static Sparsity kron(const Sparsity& a, const Sparsity& b);
 
    static Sparsity kron_contract(const Sparsity& sp_m, const Sparsity& sp_x, bool inner);
    static Sparsity triu(const Sparsity& x, bool includeDiagonal=true);
    static Sparsity tril(const Sparsity& x, bool includeDiagonal=true);
 
    static Sparsity sum2(const Sparsity &x);
    static Sparsity sum1(const Sparsity &x);
#endif //SWIG
 
    void enlarge(casadi_int nrow, casadi_int ncol,
                  const std::vector<casadi_int>& rr,
                  const std::vector<casadi_int>& cc, bool ind1=false);
 
    void enlargeRows(casadi_int nrow, const std::vector<casadi_int>& rr, bool ind1=false);
 
    void enlargeColumns(casadi_int ncol, const std::vector<casadi_int>& cc, bool ind1=false);
 
    Sparsity makeDense(std::vector<casadi_int>& SWIG_OUTPUT(mapping)) const;
 
    std::vector<casadi_int> erase(const std::vector<casadi_int>& rr,
                                  const std::vector<casadi_int>& cc, bool ind1=false);
 
    std::vector<casadi_int> erase(const std::vector<casadi_int>& rr, bool ind1=false);
 
    void append(const Sparsity& sp);
 
    void appendColumns(const Sparsity& sp);
 
    bool is_scalar(bool scalar_and_dense=false) const;
 
    bool is_dense() const;
 
    bool is_row() const;
 
    bool is_column() const;
 
    bool is_vector() const;
 
    bool is_diag() const;
 
    bool is_square() const;
 
    bool is_symmetric() const;
 
    bool is_triu(bool strictly = false) const;
 
    bool is_tril(bool strictly = false) const;
 
    bool is_singular() const;
 
    bool is_permutation() const;
 
    bool is_selection(bool allow_empty=false) const;
 
    bool is_orthonormal(bool allow_empty=false) const;
 
    bool is_orthonormal_rows(bool allow_empty=false) const;
 
    bool is_orthonormal_columns(bool allow_empty=false) const;
 
    bool rowsSequential(bool strictly=true) const;
 
    void removeDuplicates(std::vector<casadi_int>& SWIG_INOUT(mapping));
 
#ifndef SWIG
    typedef std::unordered_multimap<std::size_t, WeakRef> CachingMap;
 
    static CachingMap& getCache();
 
#ifdef CASADI_WITH_THREADSAFE_SYMBOLICS
    // Safe access to CachingMap
    static std::mutex cachingmap_mtx;
#endif //CASADI_WITH_THREADSAFE_SYMBOLICS
 
    static const Sparsity& getScalar();
 
    static const Sparsity& getScalarSparse();
 
    static const Sparsity& getEmpty();
 
#endif //SWIG
 
    std::vector<casadi_int> etree(bool ata=false) const;
 
    Sparsity ldl(std::vector<casadi_int>& SWIG_OUTPUT(p), bool amd=true) const;
 
    void qr_sparse(Sparsity& SWIG_OUTPUT(V), Sparsity& SWIG_OUTPUT(R),
                   std::vector<casadi_int>& SWIG_OUTPUT(prinv),
                   std::vector<casadi_int>& SWIG_OUTPUT(pc), bool amd=true) const;
 
    casadi_int dfs(casadi_int j, casadi_int top, std::vector<casadi_int>& SWIG_INOUT(xi),
            std::vector<casadi_int>& SWIG_INOUT(pstack),
            const std::vector<casadi_int>& pinv, std::vector<bool>& SWIG_INOUT(marked)) const;
 
    casadi_int scc(std::vector<casadi_int>& SWIG_OUTPUT(index),
            std::vector<casadi_int>& SWIG_OUTPUT(offset)) const;
 
    casadi_int btf(std::vector<casadi_int>& SWIG_OUTPUT(rowperm),
            std::vector<casadi_int>& SWIG_OUTPUT(colperm),
            std::vector<casadi_int>& SWIG_OUTPUT(rowblock),
            std::vector<casadi_int>& SWIG_OUTPUT(colblock),
            std::vector<casadi_int>& SWIG_OUTPUT(coarse_rowblock),
            std::vector<casadi_int>& SWIG_OUTPUT(coarse_colblock)) const;
 
    std::vector<casadi_int> amd() const;
 
#ifndef SWIG
    void spsolve(bvec_t* X, bvec_t* B, bool tr) const;
#endif // SWIG
 
    std::vector<casadi_int> find(bool ind1=SWIG_IND1) const;
 
#ifndef SWIG
    void find(std::vector<casadi_int>& loc, bool ind1=false) const;
#endif // SWIG
 
    Sparsity uni_coloring(const Sparsity& AT=Sparsity(),
                          casadi_int cutoff = std::numeric_limits<casadi_int>::max()) const;
 
    Sparsity star_coloring_new(std::vector<casadi_int>& SWIG_OUTPUT(which_color),
        const Dict& opts=Dict()) const;
 
    Sparsity star_coloring(casadi_int ordering = 1,
                            casadi_int cutoff = std::numeric_limits<casadi_int>::max()) const;
 
    Sparsity star_coloring2(casadi_int ordering = 1,
                            casadi_int cutoff = std::numeric_limits<casadi_int>::max()) const;
 
    std::vector<casadi_int> largest_first() const;
 
    Sparsity pmult(const std::vector<casadi_int>& p,
                    bool permute_rows=true, bool permute_columns=true,
                    bool invert_permutation=false) const;
 
    std::string dim(bool with_nz=false) const;
 
    std::string postfix_dim() const;
 
    std::string repr_el(casadi_int k) const;
 
    void spy(std::ostream &stream=casadi::uout()) const;
 
    void spy_matlab(const std::string& mfile) const;
 
    void export_code(const std::string& lang, std::ostream &stream=casadi::uout(),
       const Dict& options=Dict()) const;
 
    static std::string type_name() {return "Sparsity";}
 
    // Hash the sparsity pattern
    std::size_t hash() const;
 
    static bool test_cast(const SharedObjectInternal* ptr);
 
    static Sparsity kkt(const Sparsity& H, const Sparsity& J,
                        bool with_x_diag=true, bool with_lam_g_diag=true);
 
#ifndef SWIG
    template<typename T>
    void set(T* data, const T* val_data, const Sparsity& val_sp) const;
 
    template<typename T>
    void add(T* data, const T* val_data, const Sparsity& val_sp) const;
 
    template<typename T>
    void bor(T* data, const T* val_data, const Sparsity& val_sp) const;
 
    static std::string file_format(const std::string& filename,
      const std::string& format_hint, const std::set<std::string>& file_formats);
    static std::set<std::string> file_formats;
  private:
 
    void assign_cached(casadi_int nrow, casadi_int ncol, const std::vector<casadi_int>& colind,
                      const std::vector<casadi_int>& row, bool order_rows=false);
 
    void assign_cached(casadi_int nrow, casadi_int ncol,
                        const casadi_int* colind, const casadi_int* row, bool order_rows=false);
 
#endif //SWIG
  };
 
  CASADI_EXPORT std::size_t hash_sparsity(casadi_int nrow, casadi_int ncol,
                                          const std::vector<casadi_int>& colind,
                                          const std::vector<casadi_int>& row);
 
  CASADI_EXPORT std::size_t hash_sparsity(casadi_int nrow, casadi_int ncol,
                                          const casadi_int* colind,
                                          const casadi_int* row);
 
#ifndef SWIG
  // Template instantiations
  template<typename DataType>
  void Sparsity::set(DataType* data, const DataType* val_data, const Sparsity& val_sp) const {
    // Get dimensions of this
    const casadi_int sz = nnz();
    const casadi_int sz1 = size1();
    const casadi_int sz2 = size2();
 
    // Get dimensions of assigning matrix
    const casadi_int val_sz = val_sp.nnz();
    const casadi_int val_sz1 = val_sp.size1();
    const casadi_int val_sz2 = val_sp.size2();
    const casadi_int val_nel = val_sz1*val_sz2;
 
    // Check if sparsity matches
    if (val_sp==*this) {
      std::copy(val_data, val_data+sz, data);
    } else if (this->is_empty()) {
      // Quick return
      return;
    } else if (val_sp.is_empty()) {
      // Quick return
      return;
    } else if (val_nel==1) { // if scalar
      std::fill(data, data+sz, val_sz==0 ? DataType(0) : val_data[0]);
    } else if (sz2==val_sz2 && sz1==val_sz1) {
      // Matching dimensions
      // Sparsity
      const casadi_int* c = row();
      const casadi_int* rind = colind();
      const casadi_int* v_c = val_sp.row();
      const casadi_int* v_rind = val_sp.colind();
 
      // For all columns
      for (casadi_int i=0; i<sz2; ++i) {
 
        // Nonzero of the assigning matrix
        casadi_int v_el = v_rind[i];
 
        // First nonzero of the following column
        casadi_int v_el_end = v_rind[i+1];
 
        // Next row of the assigning matrix
        casadi_int v_j = v_el<v_el_end ? v_c[v_el] : sz1;
 
        // Assign all nonzeros
        for (casadi_int el=rind[i]; el!=rind[i+1]; ++el) {
 
          //  Get row
          casadi_int j=c[el];
 
          // Forward the assigning nonzero
          while (v_j<j) {
            v_el++;
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          }
 
          // Assign nonzero
          if (v_j==j) {
            data[el] = val_data[v_el++];
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          } else {
            data[el] = 0;
          }
        }
      }
    } else if (sz1==val_sz2 && sz2==val_sz1 && sz2 == 1) {
      // Assign transposed (this is column)
      const casadi_int* v_cind = val_sp.colind();
      const casadi_int* r = row();
      for (casadi_int el=0; el<sz; ++el) {
        casadi_int rr=r[el];
        data[el] = v_cind[rr]==v_cind[rr+1] ? 0 : val_data[v_cind[rr]];
      }
    } else if (sz1==val_sz2 && sz2==val_sz1 && sz1 == 1) {
      // Assign transposed (this is row)
      for (casadi_int el=0; el<sz; ++el) data[el] = 0;
      const casadi_int* cind = colind();
      const casadi_int* v_r = val_sp.row();
      for (casadi_int el=0; el<val_sz; ++el) {
        casadi_int rr=v_r[el];
        if (cind[rr]!=cind[rr+1]) {
          data[cind[rr]] = val_data[el];
        }
      }
    } else {
      // Make sure that dimension matches
      casadi_error("Sparsity::set<DataType>: shape mismatch. lhs is "
                   + dim() + ", while rhs is " + val_sp.dim() + ".");
    }
  }
 
  template<typename DataType>
  void Sparsity::add(DataType* data, const DataType* val_data, const Sparsity& val_sp) const {
    // Get dimensions of this
    const casadi_int sz = nnz();
    const casadi_int sz1 = size1();
    const casadi_int sz2 = size2();
    const casadi_int nel = sz1*sz2;
 
    // Get dimensions of assigning matrix
    const casadi_int val_sz = val_sp.nnz();
    const casadi_int val_sz1 = val_sp.size1();
    const casadi_int val_sz2 = val_sp.size2();
    const casadi_int val_nel = val_sz1*val_sz2;
 
    // Check if sparsity matches
    if (val_sp==*this) {
      for (casadi_int k=0; k<sz; ++k) {
        data[k] += val_data[k];
      }
    } else if (this->is_empty()) {
      // Quick return
      return;
    } else if (val_sp.is_empty()) {
      // Quick return
      return;
    }  else if (val_nel==1) { // if scalar
      if (val_sz!=0) {
        for (casadi_int k=0; k<sz; ++k) {
          data[k] += val_data[0];
        }
      }
    } else {
      // Quick return if empty
      if (nel==0 && val_nel==0) return;
 
      // Make sure that dimension matches
      casadi_assert(sz2==val_sz2 && sz1==val_sz1,
                            "Sparsity::add<DataType>: shape mismatch. lhs is "
                            + dim() + ", while rhs is " + val_sp.dim() + ".");
 
      // Sparsity
      const casadi_int* c = row();
      const casadi_int* rind = colind();
      const casadi_int* v_c = val_sp.row();
      const casadi_int* v_rind = val_sp.colind();
 
      // For all columns
      for (casadi_int i=0; i<sz2; ++i) {
 
        // Nonzero of the assigning matrix
        casadi_int v_el = v_rind[i];
 
        // First nonzero of the following column
        casadi_int v_el_end = v_rind[i+1];
 
        // Next row of the assigning matrix
        casadi_int v_j = v_el<v_el_end ? v_c[v_el] : sz1;
 
        // Assign all nonzeros
        for (casadi_int el=rind[i]; el!=rind[i+1]; ++el) {
 
          //  Get row
          casadi_int j=c[el];
 
          // Forward the assigning nonzero
          while (v_j<j) {
            v_el++;
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          }
 
          // Assign nonzero
          if (v_j==j) {
            data[el] += val_data[v_el++];
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          }
        }
      }
    }
  }
 
  template<typename DataType>
  void Sparsity::bor(DataType* data, const DataType* val_data, const Sparsity& val_sp) const {
    // Get dimensions of this
    const casadi_int sz = nnz();
    const casadi_int sz1 = size1();
    const casadi_int sz2 = size2();
    const casadi_int nel = sz1*sz2;
 
    // Get dimensions of assigning matrix
    const casadi_int val_sz = val_sp.nnz();
    const casadi_int val_sz1 = val_sp.size1();
    const casadi_int val_sz2 = val_sp.size2();
    const casadi_int val_nel = val_sz1*val_sz2;
 
    // Check if sparsity matches
    if (val_sp==*this) {
      for (casadi_int k=0; k<sz; ++k) {
        data[k] |= val_data[k];
      }
    } else if (this->is_empty()) {
      // Quick return
      return;
    } else if (val_sp.is_empty()) {
      // Quick return
      return;
    }  else if (val_nel==1) { // if scalar
      if (val_sz!=0) {
        for (casadi_int k=0; k<sz; ++k) {
          data[k] |= val_data[0];
        }
      }
    } else {
      // Quick return if empty
      if (nel==0 && val_nel==0) return;
 
      // Make sure that dimension matches
      casadi_assert(sz2==val_sz2 && sz1==val_sz1,
                            "Sparsity::add<DataType>: shape mismatch. lhs is "
                            + dim() + ", while rhs is " + val_sp.dim() + ".");
 
      // Sparsity
      const casadi_int* c = row();
      const casadi_int* rind = colind();
      const casadi_int* v_c = val_sp.row();
      const casadi_int* v_rind = val_sp.colind();
 
      // For all columns
      for (casadi_int i=0; i<sz2; ++i) {
 
        // Nonzero of the assigning matrix
        casadi_int v_el = v_rind[i];
 
        // First nonzero of the following column
        casadi_int v_el_end = v_rind[i+1];
 
        // Next row of the assigning matrix
        casadi_int v_j = v_el<v_el_end ? v_c[v_el] : sz1;
 
        // Assign all nonzeros
        for (casadi_int el=rind[i]; el!=rind[i+1]; ++el) {
 
          //  Get row
          casadi_int j=c[el];
 
          // Forward the assigning nonzero
          while (v_j<j) {
            v_el++;
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          }
 
          // Assign nonzero
          if (v_j==j) {
            data[el] |= val_data[v_el++];
            v_j = v_el<v_el_end ? v_c[v_el] : sz1;
          }
        }
      }
    }
  }
 
#endif //SWIG
 
  typedef std::map<std::string, Sparsity> SpDict;
 
} // namespace casadi
 
#endif // CASADI_SPARSITY_HPP