SparseVector.h

#pragma once

#include <Eigen/Dense>

#include "isam/util.h"

namespace isam {

class SparseVector {
  int _nnz;
  int _nnz_max;
  int* _indices;
  double* _values;

  void _copy_from(const SparseVector& vec);

  void _dealloc();

  int _search(int idx) const;

  void _resize(int new_nnz_max);

public:
  SparseVector();

  SparseVector(int nnz_max) : _nnz(0), _nnz_max(nnz_max) {
    _indices = new int[_nnz_max];
    _values = new double[_nnz_max];
  }
  
  SparseVector(const SparseVector& vec);

  SparseVector(const SparseVector& vec, int num, int first = 0);

  SparseVector(int* indices, double* values, int nnz);

  virtual ~SparseVector();

  const SparseVector& operator= (const SparseVector& vec);

  double operator()(int idx) const;

  void copy_raw(int* indices, double* values) const;

  inline void append(int idx, const double val = 0.) {
    requireDebug(_nnz==0 || _indices[_nnz-1] < idx, "SparseVector::append: index has to be after last entry");

    if (_nnz+1 > _nnz_max) {
      // automatic resizing, amortized cost O(1)
      int new_nnz_max = _nnz_max*2;
      _resize(new_nnz_max);
    }
    // insert new entry
    _indices[_nnz] = idx;
    _values[_nnz] = val;
    _nnz++;
  }

  bool set(int idx, const double val = 0.);
  
  
  bool set(int idx, const Eigen::VectorXd& vals);

  void remove(int idx);

  int first() const;

  int last() const;

  void add_entries(int num, int pos);

  void print() const;

  inline int nnz() const {
    return _nnz;
  }

  friend class SparseVectorIter;
};

class SparseVectorIter {
  const SparseVector& s;
  int index;
public:
  inline SparseVectorIter(const SparseVector& sv) : s(sv), index(0) {}

  inline bool valid() const {
    return (index < s._nnz);
  }

  inline int get() const {
    requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range.");
    int tmp = s._indices[index];
    return tmp;
  }

  inline int get(double& val) const {
    requireDebug(index < s._nnz, "SparseVectorIter::get(): Index out of range.");
    val = s._values[index];
    return s._indices[index];
  }

  inline double get_val() const {
    requireDebug(index < s._nnz, "SparseVectorIter::get_val(): Index out of range.");
    return s._values[index];
  }

  inline void next() {
    if (index < s._nnz) {
      index++;
    }
  }
};

}