array_allocator.h

Go to the documentation of this file.

// HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
// Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
// 
// HOG-Man 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.
// 
// HOG-Man 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 this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _ARRAY_ALLOCATOR_HH_
#define _ARRAY_ALLOCATOR_HH_
#include <assert.h>
#include <string.h>
#include "macros.h"

template <int N, typename Base>
  struct _ArrayAllocator{
    typedef Base BaseType;
    static const int TemplateSize=N;
    _ArrayAllocator(int n=N) {
/*       if (n!=N) */
/*      std::cerr << "Fatal n= " << n << " N=" << N << std::endl; */
      assert (n==N); (void) n;
    }
    int size() const {return N;}

    Base* ptr() {return _svalues;}

    const Base* ptr() const {return _svalues;}

    inline Base& operator [] (int i) { return _svalues[i]; }

    inline const Base& operator [] (int i) const { return _svalues[i]; }
  protected:
    Base _svalues[N];
  };


template <>
template <typename Base>
struct _ArrayAllocator <0, Base>{
  typedef Base BaseType;
  static const int TemplateSize=0;

  _ArrayAllocator(int n=0) {
    allocate(n);
  }
  
  ~_ArrayAllocator(){
    deallocate();
  }
  
  _ArrayAllocator<0,Base>& operator = (const _ArrayAllocator <0, Base>& src){
    resize(src.size());
    copy(src._dvalues);
    return *this;
  }
  
  _ArrayAllocator(const _ArrayAllocator <0, Base>& src) {
    allocate(src.size());
    copy(src._dvalues);
  }

  int size() const { return _size;}

  Base* ptr() {return _dvalues;}

  const Base* ptr() const {return _dvalues;}

  inline Base& operator [] (int i) { return _dvalues[i]; }

  inline const Base& operator [] (int i) const { return _dvalues[i]; }

  void resize(int s) {
    if (s==_size)
      return;
    if (! s)
      deallocate();
    allocate(s);
  }

 protected:
  void copy(const Base* src){
    if ( AISNAV_IS_POD(Base) )
      memcpy(_dvalues, src, _size*sizeof(Base));
    else
      for (int i=0; i<_size; i++)
        _dvalues[i]=src[i];
  }

  void deallocate() {
    if (_size)
      delete [] _dvalues;
    _dvalues=0;
    _size=0;
  }

  void allocate(int n) {
    _dvalues = n  ? new Base[n] : 0;
    _size=n;
  }
    int _size;
    Base* _dvalues;
  };


template <int N, typename Base>
  void st2dyn (_ArrayAllocator<0,Base>& dest, const _ArrayAllocator<N,Base>& src) {
  dest.resize(src.size());
  for (int i=0; i<src.size(); i++)
    dest[i]=src[i];
}

template <int N, typename Base>
  void dyn2st(_ArrayAllocator<N,Base>& dest, const _ArrayAllocator<0,Base>& src){
  assert (dest.size()==src.size());
  for (int i=0; i<src.size(); i++)
    dest[i]=src[i];
}

template <int N, int M, typename Base>
  struct _Array2DAllocator{
    typedef Base BaseType;
    static const int TemplateRows = N;
    static const int TemplateCols = M;
    typedef _Array2DAllocator<M,N,Base> TransposedType;

    _Array2DAllocator(int rows_=N, int cols_=M) {
      assert (rows_==N); (void) rows_;
      assert (cols_==M); (void) cols_;
    }

    inline Base* operator[] (int i) {return _svalues[i];}

    inline const Base* operator [] (int i) const {return _svalues[i];}

    int rows() const {return TemplateRows;}

    int cols() const {return TemplateCols;}
  protected:
    Base _svalues[N][M];
  };

template <>
template <typename Base>
struct _Array2DAllocator<0,0,Base>{
    typedef Base BaseType;
    static const int TemplateRows = 0;
    static const int TemplateCols = 0;
    typedef _Array2DAllocator<0,0,Base> TransposedType;

    inline Base* operator[] (int i) {return _dvalues[i];}

    inline const Base* operator [] (int i) const {return _dvalues[i];}

    ~_Array2DAllocator(){
      deallocate();
    }
    
    _Array2DAllocator(int rows_=0, int cols_=0) {
      allocate (rows_, cols_);
    }

    _Array2DAllocator(const _Array2DAllocator<0,0,Base>& src){
      allocate (src.rows(), src.cols());
      copy(src._dvalues);
    }

    _Array2DAllocator<0,0,Base>& operator = (const _Array2DAllocator<0,0,Base>& src){
      resize( src._rows, src._cols);
      copy(src._dvalues);
      return *this;
    }

    int rows() const {return _rows;}

    int cols() const {return _cols;}

    void resize (int n, int m) {
      if (n==cols() && m==rows())
        return;
      deallocate();
      allocate(n,m);
    }

 protected:
    Base ** _dvalues;
    void deallocate() {
      if (rows()&&cols()){
        for (int r=0; r<rows(); r++)
          delete [] _dvalues[r];
        delete [] _dvalues;
      }
      _rows=0;
      _cols=0;
      _dvalues=0;
    }

    void allocate(int n, int m){
      if (m==0 || n==0){
        _rows=0;
        _cols=0;
        _dvalues=0;
        return;
      }
      _rows=n;
      _cols=m;
      _dvalues = new Base*[_rows];
      for (int r=0; r<_rows; r++)
        _dvalues[r]=new Base[_cols];
    }

    void copy(Base** const src){
      for (int r=0; r<rows(); r++) {
        if ( AISNAV_IS_POD(Base) )
          memcpy(_dvalues[r], src[r], _cols*sizeof(Base));
        else
          for  (int c=0; c<_cols; c++)
            _dvalues[r][c]=src[r][c];
      }
    }
    int _cols;
    int _rows;
};

template <int M, int N, typename Base>
  void st2dyn (_Array2DAllocator<0,0,Base>& dest, const _Array2DAllocator<M,N,Base>& src) {
    dest.resize(src.rows(), src.cols());
    for (int r=0; r<src.rows(); r++)
      for (int c=0; c<src.cols(); c++)
        dest[r][c]=src[r][c];
  }

template <int M, int N, typename Base>
  void dyn2st(_Array2DAllocator<M,N,Base>& dest, const _Array2DAllocator<0,0,Base>& src){
    assert (dest.rows()==src.rows());
    assert (dest.cols()==src.cols());
    for (int r=0; r<src.rows(); r++)
      for (int c=0; c<src.cols(); c++)
        dest[r][c]=src[r][c];
  }


#endif