ndgridmap.hpp

#ifndef NDGRIDMAP_HPP_
#define NDGRIDMAP_HPP_

#include <iostream>
#include <vector>
#include <string>
#include <algorithm>
#include <cstddef>
#include <array>
#include <sstream>

#include <utility>

#include <fast_methods/console/console.h>


template <class T, size_t ndims> class nDGridMap {

    friend std::ostream& operator <<
    (std::ostream & os, nDGridMap<T,ndims> & g) {
        os << console::str_info("Grid cell information");
        os << "\t" << g.getCell(0).type() << std::endl;
        os << "\t" << g.ncells_ << " cells." << std::endl;
        os << "\t" << g.leafsize_ << " leafsize (m)." << std::endl;
        os << "\t" << ndims << " dimensions:" << std::endl;

        for (unsigned int i = 0; i < ndims; ++i)
            os << "\t\t" << "d" << i << "\tsize: " << g.dimsize_[i] << std::endl;

        return os;
    }

    public:

      nDGridMap () : leafsize_(1.0f), clean_(true) {}

        nDGridMap
        (const std::array<unsigned int, ndims> & dimsize, double leafsize = 1.0f) :
        leafsize_(leafsize),
        clean_(true) {
            resize(dimsize);
        }

        void resize
        (const std::array<unsigned int, ndims> & dimsize) {
            dimsize_ = dimsize;
            // Computing the total number of cells and the auxiliar array d_.
            ncells_= 1;
            for (unsigned int i = 0; i < ndims; ++i) {
                ncells_ *= dimsize_[i];
                d_[i] = ncells_;
            }

            //Resizing gridmap and initializing with default values.
            cells_.clear();
            cells_.resize(ncells_, T());

            // Setting the index_ member of the cells, which a-priori is unknown.
            for (unsigned int i = 0; i < cells_.size(); ++i)
                cells_[i].setIndex(i);
            clean_ = true;
        }

        inline T & operator[]
        (unsigned int idx) {
            return cells_[idx];
        }

        inline double getLeafSize() const { return leafsize_; }

        inline void setLeafSize(const double leafsize) { leafsize_ = leafsize; }

        inline T & getCell
        (unsigned int idx) {
            return cells_[idx];
            }

        inline std::array<unsigned int, ndims> getDimSizes() const { return dimsize_;}

        double getMinValueInDim
        (unsigned int idx, unsigned int dim) {
            n_neighs = 0; // How many neighbors obtained in that dimension.
            getNeighborsInDim(idx,n_,dim);

            if (n_neighs == 1)
                return cells_[n_[0]].getValue();
            else
                return (cells_[n_[0]].getValue()<cells_[n_[1]].getValue()) ? cells_[n_[0]].getValue() : cells_[n_[1]].getValue();
        }

        unsigned int getNumberNeighborsInDim
        (int idx, std::array<unsigned int, ndims> &m, unsigned int dim)   {
            n_neighs = 0;
            getNeighborsInDim(idx,n_,dim);
            m = n_;
            return n_neighs;
        }

        unsigned int getNeighbors
        (unsigned int idx, std::array<unsigned int, 2*ndims> & neighs) {
            n_neighs = 0;
            for (unsigned int i = 0; i < ndims; ++i)
                getNeighborsInDim(idx,neighs,i);

            return n_neighs;
        }

        void getNeighborsInDim
        (unsigned int idx, std::array<unsigned int, 2*ndims>& neighs, unsigned int dim) {
            unsigned int c1,c2; // Candidate neighbors in dimension.
            if (dim == 0) {
                c1 = idx-1;
                c2 = idx+1;
            }
            else {
                c1 = idx-d_[dim-1];
                c2 = idx+d_[dim-1];
            }
            // Checking neighbor 1: is in the same n-dimensional slice (row, plane, cube, etc)?
            if ((c1 >= 0) && (c1/d_[dim] == idx/d_[dim]))
                neighs[n_neighs++] = c1;
            // Checking neighbor 2. Full check, not necessary: if >ncells_ then is in another slice.
            if (c2/d_[dim] == idx/d_[dim])
                neighs[n_neighs++] = c2;
        }

        void getNeighborsInDim
        (unsigned int idx, std::array<unsigned int, 2>& neighs, unsigned int dim) {
            // Candidate neighbors in dimension.
            unsigned int c1,c2;
            if (dim == 0) {
                c1 = idx-1;
                c2 = idx+1;
            }
            else {
                c1 = idx-d_[dim-1];
                c2 = idx+d_[dim-1];
            }
            // Checking neighbor 1: is in the same n-dimensional slice (row, plane, cube, etc)?
            if ((c1 >= 0) && (c1/d_[dim] == idx/d_[dim]))
                neighs[n_neighs++] = c1;
            // Checking neighbor 2. Full check, not necessary: if >ncells_ then is in another slice.
            if (c2/d_[dim] == idx/d_[dim])
                neighs[n_neighs++] = c2;
        }

        unsigned int idx2coord
        (unsigned int idx, std::array<unsigned int, ndims> & coords) {
            if (coords.size() != ndims)
                return -1;
            else {
                coords[ndims-1] = idx/d_[ndims-2]; // First step done apart.
                unsigned int aux = idx - coords[ndims-1]*d_[ndims-2];
                for (unsigned int i = ndims - 2; i > 0; --i) {
                    coords[i] = aux/d_[i-1];
                    aux -= coords[i]*d_[i-1];
                }
                coords[0] = aux; //Last step done apart.
            }
            return 1;
        }

        unsigned int coord2idx
        (const std::array<unsigned int, ndims> & coords, unsigned int & idx) {
            if (coords.size() != ndims)
                return -1;
            else {
                idx = coords[0];
                for(unsigned int i = 1; i < ndims; ++i)
                    idx += coords[i]*d_[i-1];
            }
            return 1;
        }

        void showCoords
        (unsigned int idx) {
            std::array<unsigned int, ndims> coords;
            idx2coord(idx, coords);
            for (unsigned int i = 0; i < ndims; ++i)
                std::cout << coords[i] << "\t";
            std::cout << '\n';
        }

         void showCoords
         (std::array<unsigned int, ndims> coords) {
             for (unsigned int i = 0; i < ndims; ++i)
                 std::cout << coords[i] << "\t";
             std::cout << '\n';
         }

        void showIdx
        (const std::array<unsigned int, ndims> & coords) {
            unsigned int idx;
            coord2idx(coords, idx);
            std::cout << idx << '\n';
        }

        inline unsigned int size
        () const {
            return ncells_;
        }

        inline double getMaxValue
        () const {
            double max = 0;
            for (const T & c:cells_) {
                if (!isinf(c.getValue()) && c.getValue() > max)
                    max = c.getValue();
            }
            return max;
        }

        inline bool isClean
        () const {
            return clean_;
        }

        inline void setClean
        (bool c) {
            clean_ = c;
        }

        void clean
        () {
            if(!clean_) {
                for (T & c:cells_)
                    c.setDefault();
                clean_ = true;
            }
        }

        void clear
        () {
            cells_.clear();
            occupied_.clear();
        }

        std::string getDimSizesStr()
        {
            std::stringstream ss;
            for(const auto& d : dimsize_)
                ss << d << "\t";
            return ss.str();
        }

        inline void setOccupiedCells
        (const std::vector<unsigned int> & obs) {
            occupied_ = obs;
        }

        inline void setOccupiedCells
        (std::vector<unsigned int>&& obs) {
            occupied_ = std::move(obs);
        }

        inline void getOccupiedCells
        (std::vector<unsigned int> & obs) const {
            obs = occupied_;
        }

        static constexpr size_t getNDims() {return ndims;}

        double getAvgSpeed
        () {
            double sum = 0;
            unsigned int nObs = 0;
            for (const T & c : cells_) {
                if (!c.isOccupied())
                    sum += c.getVelocity();
                else
                    ++nObs;
            }
            return sum/(ncells_ - nObs);
        }

        double getMaxSpeed
        () {
            double max = 0;
            for (const T & c : cells_)
                if (max < c.getVelocity())
                    max = c.getVelocity();
            return max;
        }

    private:
        std::vector<T> cells_;

        std::array<unsigned int, ndims> dimsize_;

        double leafsize_;

        unsigned int ncells_;

        bool clean_;

        // Auxiliar vectors to speed things up.
        std::array<unsigned int, ndims> d_;

        std::array<unsigned int, 2> n_;

        unsigned int n_neighs;

        std::vector<unsigned int> occupied_;
};

#endif /* NDGRIDCELL_HPP_*/