Grid.h

Go to the documentation of this file.

 
#pragma once
 
#include <array>
#include <bitset>
#include <vector>
 
#include "BaseLib/Logging.h"
 
// GeoLib
#include "AABB.h"
#ifndef NDEBUG
#include "GEOObjects.h"
#endif
 
namespace GeoLib
{
template <typename POINT>
class Grid final : public GeoLib::AABB
{
public:
    template <typename InputIterator>
    Grid(InputIterator first, InputIterator last,
         std::size_t max_num_per_grid_cell = 512);
 
    virtual ~Grid() { delete[] _grid_cell_nodes_map; }
 
    template <typename P>
    POINT* getNearestPoint(P const& pnt) const;
 
    template <typename P>
    std::vector<std::size_t> getPointsInEpsilonEnvironment(P const& pnt,
                                                           double eps) const;
 
    template <typename P>
    std::vector<std::vector<POINT*> const*>
    getPntVecsOfGridCellsIntersectingCube(P const& center,
                                          double half_len) const;
 
    std::vector<std::vector<POINT*> const*>
    getPntVecsOfGridCellsIntersectingCuboid(
        Eigen::Vector3d const& min_pnt, Eigen::Vector3d const& max_pnt) const;
 
#ifndef NDEBUG
    void createGridGeometry(GeoLib::GEOObjects* geo_obj) const;
#endif
 
private:
    void initNumberOfSteps(std::size_t n_per_cell, std::size_t n_pnts,
                           std::array<double, 3> const& extensions);
 
    template <typename T>
    std::array<std::size_t, 3> getGridCoords(T const& pnt) const;
 
    template <typename P>
    std::array<double, 6> getPointCellBorderDistances(
        P const& pnt, std::array<std::size_t, 3> const& coords) const;
 
    template <typename P>
    bool calcNearestPointInGridCell(P const& pnt,
                                    std::array<std::size_t, 3> const& coords,
                                    double& sqr_min_dist,
                                    POINT*& nearest_pnt) const;
 
    static POINT* copyOrAddress(POINT& p) { return &p; }
    static POINT const* copyOrAddress(POINT const& p) { return &p; }
    static POINT* copyOrAddress(POINT* p) { return p; }
 
    std::array<std::size_t, 3> _n_steps = {{1, 1, 1}};
    std::array<double, 3> _step_sizes = {{0.0, 0.0, 0.0}};
    std::vector<POINT*>* _grid_cell_nodes_map = nullptr;
};
 
template <typename POINT>
template <typename InputIterator>
Grid<POINT>::Grid(InputIterator first, InputIterator last,
                  std::size_t max_num_per_grid_cell)
    : GeoLib::AABB(first, last)
{
    auto const n_pnts(std::distance(first, last));
 
    auto const& max{getMaxPoint()};
    auto const& min{getMinPoint()};
    std::array<double, 3> delta = {
        {max[0] - min[0], max[1] - min[1], max[2] - min[2]}};
 
    // enlarge delta
    for (auto& d : delta)
    {
        d = std::nextafter(d, std::numeric_limits<double>::max());
    }
 
    assert(n_pnts > 0);
    initNumberOfSteps(max_num_per_grid_cell, static_cast<std::size_t>(n_pnts),
                      delta);
 
    const std::size_t n_plane(_n_steps[0] * _n_steps[1]);
    _grid_cell_nodes_map = new std::vector<POINT*>[n_plane * _n_steps[2]];
 
    // some frequently used expressions to fill the grid vectors
    for (std::size_t k(0); k < 3; k++)
    {
        if (std::abs(delta[k]) < std::numeric_limits<double>::epsilon())
        {
            delta[k] = std::numeric_limits<double>::epsilon();
        }
        _step_sizes[k] = delta[k] / _n_steps[k];
    }
 
    // fill the grid vectors
    InputIterator it(first);
    while (it != last)
    {
        std::array<std::size_t, 3> coords(getGridCoords(*copyOrAddress(*it)));
        if (coords < _n_steps)
        {
            std::size_t const pos(coords[0] + coords[1] * _n_steps[0] +
                                  coords[2] * n_plane);
            _grid_cell_nodes_map[pos].push_back(
                const_cast<POINT*>(copyOrAddress(*it)));
        }
        else
        {
            ERR("Grid constructor: error computing indices [{:d}, {:d}, {:d}], "
                "max indices [{:d}, {:d}, {:d}].",
                coords[0], coords[1], coords[2], _n_steps[0], _n_steps[1],
                _n_steps[2]);
        }
        it++;
    }
}
 
template <typename POINT>
template <typename P>
std::vector<std::vector<POINT*> const*>
Grid<POINT>::getPntVecsOfGridCellsIntersectingCube(P const& center,
                                                   double half_len) const
{
    Eigen::Vector3d const c{center[0], center[1], center[2]};
    std::array<std::size_t, 3> min_coords(getGridCoords(c.array() - half_len));
    std::array<std::size_t, 3> max_coords(getGridCoords(c.array() + half_len));
 
    std::vector<std::vector<POINT*> const*> pnts;
    pnts.reserve(
        (Eigen::Map<Eigen::Matrix<std::size_t, 3, 1>>(max_coords.data()) -
         Eigen::Map<Eigen::Matrix<std::size_t, 3, 1>>(min_coords.data()))
            .prod());
 
    std::size_t const steps0_x_steps1(_n_steps[0] * _n_steps[1]);
    for (std::size_t c0 = min_coords[0]; c0 < max_coords[0] + 1; c0++)
    {
        for (std::size_t c1 = min_coords[1]; c1 < max_coords[1] + 1; c1++)
        {
            const std::size_t coords0_p_coords1_x_steps0(c0 + c1 * _n_steps[0]);
            for (std::size_t c2 = min_coords[2]; c2 < max_coords[2] + 1; c2++)
            {
                pnts.push_back(
                    &(_grid_cell_nodes_map[coords0_p_coords1_x_steps0 +
                                           c2 * steps0_x_steps1]));
            }
        }
    }
    return pnts;
}
 
template <typename POINT>
std::vector<std::vector<POINT*> const*>
Grid<POINT>::getPntVecsOfGridCellsIntersectingCuboid(
    Eigen::Vector3d const& min_pnt, Eigen::Vector3d const& max_pnt) const
{
    std::array<std::size_t, 3> min_coords(getGridCoords(min_pnt));
    std::array<std::size_t, 3> max_coords(getGridCoords(max_pnt));
 
    std::vector<std::vector<POINT*> const*> pnts;
    pnts.reserve(
        (Eigen::Map<Eigen::Matrix<std::size_t, 3, 1>>(max_coords.data()) -
         Eigen::Map<Eigen::Matrix<std::size_t, 3, 1>>(min_coords.data()))
            .prod());
 
    std::size_t const steps0_x_steps1(_n_steps[0] * _n_steps[1]);
    for (std::size_t c0 = min_coords[0]; c0 < max_coords[0] + 1; c0++)
    {
        for (std::size_t c1 = min_coords[1]; c1 < max_coords[1] + 1; c1++)
        {
            const std::size_t coords0_p_coords1_x_steps0(c0 + c1 * _n_steps[0]);
            for (std::size_t c2 = min_coords[2]; c2 < max_coords[2] + 1; c2++)
            {
                pnts.push_back(
                    &(_grid_cell_nodes_map[coords0_p_coords1_x_steps0 +
                                           c2 * steps0_x_steps1]));
            }
        }
    }
    return pnts;
}
 
#ifndef NDEBUG
template <typename POINT>
void Grid<POINT>::createGridGeometry(GeoLib::GEOObjects* geo_obj) const
{
    std::vector<std::string> grid_names;
 
    GeoLib::Point const& llf(getMinPoint());
    GeoLib::Point const& urb(getMaxPoint());
 
    const double dx((urb[0] - llf[0]) / _n_steps[0]);
    const double dy((urb[1] - llf[1]) / _n_steps[1]);
    const double dz((urb[2] - llf[2]) / _n_steps[2]);
 
    // create grid names and grid boxes as geometry
    for (std::size_t i(0); i < _n_steps[0]; i++)
    {
        for (std::size_t j(0); j < _n_steps[1]; j++)
        {
            for (std::size_t k(0); k < _n_steps[2]; k++)
            {
                std::string name("Grid-");
                name += std::to_string(i);
                name += "-";
                name += std::to_string(j);
                name += "-";
                name += std::to_string(k);
                grid_names.push_back(name);
 
                {
                    auto points =
                        std::make_unique<std::vector<GeoLib::Point*>>();
                    points->push_back(new GeoLib::Point(
                        llf[0] + i * dx, llf[1] + j * dy, llf[2] + k * dz));
                    points->push_back(new GeoLib::Point(llf[0] + i * dx,
                                                        llf[1] + (j + 1) * dy,
                                                        llf[2] + k * dz));
                    points->push_back(new GeoLib::Point(llf[0] + (i + 1) * dx,
                                                        llf[1] + (j + 1) * dy,
                                                        llf[2] + k * dz));
                    points->push_back(new GeoLib::Point(llf[0] + (i + 1) * dx,
                                                        llf[1] + j * dy,
                                                        llf[2] + k * dz));
                    points->push_back(new GeoLib::Point(llf[0] + i * dx,
                                                        llf[1] + j * dy,
                                                        llf[2] + (k + 1) * dz));
                    points->push_back(new GeoLib::Point(llf[0] + i * dx,
                                                        llf[1] + (j + 1) * dy,
                                                        llf[2] + (k + 1) * dz));
                    points->push_back(new GeoLib::Point(llf[0] + (i + 1) * dx,
                                                        llf[1] + (j + 1) * dy,
                                                        llf[2] + (k + 1) * dz));
                    points->push_back(new GeoLib::Point(llf[0] + (i + 1) * dx,
                                                        llf[1] + j * dy,
                                                        llf[2] + (k + 1) * dz));
                    geo_obj->addPointVec(std::move(points), grid_names.back(),
                                         nullptr);
                }
 
                auto plys = std::make_unique<std::vector<GeoLib::Polyline*>>();
                auto const& points = *geo_obj->getPointVec(grid_names.back());
                auto* ply0(new GeoLib::Polyline(points));
 
                for (std::size_t l(0); l < 4; l++)
                    ply0->addPoint(l);
                ply0->addPoint(0);
                plys->push_back(ply0);
 
                auto* ply1(new GeoLib::Polyline(points));
                for (std::size_t l(4); l < 8; l++)
                    ply1->addPoint(l);
                ply1->addPoint(4);
                plys->push_back(ply1);
 
                auto* ply2(new GeoLib::Polyline(points));
                ply2->addPoint(0);
                ply2->addPoint(4);
                plys->push_back(ply2);
 
                auto* ply3(new GeoLib::Polyline(points));
                ply3->addPoint(1);
                ply3->addPoint(5);
                plys->push_back(ply3);
 
                auto* ply4(new GeoLib::Polyline(points));
                ply4->addPoint(2);
                ply4->addPoint(6);
                plys->push_back(ply4);
 
                auto* ply5(new GeoLib::Polyline(points));
                ply5->addPoint(3);
                ply5->addPoint(7);
                plys->push_back(ply5);
 
                geo_obj->addPolylineVec(std::move(plys), grid_names.back(),
                                        nullptr);
            }
        }
    }
    std::string merged_geo_name("Grid");
 
    geo_obj->mergeGeometries(grid_names, merged_geo_name);
}
#endif
 
template <typename POINT>
template <typename T>
std::array<std::size_t, 3> Grid<POINT>::getGridCoords(T const& pnt) const
{
    auto const& min_point{getMinPoint()};
    auto const& max_point{getMinPoint()};
    std::array<std::size_t, 3> coords{0, 0, 0};
    for (std::size_t k(0); k < 3; k++)
    {
        if (pnt[k] < min_point[k])
        {
            continue;
        }
        if (pnt[k] >= max_point[k])
        {
            coords[k] = _n_steps[k] - 1;
            continue;
        }
        coords[k] = static_cast<std::size_t>(
            std::floor((pnt[k] - min_point[k])) /
            std::nextafter(_step_sizes[k], std::numeric_limits<double>::max()));
    }
    return coords;
}
 
template <typename POINT>
template <typename P>
std::array<double, 6> Grid<POINT>::getPointCellBorderDistances(
    P const& pnt, std::array<std::size_t, 3> const& coords) const
{
    auto const& min_point{getMinPoint()};
    std::array<double, 6> dists{};
    dists[0] =
        std::abs(pnt[2] - min_point[2] + coords[2] * _step_sizes[2]);  // bottom
    dists[5] = std::abs(pnt[2] - min_point[2] +
                        (coords[2] + 1) * _step_sizes[2]);  // top
 
    dists[1] =
        std::abs(pnt[1] - min_point[1] + coords[1] * _step_sizes[1]);  // front
    dists[3] = std::abs(pnt[1] - min_point[1] +
                        (coords[1] + 1) * _step_sizes[1]);  // back
 
    dists[4] =
        std::abs(pnt[0] - min_point[0] + coords[0] * _step_sizes[0]);  // left
    dists[2] = std::abs(pnt[0] - min_point[0] +
                        (coords[0] + 1) * _step_sizes[0]);  // right
    return dists;
}
 
template <typename POINT>
template <typename P>
POINT* Grid<POINT>::getNearestPoint(P const& pnt) const
{
    std::array<std::size_t, 3> coords(getGridCoords(pnt));
    auto const& min_point{getMinPoint()};
    auto const& max_point{getMaxPoint()};
 
    double sqr_min_dist = (max_point - min_point).squaredNorm();
    POINT* nearest_pnt(nullptr);
 
    std::array<double, 6> dists(getPointCellBorderDistances(pnt, coords));
 
    if (calcNearestPointInGridCell(pnt, coords, sqr_min_dist, nearest_pnt))
    {
        double min_dist(std::sqrt(sqr_min_dist));
        if (dists[0] >= min_dist && dists[1] >= min_dist &&
            dists[2] >= min_dist && dists[3] >= min_dist &&
            dists[4] >= min_dist && dists[5] >= min_dist)
        {
            return nearest_pnt;
        }
    }
    else
    {
        // search in all border cells for at least one neighbor
        double sqr_min_dist_tmp;
        POINT* nearest_pnt_tmp(nullptr);
        std::size_t offset(1);
 
        while (nearest_pnt == nullptr)
        {
            std::array<std::size_t, 3> ijk{
                {coords[0] < offset ? 0 : coords[0] - offset,
                 coords[1] < offset ? 0 : coords[1] - offset,
                 coords[2] < offset ? 0 : coords[2] - offset}};
            for (; ijk[0] < coords[0] + offset; ijk[0]++)
            {
                for (; ijk[1] < coords[1] + offset; ijk[1]++)
                {
                    for (; ijk[2] < coords[2] + offset; ijk[2]++)
                    {
                        // do not check the origin grid cell twice
                        if (ijk[0] == coords[0] && ijk[1] == coords[1] &&
                            ijk[2] == coords[2])
                        {
                            continue;
                        }
                        // check if temporary grid cell coordinates are valid
                        if (ijk[0] >= _n_steps[0] || ijk[1] >= _n_steps[1] ||
                            ijk[2] >= _n_steps[2])
                        {
                            continue;
                        }
 
                        if (calcNearestPointInGridCell(
                                pnt, ijk, sqr_min_dist_tmp, nearest_pnt_tmp))
                        {
                            if (sqr_min_dist_tmp < sqr_min_dist)
                            {
                                sqr_min_dist = sqr_min_dist_tmp;
                                nearest_pnt = nearest_pnt_tmp;
                            }
                        }
                    }  // end k
                }      // end j
            }          // end i
            offset++;
        }  // end while
    }      // end else
 
    auto to_eigen = [](auto const& point)
    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
 
    double len((to_eigen(pnt) - to_eigen(*nearest_pnt)).norm());
    // search all other grid cells within the cube with the edge nodes
    std::vector<std::vector<POINT*> const*> vecs_of_pnts(
        getPntVecsOfGridCellsIntersectingCube(pnt, len));
 
    const std::size_t n_vecs(vecs_of_pnts.size());
    for (std::size_t j(0); j < n_vecs; j++)
    {
        std::vector<POINT*> const& pnts(*(vecs_of_pnts[j]));
        const std::size_t n_pnts(pnts.size());
        for (std::size_t k(0); k < n_pnts; k++)
        {
            const double sqr_dist(
                (to_eigen(pnt) - to_eigen(*pnts[k])).squaredNorm());
            if (sqr_dist < sqr_min_dist)
            {
                sqr_min_dist = sqr_dist;
                nearest_pnt = pnts[k];
            }
        }
    }
 
    return nearest_pnt;
}
 
template <typename POINT>
void Grid<POINT>::initNumberOfSteps(std::size_t n_per_cell, std::size_t n_pnts,
                                    std::array<double, 3> const& extensions)
{
    double const max_extension(
        *std::max_element(extensions.cbegin(), extensions.cend()));
 
    std::bitset<3> dim;  // all bits set to zero
    for (std::size_t k(0); k < 3; ++k)
    {
        // set dimension if the ratio kth-extension/max_extension >= 1e-4
        if (extensions[k] >= 1e-4 * max_extension)
        {
            dim[k] = true;
        }
    }
 
    // structured grid: n_cells = _n_steps[0] * _n_steps[1] * _n_steps[2]
    // *** condition: n_pnts / n_cells < n_per_cell
    // => n_pnts / n_per_cell < n_cells
    // _n_steps[1] = _n_steps[0] * extensions[1]/extensions[0],
    // _n_steps[2] = _n_steps[0] * extensions[2]/extensions[0],
    // => n_cells = _n_steps[0]^3 * extensions[1]/extensions[0] *
    //              extensions[2]/extensions[0],
    // => _n_steps[0] = cbrt(n_cells * extensions[0]^2 /
    //                       (extensions[1]*extensions[2]))
    auto sc_ceil = [](double v) { return static_cast<std::size_t>(ceil(v)); };
 
    switch (dim.count())
    {
        case 3:  // 3d case
            _n_steps[0] = sc_ceil(
                std::cbrt(n_pnts * (extensions[0] / extensions[1]) *
                          (extensions[0] / extensions[2]) / n_per_cell));
            _n_steps[1] = sc_ceil(
                _n_steps[0] * std::min(extensions[1] / extensions[0], 100.0));
            _n_steps[2] = sc_ceil(
                _n_steps[0] * std::min(extensions[2] / extensions[0], 100.0));
            break;
        case 2:  // 2d cases
            if (dim[0] && dim[1])
            {  // xy
                _n_steps[0] = sc_ceil(std::sqrt(n_pnts * extensions[0] /
                                                (n_per_cell * extensions[1])));
                _n_steps[1] =
                    sc_ceil(_n_steps[0] *
                            std::min(extensions[1] / extensions[0], 100.0));
            }
            else if (dim[0] && dim[2])
            {  // xz
                _n_steps[0] = sc_ceil(std::sqrt(n_pnts * extensions[0] /
                                                (n_per_cell * extensions[2])));
                _n_steps[2] =
                    sc_ceil(_n_steps[0] *
                            std::min(extensions[2] / extensions[0], 100.0));
            }
            else if (dim[1] && dim[2])
            {  // yz
                _n_steps[1] = sc_ceil(std::sqrt(n_pnts * extensions[1] /
                                                (n_per_cell * extensions[2])));
                _n_steps[2] =
                    sc_ceil(std::min(extensions[2] / extensions[1], 100.0));
            }
            break;
        case 1:  // 1d cases
            for (std::size_t k(0); k < 3; ++k)
            {
                if (dim[k])
                {
                    _n_steps[k] =
                        sc_ceil(static_cast<double>(n_pnts) / n_per_cell);
                }
            }
    }
}
 
template <typename POINT>
template <typename P>
bool Grid<POINT>::calcNearestPointInGridCell(
    P const& pnt,
    std::array<std::size_t, 3> const& coords,
    double& sqr_min_dist,
    POINT*& nearest_pnt) const
{
    const std::size_t grid_idx(coords[0] + coords[1] * _n_steps[0] +
                               coords[2] * _n_steps[0] * _n_steps[1]);
    std::vector<typename std::add_pointer<
        typename std::remove_pointer<POINT>::type>::type> const&
        pnts(_grid_cell_nodes_map[grid_idx]);
    if (pnts.empty())
        return false;
 
    auto to_eigen = [](auto const& point)
    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
 
    const std::size_t n_pnts(pnts.size());
    sqr_min_dist = (to_eigen(*pnts[0]) - to_eigen(pnt)).squaredNorm();
    nearest_pnt = pnts[0];
    for (std::size_t i(1); i < n_pnts; i++)
    {
        const double sqr_dist(
            (to_eigen(*pnts[i]) - to_eigen(pnt)).squaredNorm());
        if (sqr_dist < sqr_min_dist)
        {
            sqr_min_dist = sqr_dist;
            nearest_pnt = pnts[i];
        }
    }
    return true;
}
 
template <typename POINT>
template <typename P>
std::vector<std::size_t> Grid<POINT>::getPointsInEpsilonEnvironment(
    P const& pnt, double eps) const
{
    std::vector<std::vector<POINT*> const*> vec_pnts(
        getPntVecsOfGridCellsIntersectingCube(pnt, eps));
 
    double const sqr_eps(eps * eps);
 
    auto to_eigen = [](auto const& point)
    { return Eigen::Map<Eigen::Vector3d const>(point.getCoords()); };
 
    std::vector<std::size_t> pnts;
    for (auto vec : vec_pnts)
    {
        for (auto const p : *vec)
        {
            if ((to_eigen(*p) - to_eigen(pnt)).squaredNorm() <= sqr_eps)
            {
                pnts.push_back(p->getID());
            }
        }
    }
 
    return pnts;
}
 
}  // end namespace GeoLib