class ImplicitSurface {
public:
    virtual float evaluate(float x , float y , float z) const = 0;
};

class MarchingCubes : public pcl::MarchingCubes<pcl::PointNormal> {
public:
    MarchingCubes(ImplicitSurface*);
    void voxelizeData();

private:
    ImplicitSurface* implicitSurface;
};

void MarchingCubes::voxelizeData() {
    for (int x = 0; x < res_x_; ++x)
        for (int y = 0; y < res_y_; ++y)
            for (int z = 0; z < res_z_; ++z)
            {
                std::vector<int> nn_indices;
                std::vector<float> nn_sqr_dists;

                Eigen::Vector3f point;
                point[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (x) / float (res_x_);
                point[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (y) / float (res_y_);
                point[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (z) / float (res_z_);

                pcl::PointNormal p;
                p.getVector3fMap () = point;

                tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists);

                float currentValue = implicitSurface->evaluate (point[0], point[1], point[2]);
                grid_[x * res_y_ * res_z_ + y * res_z_ + z] = currentValue;
            }
}