OctTree-impl.h

Go to the documentation of this file.

 
namespace GeoLib
{
template <typename POINT, std::size_t MAX_POINTS>
OctTree<POINT, MAX_POINTS>* OctTree<POINT, MAX_POINTS>::createOctTree(
    Eigen::Vector3d ll, Eigen::Vector3d ur, double eps)
{
    // compute an axis aligned cube around the points ll and ur
    const double dx(ur[0] - ll[0]);
    const double dy(ur[1] - ll[1]);
    const double dz(ur[2] - ll[2]);
    if (dx >= dy && dx >= dz)
    {
        ll[1] -= (dx - dy) / 2.0;
        ur[1] += (dx - dy) / 2.0;
        ll[2] -= (dx - dz) / 2.0;
        ur[2] += (dx - dz) / 2.0;
    }
    else
    {
        if (dy >= dx && dy >= dz)
        {
            ll[0] -= (dy - dx) / 2.0;
            ur[0] += (dy - dx) / 2.0;
            ll[2] -= (dy - dz) / 2.0;
            ur[2] += (dy - dz) / 2.0;
        }
        else
        {
            ll[0] -= (dz - dx) / 2.0;
            ur[0] += (dz - dx) / 2.0;
            ll[1] -= (dz - dy) / 2.0;
            ur[1] += (dz - dy) / 2.0;
        }
    }
    if (eps == 0.0)
    {
        eps = std::numeric_limits<double>::epsilon();
    }
    for (std::size_t k(0); k < 3; ++k)
    {
        if (ur[k] - ll[k] > 0.0)
        {
            ur[k] += (ur[k] - ll[k]) * 1e-6;
        }
        else
        {
            ur[k] += eps;
        }
    }
    Eigen::Vector3d lower_left{ll[0], ll[1], ll[2]};
    Eigen::Vector3d upper_right{ur[0], ur[1], ur[2]};
    return new OctTree<POINT, MAX_POINTS>(lower_left, upper_right, eps);
}
 
template <typename POINT, std::size_t MAX_POINTS>
OctTree<POINT, MAX_POINTS>::~OctTree()
{
    for (auto c : _children)
    {
        delete c;
    }
}
 
template <typename POINT, std::size_t MAX_POINTS>
bool OctTree<POINT, MAX_POINTS>::addPoint(POINT* pnt, POINT*& ret_pnt)
{
    // first do a range query using a small box around the point pnt
    std::vector<POINT*> query_pnts;
    Eigen::Vector3d const min = pnt->asEigenVector3d().array() - _eps;
    Eigen::Vector3d const max = {
        std::abs(((*pnt)[0] + _eps) - (*pnt)[0]) > 0.0
            ? (*pnt)[0] + _eps
            : std::nextafter((*pnt)[0],
                             std::numeric_limits<double>::infinity()),
        std::abs(((*pnt)[1] + _eps) - (*pnt)[1]) > 0.0
            ? (*pnt)[1] + _eps
            : std::nextafter((*pnt)[1],
                             std::numeric_limits<double>::infinity()),
        std::abs(((*pnt)[2] + _eps) - (*pnt)[2]) > 0.0
            ? (*pnt)[2] + _eps
            : std::nextafter((*pnt)[2],
                             std::numeric_limits<double>::infinity())};
    getPointsInRange(min, max, query_pnts);
    auto const it =
        std::find_if(query_pnts.begin(), query_pnts.end(),
                     [pnt, this](auto const* p)
                     {
                         return (p->asEigenVector3d() - pnt->asEigenVector3d())
                                    .squaredNorm() < _eps * _eps;
                     });
    if (it != query_pnts.end())
    {
        ret_pnt = *it;
        return false;
    }
 
    // the point pnt is not yet in the OctTree
    if (isOutside(pnt))
    {
        ret_pnt = nullptr;
        return false;
    }
 
    // at this place it holds true that the point is within [_ll, _ur]
    if (!_is_leaf)
    {
        for (auto c : _children)
        {
            if (c->addPoint_(pnt, ret_pnt))
            {
                return true;
            }
            if (ret_pnt != nullptr)
            {
                return false;
            }
        }
    }
 
    ret_pnt = pnt;
 
    if (_pnts.size() < MAX_POINTS)
    {
        _pnts.push_back(pnt);
    }
    else
    {  // i.e. _pnts.size () == MAX_POINTS
        splitNode(pnt);
        _pnts.clear();
    }
    return true;
}
 
template <typename POINT, std::size_t MAX_POINTS>
template <typename T>
void OctTree<POINT, MAX_POINTS>::getPointsInRange(
    T const& min, T const& max, std::vector<POINT*>& pnts) const
{
    if (max[0] == min[0] || max[1] == min[1] || max[2] == min[2])
    {
        ERR("The search range [{}, {}) x [{}, {}) x [{}, {}) is empty.", min[0],
            max[0], min[1], max[1], min[2], max[2]);
        return;
    }
 
    return getPointsInRange_(min, max, pnts);
}
 
template <typename POINT, std::size_t MAX_POINTS>
template <typename T>
void OctTree<POINT, MAX_POINTS>::getPointsInRange_(
    T const& min, T const& max, std::vector<POINT*>& pnts) const
{
    if (_ur[0] < min[0] || _ur[1] < min[1] || _ur[2] < min[2])
    {
        return;
    }
 
    if (max[0] < _ll[0] || max[1] < _ll[1] || max[2] < _ll[2])
    {
        return;
    }
 
    if (_is_leaf)
    {
        std::copy_if(_pnts.begin(), _pnts.end(), std::back_inserter(pnts),
                     [&min, &max](auto const* p)
                     {
                         return (min[0] <= (*p)[0] && (*p)[0] < max[0] &&
                                 min[1] <= (*p)[1] && (*p)[1] < max[1] &&
                                 min[2] <= (*p)[2] && (*p)[2] < max[2]);
                     });
    }
    else
    {
        for (std::size_t k(0); k < 8; k++)
        {
            _children[k]->getPointsInRange_(min, max, pnts);
        }
    }
}
 
template <typename POINT, std::size_t MAX_POINTS>
OctTree<POINT, MAX_POINTS>::OctTree(Eigen::Vector3d const& ll,
                                    Eigen::Vector3d const& ur, double eps)
    : _ll(ll), _ur(ur), _is_leaf(true), _eps(eps)
{
    _children.fill(nullptr);
}
 
template <typename POINT, std::size_t MAX_POINTS>
bool OctTree<POINT, MAX_POINTS>::addPoint_(POINT* pnt, POINT*& ret_pnt)
{
    if (isOutside(pnt))
    {
        ret_pnt = nullptr;
        return false;
    }
 
    // at this place it holds true that the point is within [_ll, _ur]
    if (!_is_leaf)
    {
        for (auto c : _children)
        {
            if (c->addPoint_(pnt, ret_pnt))
            {
                return true;
            }
            if (ret_pnt != nullptr)
            {
                return false;
            }
        }
    }
 
    ret_pnt = pnt;
    if (_pnts.size() < MAX_POINTS)
    {
        _pnts.push_back(pnt);
    }
    else
    {  // i.e. _pnts.size () == MAX_POINTS
        splitNode(pnt);
        _pnts.clear();
    }
    return true;
}
 
template <typename POINT, std::size_t MAX_POINTS>
bool OctTree<POINT, MAX_POINTS>::addPointToChild(POINT* pnt)
{
    if (isOutside(pnt))
    {
        return false;
    }
 
    if (_pnts.size() < MAX_POINTS)
    {
        _pnts.push_back(pnt);
    }
    else
    {  // i.e. _pnts.size () == MAX_POINTS
        splitNode(pnt);
        _pnts.clear();
    }
    return true;
}
 
template <typename POINT, std::size_t MAX_POINTS>
void OctTree<POINT, MAX_POINTS>::splitNode(POINT* pnt)
{
    const double x_mid((_ur[0] + _ll[0]) / 2.0);
    const double y_mid((_ur[1] + _ll[1]) / 2.0);
    const double z_mid((_ur[2] + _ll[2]) / 2.0);
    Eigen::Vector3d p0{x_mid, y_mid, _ll[2]};
    Eigen::Vector3d p1{_ur[0], _ur[1], z_mid};
 
    // create child NEL
    _children[static_cast<std::int8_t>(Quadrant::NEL)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // create child NWL
    p0[0] = _ll[0];
    p1[0] = x_mid;
    _children[static_cast<std::int8_t>(Quadrant::NWL)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // create child SWL
    p0[1] = _ll[1];
    p1[1] = y_mid;
    _children[static_cast<std::int8_t>(Quadrant::SWL)] =
        new OctTree<POINT, MAX_POINTS>(_ll, p1, _eps);
 
    // create child NEU
    _children[static_cast<std::int8_t>(Quadrant::NEU)] =
        new OctTree<POINT, MAX_POINTS>(p1, _ur, _eps);
 
    // create child SEL
    p0[0] = x_mid;
    p1[0] = _ur[0];
    _children[static_cast<std::int8_t>(Quadrant::SEL)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // create child NWU
    p0[0] = _ll[0];
    p0[1] = y_mid;
    p0[2] = z_mid;
    p1[0] = x_mid;
    p1[1] = _ur[1];
    p1[2] = _ur[2];
    _children[static_cast<std::int8_t>(Quadrant::NWU)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // create child SWU
    p0[1] = _ll[1];
    p1[1] = y_mid;
    _children[static_cast<std::int8_t>(Quadrant::SWU)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // create child SEU
    p0[0] = x_mid;
    p1[0] = _ur[0];
    p1[1] = y_mid;
    p1[2] = _ur[2];
    _children[static_cast<std::int8_t>(Quadrant::SEU)] =
        new OctTree<POINT, MAX_POINTS>(p0, p1, _eps);
 
    // add the passed point pnt to the children at first
    for (std::size_t k(0); k < 8; k++)
    {
        if (_children[k]->addPointToChild(pnt))
        {
            break;
        }
    }
 
    // distribute points to sub quadtrees
    const std::size_t n_pnts(_pnts.size());
    for (std::size_t j(0); j < n_pnts; j++)
    {
        if (std::any_of(begin(_children), end(_children),
                        [&](auto* child)
                        { return child->addPointToChild(_pnts[j]); }))
        {
            continue;
        }
    }
    _is_leaf = false;
}
 
template <typename POINT, std::size_t MAX_POINTS>
bool OctTree<POINT, MAX_POINTS>::isOutside(POINT* pnt) const
{
    if ((*pnt)[0] < _ll[0] || (*pnt)[1] < _ll[1] || (*pnt)[2] < _ll[2])
    {
        return true;
    }
    if ((*pnt)[0] >= _ur[0] || (*pnt)[1] >= _ur[1] || (*pnt)[2] >= _ur[2])
    {
        return true;
    }
    return false;
}
}  // end namespace GeoLib