SpaceIndex.h

#ifndef SPACEINDEX_H
#define SPACEINDEX_H

#include <vector>
#include <Eigen/Eigen>

#include "Logging/Logger.h"
#include "API/project.hpp"
#include "utils/Geometry.h"


class SpaceIndex
{
    MOVE3D_STATIC_LOGGER;
public:
    SpaceIndex();
    ~SpaceIndex();

    bool compute();

    double freeVolume() const;
    double occupiedVolume() const;

    double distance(SpaceIndex const &other);

    double checkCell(p3d_vector3 p);

    bool isCellFree(Eigen::Vector3d p);
    void setCellFree(Eigen::Vector3d p,bool free);
private:
    double _cellRadius;
    double _distance;
    Eigen::Vector3d _center;

    std::vector<Eigen::Vector3d> _free;
    std::vector<Eigen::Vector3d> _occup;
    std::vector<bool> _grid;

    std::vector<std::pair<double,unsigned int> > _sorted_robot; 

};

#endif // SPACEINDEX_H