DgmOctree.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include <DgmOctree.h>
 
// local
#include <CVMiscTools.h>
#include <GenericProgressCallback.h>
#include <ParallelSort.h>
#include <RayAndBox.h>
#include <ReferenceCloud.h>
#include <ScalarField.h>
 
// system
#include <cstdio>
#include <set>
 
// DGM: tests in progress
// #define COMPUTE_NN_SEARCH_STATISTICS
// #define ADAPTATIVE_BINARY_SEARCH
 
#ifdef CV_CORE_LIB_USES_QT_CONCURRENT
#ifndef CV_DEBUG
// enables multi-threading handling
#define ENABLE_MT_OCTREE
 
#include <QThreadPool>
#include <QtConcurrentMap>
#include <QtCore>
#endif
#endif
 
using namespace cloudViewer;
 
/**********************************/
/* PRE COMPUTED VALUES AND TABLES */
/**********************************/
 
static const double LOG_NAT_2 = log(2.0);
 
struct BitShiftValues {
    BitShiftValues() {
        // we compute all possible values
        for (unsigned char level = 0; level <= DgmOctree::MAX_OCTREE_LEVEL;
             ++level) {
            values[level] =
                    (3 * (cloudViewer::DgmOctree::MAX_OCTREE_LEVEL - level));
        }
    }
 
    unsigned char values[DgmOctree::MAX_OCTREE_LEVEL + 1];
};
static BitShiftValues PRE_COMPUTED_BIT_SHIFT_VALUES;
 
struct MonoDimensionalCellCodes {
 
    static const int VALUE_COUNT = cloudViewer::DgmOctree::MAX_OCTREE_LENGTH;
 
    MonoDimensionalCellCodes() {
        // we compute all possible values for cell codes
        //(along a unique dimension, the other ones are just shifted)
        for (int value = 0; value < VALUE_COUNT; ++value) {
            int mask = VALUE_COUNT;
            cloudViewer::DgmOctree::CellCode code = 0;
            for (unsigned char k = 0;
                 k < cloudViewer::DgmOctree::MAX_OCTREE_LEVEL; k++) {
                mask >>= 1;
                code <<= 3;
                if (value & mask) {
                    code |= 1;
                }
            }
            values[value] = code;
        }
 
        // we compute all possible masks as well! (all dimensions)
        // cloudViewer::DgmOctree::CellCode baseMask = (1 << (3 *
        // cloudViewer::DgmOctree::MAX_OCTREE_LEVEL)); for (int level =
        // cloudViewer::DgmOctree::MAX_OCTREE_LEVEL; level >= 0; --level)
        //{
        //  masks[level] = baseMask - 1;
        //  baseMask >>= 3;
        // }
    }
 
    cloudViewer::DgmOctree::CellCode values[VALUE_COUNT];
 
    // cloudViewer::DgmOctree::CellCode
    // masks[cloudViewer::DgmOctree::MAX_OCTREE_LEVEL + 1];
};
static MonoDimensionalCellCodes PRE_COMPUTED_POS_CODES;
 
/**********************************/
/*        STATIC ACCESSORS        */
/**********************************/
 
unsigned char DgmOctree::GET_BIT_SHIFT(unsigned char level) {
    // return (3 * (cloudViewer::DgmOctree::MAX_OCTREE_LEVEL - level));
    return PRE_COMPUTED_BIT_SHIFT_VALUES.values[level];
}
 
int DgmOctree::OCTREE_LENGTH(int level) { return (1 << level); }
 
DgmOctree::CellCode DgmOctree::GenerateTruncatedCellCode(const Tuple3i& cellPos,
                                                         unsigned char level) {
    assert(cellPos.x >= 0 &&
           cellPos.x < MonoDimensionalCellCodes::VALUE_COUNT &&
           cellPos.y >= 0 &&
           cellPos.y < MonoDimensionalCellCodes::VALUE_COUNT &&
           cellPos.z >= 0 && cellPos.z < MonoDimensionalCellCodes::VALUE_COUNT);
 
    const unsigned char dec = MAX_OCTREE_LEVEL - level;
 
    return (PRE_COMPUTED_POS_CODES.values[cellPos.x << dec] |
            (PRE_COMPUTED_POS_CODES.values[cellPos.y << dec] << 1) |
            (PRE_COMPUTED_POS_CODES.values[cellPos.z << dec] << 2)
 
                    ) >>
           GET_BIT_SHIFT(level);
}
 
#ifndef OCTREE_CODES_64_BITS
DgmOctree::CellCode DgmOctree::GenerateTruncatedCellCode(const Tuple3s& cellPos,
                                                         unsigned char level) {
    assert(cellPos.x >= 0 &&
           cellPos.x < MonoDimensionalCellCodes::VALUE_COUNT &&
           cellPos.y >= 0 &&
           cellPos.y < MonoDimensionalCellCodes::VALUE_COUNT &&
           cellPos.z >= 0 && cellPos.z < MonoDimensionalCellCodes::VALUE_COUNT);
 
    const unsigned char dec = MAX_OCTREE_LEVEL - level;
 
    return (PRE_COMPUTED_POS_CODES.values[cellPos.x << dec] |
            (PRE_COMPUTED_POS_CODES.values[cellPos.y << dec] << 1) |
            (PRE_COMPUTED_POS_CODES.values[cellPos.z << dec] << 2)
 
                    ) >>
           GET_BIT_SHIFT(level);
}
#endif
 
static inline DgmOctree::CellCode GenerateCellCodeForDim(int pos) {
    return PRE_COMPUTED_POS_CODES.values[pos];
}
 
bool DgmOctree::MultiThreadSupport() {
#ifdef ENABLE_MT_OCTREE
    return true;
#else
    return false;
#endif
}
 
/**********************************/
/*        EVERYTHING ELSE!        */
/**********************************/
 
DgmOctree::DgmOctree(GenericIndexedCloudPersist* cloud)
    : m_theAssociatedCloud(cloud),
      m_numberOfProjectedPoints(0),
      m_nearestPow2(0) {
    clear();
 
    assert(m_theAssociatedCloud);
}
 
void DgmOctree::clear() {
    // reset internal tables
    m_dimMin = m_pointsMin = m_dimMax = m_pointsMax = CCVector3(0, 0, 0);
 
    m_numberOfProjectedPoints = 0;
    m_nearestPow2 = 0;
    m_thePointsAndTheirCellCodes.resize(0);
 
    memset(m_fillIndexes, 0, sizeof(int) * (MAX_OCTREE_LEVEL + 1) * 6);
    memset(m_cellSize, 0, sizeof(PointCoordinateType) * (MAX_OCTREE_LEVEL + 2));
    updateCellCountTable();
}
 
int DgmOctree::build(GenericProgressCallback* progressCb) {
    if (!m_thePointsAndTheirCellCodes.empty()) clear();
 
    updateMinAndMaxTables();
 
    return genericBuild(progressCb);
}
 
int DgmOctree::build(const CCVector3& octreeMin,
                     const CCVector3& octreeMax,
                     const CCVector3* pointsMinFilter /*=0*/,
                     const CCVector3* pointsMaxFilter /*=0*/,
                     GenericProgressCallback* progressCb /*=0*/) {
    if (!m_thePointsAndTheirCellCodes.empty()) clear();
 
    m_dimMin = octreeMin;
    m_dimMax = octreeMax;
 
    // the user can specify boundaries for points different than the octree box!
    m_pointsMin = (pointsMinFilter ? *pointsMinFilter : m_dimMin);
    m_pointsMax = (pointsMaxFilter ? *pointsMaxFilter : m_dimMax);
 
    return genericBuild(progressCb);
}
 
int DgmOctree::genericBuild(GenericProgressCallback* progressCb) {
    unsigned pointCount =
            (m_theAssociatedCloud ? m_theAssociatedCloud->size() : 0);
    if (pointCount == 0) {
        // no cloud/point?!
        return -1;
    }
 
    // allocate memory
    try {
        m_thePointsAndTheirCellCodes.resize(
                pointCount);  // resize + operator[] is faster than reserve +
                              // push_back!
    } catch (... /*const std::bad_alloc&*/)  // out of memory
    {
        return -1;
    }
 
    m_numberOfProjectedPoints = 0;
    m_nearestPow2 = 0;
 
    // update the pre-computed 'cell size per level of subdivision' array
    updateCellSizeTable();
 
    // progress notification (optional)
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Build Octree");
            char infosBuffer[256];
            sprintf(infosBuffer, "Projecting %u points\nMax. depth: %i",
                    pointCount, MAX_OCTREE_LEVEL);
            progressCb->setInfo(infosBuffer);
        }
        progressCb->update(0);
        progressCb->start();
    }
    NormalizedProgress nprogress(
            progressCb, pointCount,
            90);  // first phase: 90% (we keep 10% for sort)
 
    // fill indexes table (we'll fill the max. level, then deduce the others
    // from this one)
    int* fillIndexesAtMaxLevel = m_fillIndexes + (MAX_OCTREE_LEVEL * 6);
 
    // for all points
    cellsContainer::iterator it = m_thePointsAndTheirCellCodes.begin();
    for (unsigned i = 0; i < pointCount; i++) {
        const CCVector3* P = m_theAssociatedCloud->getPoint(i);
 
        // does the point falls in the 'accepted points' box?
        //(potentially different from the octree box - see DgmOctree::build)
        if ((P->x >= m_pointsMin[0]) && (P->x <= m_pointsMax[0]) &&
            (P->y >= m_pointsMin[1]) && (P->y <= m_pointsMax[1]) &&
            (P->z >= m_pointsMin[2]) && (P->z <= m_pointsMax[2])) {
            // compute the position of the cell that includes this point
            Tuple3i cellPos;
            getTheCellPosWhichIncludesThePoint(P, cellPos);
 
            // clipping X
            if (cellPos.x < 0)
                cellPos.x = 0;
            else if (cellPos.x >= MAX_OCTREE_LENGTH)
                cellPos.x = MAX_OCTREE_LENGTH - 1;
            // clipping Y
            if (cellPos.y < 0)
                cellPos.y = 0;
            else if (cellPos.y >= MAX_OCTREE_LENGTH)
                cellPos.y = MAX_OCTREE_LENGTH - 1;
            // clipping Z
            if (cellPos.z < 0)
                cellPos.z = 0;
            else if (cellPos.z >= MAX_OCTREE_LENGTH)
                cellPos.z = MAX_OCTREE_LENGTH - 1;
 
            it->theIndex = i;
            it->theCode = GenerateTruncatedCellCode(cellPos, MAX_OCTREE_LEVEL);
 
            if (m_numberOfProjectedPoints) {
                if (fillIndexesAtMaxLevel[0] > cellPos.x)
                    fillIndexesAtMaxLevel[0] = cellPos.x;
                else if (fillIndexesAtMaxLevel[3] < cellPos.x)
                    fillIndexesAtMaxLevel[3] = cellPos.x;
 
                if (fillIndexesAtMaxLevel[1] > cellPos.y)
                    fillIndexesAtMaxLevel[1] = cellPos.y;
                else if (fillIndexesAtMaxLevel[4] < cellPos.y)
                    fillIndexesAtMaxLevel[4] = cellPos.y;
 
                if (fillIndexesAtMaxLevel[2] > cellPos.z)
                    fillIndexesAtMaxLevel[2] = cellPos.z;
                else if (fillIndexesAtMaxLevel[5] < cellPos.z)
                    fillIndexesAtMaxLevel[5] = cellPos.z;
            } else {
                fillIndexesAtMaxLevel[0] = fillIndexesAtMaxLevel[3] = cellPos.x;
                fillIndexesAtMaxLevel[1] = fillIndexesAtMaxLevel[4] = cellPos.y;
                fillIndexesAtMaxLevel[2] = fillIndexesAtMaxLevel[5] = cellPos.z;
            }
 
            ++it;
            ++m_numberOfProjectedPoints;
        }
 
        if (!nprogress.oneStep()) {
            m_thePointsAndTheirCellCodes.resize(0);
            m_numberOfProjectedPoints = 0;
            if (progressCb) {
                progressCb->stop();
            }
            return 0;
        }
    }
 
    // we deduce the lower levels 'fill indexes' from the highest level
    {
        for (int k = MAX_OCTREE_LEVEL - 1; k >= 0; k--) {
            int* fillIndexes = m_fillIndexes + (k * 6);
            for (int dim = 0; dim < 6; ++dim) {
                fillIndexes[dim] = (fillIndexes[dim + 6] >> 1);
            }
        }
    }
 
    if (m_numberOfProjectedPoints < pointCount)
        m_thePointsAndTheirCellCodes.resize(
                m_numberOfProjectedPoints);  // smaller --> should always be ok
 
    if (progressCb && progressCb->textCanBeEdited()) {
        progressCb->setInfo("Sorting cells...");
    }
 
    // we sort the 'cells' by ascending code order
    ParallelSort(m_thePointsAndTheirCellCodes.begin(),
                 m_thePointsAndTheirCellCodes.end(), IndexAndCode::codeComp);
 
    // update the pre-computed 'number of cells per level of subdivision' array
    updateCellCountTable();
 
    // end of process notification
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            char buffer[256];
            if (m_numberOfProjectedPoints == pointCount) {
                sprintf(buffer,
                        "[Octree::build] Octree successfully built... %u "
                        "points (ok)!",
                        m_numberOfProjectedPoints);
            } else {
                if (m_numberOfProjectedPoints == 0)
                    sprintf(buffer,
                            "[Octree::build] Warning : no point projected in "
                            "the Octree!");
                else
                    sprintf(buffer,
                            "[Octree::build] Warning: some points have been "
                            "filtered out (%u/%u)",
                            pointCount - m_numberOfProjectedPoints, pointCount);
            }
            progressCb->setInfo(buffer);
        }
 
        // DGM: the dialog may close itself once we set the progress to 100%
        // (hiding the above information!)
        progressCb->update(100.0f);
        progressCb->stop();
    }
 
    m_nearestPow2 =
            (1 << static_cast<int>(
                     log(static_cast<double>(m_numberOfProjectedPoints - 1)) /
                     LOG_NAT_2));
 
    return static_cast<int>(m_numberOfProjectedPoints);
}
 
void DgmOctree::updateMinAndMaxTables() {
    if (!m_theAssociatedCloud) return;
 
    m_theAssociatedCloud->getBoundingBox(m_pointsMin, m_pointsMax);
    m_dimMin = m_pointsMin;
    m_dimMax = m_pointsMax;
 
    CCMiscTools::MakeMinAndMaxCubical(m_dimMin, m_dimMax);
}
 
void DgmOctree::updateCellSizeTable() {
    // update the cell dimension for each subdivision level
    m_cellSize[0] = m_dimMax.x - m_dimMin.x;
 
    unsigned long long d = 1;
    for (int k = 1; k <= MAX_OCTREE_LEVEL; k++) {
        d <<= 1;
        m_cellSize[k] = m_cellSize[0] / d;
    }
}
 
void DgmOctree::updateCellCountTable() {
    // level 0 is just the octree bounding-box
    for (unsigned char i = 0; i <= MAX_OCTREE_LEVEL; ++i) {
        computeCellsStatistics(i);
    }
}
 
void DgmOctree::computeCellsStatistics(unsigned char level) {
    assert(level <= MAX_OCTREE_LEVEL);
 
    // empty octree case?!
    if (m_thePointsAndTheirCellCodes.empty()) {
        // DGM: we make as if there were 1 point to avoid some degenerated
        // cases!
        m_cellCount[level] = 1;
        m_maxCellPopulation[level] = 1;
        m_averageCellPopulation[level] = 1.0;
        m_stdDevCellPopulation[level] = 0.0;
        return;
    }
 
    // level '0' specific case
    if (level == 0) {
        m_cellCount[level] = 1;
        m_maxCellPopulation[level] =
                static_cast<unsigned>(m_thePointsAndTheirCellCodes.size());
        m_averageCellPopulation[level] =
                static_cast<double>(m_thePointsAndTheirCellCodes.size());
        m_stdDevCellPopulation[level] = 0.0;
        return;
    }
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(level);
 
    // iterator on octree elements
    cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
    // we init scan with first element
    CellCode predCode = (p->theCode >> bitDec);
    unsigned counter = 0;
    unsigned cellCounter = 0;
    unsigned maxCellPop = 0;
    double sum = 0.0, sum2 = 0.0;
 
    for (; p != m_thePointsAndTheirCellCodes.end(); ++p) {
        CellCode currentCode = (p->theCode >> bitDec);
        if (predCode != currentCode) {
            sum += static_cast<double>(cellCounter);
            sum2 += static_cast<double>(cellCounter) *
                    static_cast<double>(cellCounter);
 
            if (maxCellPop < cellCounter) maxCellPop = cellCounter;
 
            // new cell
            predCode = currentCode;
            cellCounter = 0;
            ++counter;
        }
        ++cellCounter;
    }
 
    // don't forget last cell!
    sum += static_cast<double>(cellCounter);
    sum2 += static_cast<double>(cellCounter) * static_cast<double>(cellCounter);
    if (maxCellPop < cellCounter) maxCellPop = cellCounter;
    ++counter;
 
    assert(counter > 0);
    m_cellCount[level] = counter;
    m_maxCellPopulation[level] = maxCellPop;
    m_averageCellPopulation[level] = sum / static_cast<double>(counter);
    m_stdDevCellPopulation[level] = sqrt(
            sum2 / static_cast<double>(counter) -
            m_averageCellPopulation[level] * m_averageCellPopulation[level]);
}
 
void DgmOctree::getBoundingBox(CCVector3& bbMin, CCVector3& bbMax) const {
    bbMin = m_dimMin;
    bbMax = m_dimMax;
}
 
void DgmOctree::getCellPos(CellCode code,
                           unsigned char level,
                           Tuple3i& cellPos,
                           bool isCodeTruncated) const {
    // binary shift for cell code truncation
    if (!isCodeTruncated) {
        code >>= GET_BIT_SHIFT(level);
    }
 
    cellPos = Tuple3i(0, 0, 0);
 
    int bitMask = 1;
    for (unsigned char k = 0; k < level; ++k) {
        if (code & 4) cellPos.z |= bitMask;
        if (code & 2) cellPos.y |= bitMask;
        if (code & 1) cellPos.x |= bitMask;
 
        code >>= 3;
        bitMask <<= 1;
    }
}
 
void DgmOctree::computeCellLimits(CellCode code,
                                  unsigned char level,
                                  CCVector3& cellMin,
                                  CCVector3& cellMax,
                                  bool isCodeTruncated) const {
    Tuple3i cellPos;
    getCellPos(code, level, cellPos, isCodeTruncated);
 
    const PointCoordinateType& cs = getCellSize(level);
 
    cellMin.x = m_dimMin[0] + cs * cellPos.x;
    cellMin.y = m_dimMin[1] + cs * cellPos.y;
    cellMin.z = m_dimMin[2] + cs * cellPos.z;
 
    cellMax = cellMin + CCVector3(cs, cs, cs);
}
 
bool DgmOctree::getPointsInCell(CellCode cellCode,
                                unsigned char level,
                                ReferenceCloud* subset,
                                bool isCodeTruncated /*=false*/,
                                bool clearOutputCloud /* = true*/) const {
    unsigned char bitDec = GET_BIT_SHIFT(level);
    if (!isCodeTruncated) {
        cellCode >>= bitDec;
    }
 
    unsigned cellIndex = getCellIndex(cellCode, bitDec);
    // check that cell exists!
    if (cellIndex < m_numberOfProjectedPoints) {
        return getPointsInCellByCellIndex(subset, cellIndex, level,
                                          clearOutputCloud);
    } else if (clearOutputCloud) {
        subset->clear();
    }
 
    return true;
}
 
unsigned DgmOctree::getCellIndex(CellCode truncatedCellCode,
                                 unsigned char bitDec) const {
    // inspired from the algorithm proposed by MATT PULVER (see
    // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/)
    // DGM: it's not faster, but the code is simpler ;)
    unsigned i = 0;
    unsigned b = m_nearestPow2;
 
    for (; b; b >>= 1) {
        unsigned j = i | b;
        if (j < m_numberOfProjectedPoints) {
            CellCode middleCode =
                    (m_thePointsAndTheirCellCodes[j].theCode >> bitDec);
            if (middleCode < truncatedCellCode) {
                // what we are looking for is on the right
                i = j;
            } else if (middleCode == truncatedCellCode) {
                // we must check that it's the first element equal to input code
                if (j == 0 || (m_thePointsAndTheirCellCodes[j - 1].theCode >>
                               bitDec) != truncatedCellCode) {
                    // what we are looking for is right here
                    return j;
                }
                // otherwise what we are looking for is on the left!
            }
        }
    }
 
    return (m_thePointsAndTheirCellCodes[i].theCode >> bitDec) ==
                           truncatedCellCode
                   ? i
                   : m_numberOfProjectedPoints;
}
 
// optimized version with profiling
#ifdef COMPUTE_NN_SEARCH_STATISTICS
static double s_jumps = 0.0;
static double s_binarySearchCount = 0.0;
#endif
 
#ifdef ADAPTATIVE_BINARY_SEARCH
unsigned DgmOctree::getCellIndex(CellCode truncatedCellCode,
                                 unsigned char bitDec,
                                 unsigned begin,
                                 unsigned end) const {
    assert(truncatedCellCode != INVALID_CELL_CODE);
    assert(end >= begin);
    assert(end < m_numberOfProjectedPoints);
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
    s_binarySearchCount += 1;
#endif
 
    // if query cell code is lower than or equal to the first octree cell code,
    // then it's either the good one or there's no match
    CellCode beginCode =
            (m_thePointsAndTheirCellCodes[begin].theCode >> bitDec);
    if (truncatedCellCode < beginCode)
        return m_numberOfProjectedPoints;
    else if (truncatedCellCode == beginCode)
        return begin;
 
    // if query cell code is higher than the last octree cell code, then there's
    // no match
    CellCode endCode = (m_thePointsAndTheirCellCodes[end].theCode >> bitDec);
    if (truncatedCellCode > endCode) return m_numberOfProjectedPoints;
 
    while (true) {
        float centralPoint =
                0.5f +
                0.75f * (static_cast<float>(truncatedCellCode - beginCode) /
                         (-0.5f));  // 0.75 = speed coef (empirical)
        unsigned middle = begin + static_cast<unsigned>(centralPoint *
                                                        float(end - begin));
        CellCode middleCode =
                (m_thePointsAndTheirCellCodes[middle].theCode >> bitDec);
 
        if (middleCode < truncatedCellCode) {
            // no more cell in-between?
            if (middle == begin) return m_numberOfProjectedPoints;
 
            begin = middle;
            beginCode = middleCode;
        } else if (middleCode > truncatedCellCode) {
            // no more cell in-between?
            if (middle == begin) return m_numberOfProjectedPoints;
 
            end = middle;
            endCode = middleCode;
        } else {
            // if the previous point doesn't correspond, then we have just found
            // the first good one!
            if ((m_thePointsAndTheirCellCodes[middle - 1].theCode >> bitDec) !=
                truncatedCellCode)
                return middle;
            end = middle;
            endCode = middleCode;
        }
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        s_jumps += 1.0;
#endif
    }
 
    // we shouldn't get there!
    return m_numberOfProjectedPoints;
}
 
#else
 
unsigned DgmOctree::getCellIndex(CellCode truncatedCellCode,
                                 unsigned char bitDec,
                                 unsigned begin,
                                 unsigned end) const {
    assert(truncatedCellCode != INVALID_CELL_CODE);
    assert(end >= begin && end < m_numberOfProjectedPoints);
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
    s_binarySearchCount += 1;
#endif
 
    // inspired from the algorithm proposed by MATT PULVER (see
    // http://eigenjoy.com/2011/01/21/worlds-fastest-binary-search/)
    // DGM: it's not faster, but the code is simpler ;)
    unsigned i = 0;
    unsigned count = end - begin + 1;
    unsigned b = (1 << static_cast<int>(log(static_cast<double>(count - 1)) /
                                        LOG_NAT_2));
    for (; b; b >>= 1) {
        unsigned j = i | b;
        if (j < count) {
            CellCode middleCode =
                    (m_thePointsAndTheirCellCodes[begin + j].theCode >> bitDec);
            if (middleCode < truncatedCellCode) {
                // what we are looking for is on the right
                i = j;
            } else if (middleCode == truncatedCellCode) {
                // we must check that it's the first element equal to input code
                if (j == 0 ||
                    (m_thePointsAndTheirCellCodes[begin + j - 1].theCode >>
                     bitDec) != truncatedCellCode) {
                    // what we are looking for is right here
                    return j + begin;
                }
                // otheriwse what we are looking for is on the left!
            }
        }
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        s_jumps += 1.0;
#endif
    }
 
    i += begin;
 
    return (m_thePointsAndTheirCellCodes[i].theCode >> bitDec) ==
                           truncatedCellCode
                   ? i
                   : m_numberOfProjectedPoints;
}
#endif
 
unsigned DgmOctree::findPointNeighbourhood(
        const CCVector3* queryPoint,
        ReferenceCloud* Yk,
        unsigned maxNumberOfNeighbors,
        unsigned char level,
        double& maxSquareDist,
        double maxSearchDist /*=0*/,
        int* finalNeighbourhoodSize /*=nullptr*/) const {
    assert(queryPoint);
    NearestNeighboursSearchStruct nNSS;
    nNSS.queryPoint = *queryPoint;
    nNSS.level = level;
    nNSS.minNumberOfNeighbors = maxNumberOfNeighbors;
    bool inbounds = false;
    getTheCellPosWhichIncludesThePoint(&nNSS.queryPoint, nNSS.cellPos,
                                       nNSS.level, inbounds);
    nNSS.alreadyVisitedNeighbourhoodSize = inbounds ? 0 : 1;
 
    computeCellCenter(nNSS.cellPos, level, nNSS.cellCenter);
    nNSS.maxSearchSquareDistd =
            (maxSearchDist > 0 ? maxSearchDist * maxSearchDist : 0);
 
    // special case: N=1
    if (maxNumberOfNeighbors == 1) {
        maxSquareDist = findTheNearestNeighborStartingFromCell(nNSS);
 
        if (finalNeighbourhoodSize) {
            *finalNeighbourhoodSize = nNSS.alreadyVisitedNeighbourhoodSize;
        }
 
        if (maxSquareDist >= 0) {
            Yk->addPointIndex(nNSS.theNearestPointIndex);
            return 1;
        } else {
            return 0;
        }
    }
 
    // general case: N>1
    unsigned nnFound = findNearestNeighborsStartingFromCell(nNSS);
    if (nnFound) {
        // nnFound can be superior to maxNumberOfNeighbors
        // so we only keep the 'maxNumberOfNeighbors' firsts
        nnFound = std::min(nnFound, maxNumberOfNeighbors);
 
        for (unsigned j = 0; j < nnFound; ++j)
            Yk->addPointIndex(nNSS.pointsInNeighbourhood[j].pointIndex);
 
        maxSquareDist = nNSS.pointsInNeighbourhood.back().squareDistd;
    } else {
        maxSquareDist = -1.0;
    }
 
    if (finalNeighbourhoodSize) {
        *finalNeighbourhoodSize = nNSS.alreadyVisitedNeighbourhoodSize;
    }
 
    return nnFound;
}
 
void DgmOctree::getCellDistanceFromBorders(const Tuple3i& cellPos,
                                           unsigned char level,
                                           int* cellDists) const {
    const int* fillIndexes = m_fillIndexes + 6 * level;
 
    int* _cellDists = cellDists;
    *_cellDists++ = cellPos.x - fillIndexes[0];
    *_cellDists++ = fillIndexes[3] - cellPos.x;
    *_cellDists++ = cellPos.y - fillIndexes[1];
    *_cellDists++ = fillIndexes[4] - cellPos.y;
    *_cellDists++ = cellPos.z - fillIndexes[2];
    *_cellDists++ = fillIndexes[5] - cellPos.z;
}
 
void DgmOctree::getCellDistanceFromBorders(const Tuple3i& cellPos,
                                           unsigned char level,
                                           int neighbourhoodLength,
                                           int* limits) const {
    const int* fillIndexes = m_fillIndexes + 6 * level;
 
    int* _limits = limits;
    for (int dim = 0; dim < 3; ++dim) {
        // min dim.
        {
            int a = cellPos.u[dim] - fillIndexes[dim];
            if (a < -neighbourhoodLength)
                a = -neighbourhoodLength;
            else if (a > neighbourhoodLength)
                a = neighbourhoodLength;
            *_limits++ = a;
        }
 
        // max dim.
        {
            int b = fillIndexes[3 + dim] - cellPos.u[dim];
            if (b < -neighbourhoodLength)
                b = -neighbourhoodLength;
            else if (b > neighbourhoodLength)
                b = neighbourhoodLength;
            *_limits++ = b;
        }
    }
}
 
void DgmOctree::getNeighborCellsAround(
        const Tuple3i& cellPos,
        cellIndexesContainer& neighborCellsIndexes,
        int neighbourhoodLength,
        unsigned char level) const {
    assert(neighbourhoodLength > 0);
 
    // get distance form cell to octree neighbourhood borders
    int limits[6];
    getCellDistanceFromBorders(cellPos, level, neighbourhoodLength, limits);
 
    // limits are expressed in terms of cells at the CURRENT 'level'!
    const int& iMin = limits[0];
    const int& iMax = limits[1];
    const int& jMin = limits[2];
    const int& jMax = limits[3];
    const int& kMin = limits[4];
    const int& kMax = limits[5];
 
    // binary shift for cell code truncation
    const unsigned char bitDec = GET_BIT_SHIFT(level);
 
    for (int i = -iMin; i <= iMax; i++) {
        bool iBorder =
                (abs(i) ==
                 neighbourhoodLength);  // test: are we on a plane of equation
                                        // 'X = +/-neighbourhoodLength'?
        CellCode c0 = GenerateCellCodeForDim(cellPos.x + i);
 
        for (int j = -jMin; j <= jMax; j++) {
            CellCode c1 = c0 | (GenerateCellCodeForDim(cellPos.y + j) << 1);
 
            if (iBorder ||
                (abs(j) == neighbourhoodLength))  // test: are we already on one
                                                  // of the X or Y borders?
            {
                for (int k = -kMin; k <= kMax; k++) {
                    CellCode c2 =
                            c1 | (GenerateCellCodeForDim(cellPos.z + k) << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        neighborCellsIndexes.push_back(index);
                    }
                }
 
            } else  // otherwise we are inside the neighbourhood
            {
                if (kMin ==
                    neighbourhoodLength)  // test: does the plane of equation 'Z
                                          // = -neighbourhoodLength' is inside
                                          // the octree box?
                {
                    CellCode c2 = c1 | (GenerateCellCodeForDim(
                                                cellPos.z - neighbourhoodLength)
                                        << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        neighborCellsIndexes.push_back(index);
                    }
                }
 
                if (kMax ==
                    neighbourhoodLength)  // test: does the plane of equation 'Z
                                          // = +neighbourhoodLength' is inside
                                          // the octree box?
                {
                    CellCode c2 = c1 + (GenerateCellCodeForDim(cellPos.z + kMax)
                                        << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        neighborCellsIndexes.push_back(index);
                    }
                }
            }
        }
    }
}
 
void DgmOctree::getPointsInNeighbourCellsAround(
        NearestNeighboursSearchStruct& nNSS,
        int neighbourhoodLength,
        bool getOnlyPointsWithValidScalar /*=false*/) const {
    assert(neighbourhoodLength >= nNSS.alreadyVisitedNeighbourhoodSize);
 
    // get distance form cell to octree neighbourhood borders
    int limits[6];
    getCellDistanceFromBorders(nNSS.cellPos, nNSS.level, neighbourhoodLength,
                               limits);
 
    // limits are expressed in terms of cells at the CURRENT 'level'!
    const int& iMin = limits[0];
    const int& iMax = limits[1];
    const int& jMin = limits[2];
    const int& jMax = limits[3];
    const int& kMin = limits[4];
    const int& kMax = limits[5];
 
    // binary shift for cell code truncation
    const unsigned char bitDec = GET_BIT_SHIFT(nNSS.level);
 
    for (int i = -iMin; i <= iMax; i++) {
        bool iBorder =
                (abs(i) ==
                 neighbourhoodLength);  // test: are we on a plane of equation
                                        // 'X = +/-neighbourhoodLength'?
        CellCode c0 = GenerateCellCodeForDim(nNSS.cellPos.x + i);
 
        for (int j = -jMin; j <= jMax; j++) {
            CellCode c1 =
                    c0 | (GenerateCellCodeForDim(nNSS.cellPos.y + j) << 1);
 
            // if i or j is on the boundary
            if (iBorder ||
                (abs(j) == neighbourhoodLength))  // test: are we already on one
                                                  // of the X or Y borders?
            {
                for (int k = -kMin; k <= kMax; k++) {
                    CellCode c2 =
                            c1 |
                            (GenerateCellCodeForDim(nNSS.cellPos.z + k) << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        // we increase 'pointsInNeighbourCells' capacity with
                        // average cell size
                        try {
                            nNSS.pointsInNeighbourhood.reserve(
                                    nNSS.pointsInNeighbourhood.size() +
                                    static_cast<unsigned>(
                                            ceil(m_averageCellPopulation
                                                         [nNSS.level])));
                        } catch (
                                ... /*const std::bad_alloc&*/)  // out of memory
                        {
                            // DGM TODO: Shall we stop? shall we try to go on,
                            // as we are not sure that we will actually need
                            // this much points?
                            assert(false);
                        }
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            if (!getOnlyPointsWithValidScalar ||
                                ScalarField::ValidValue(
                                        m_theAssociatedCloud
                                                ->getPointScalarValue(
                                                        p->theIndex))) {
                                nNSS.pointsInNeighbourhood.emplace_back(
                                        m_theAssociatedCloud
                                                ->getPointPersistentPtr(
                                                        p->theIndex),
                                        p->theIndex);
                            }
                        }
                    }
                }
 
            } else  // otherwise we are inside the neighbourhood
            {
                if (kMin ==
                    neighbourhoodLength)  // test: does the plane of equation 'Z
                                          // = -neighbourhoodLength' is inside
                                          // the octree box?
                {
                    CellCode c2 =
                            c1 | (GenerateCellCodeForDim(nNSS.cellPos.z -
                                                         neighbourhoodLength)
                                  << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        // we increase 'nNSS.pointsInNeighbourhood' capacity
                        // with average cell size
                        try {
                            nNSS.pointsInNeighbourhood.reserve(
                                    nNSS.pointsInNeighbourhood.size() +
                                    static_cast<unsigned>(
                                            ceil(m_averageCellPopulation
                                                         [nNSS.level])));
                        } catch (
                                ... /*const std::bad_alloc&*/)  // out of memory
                        {
                            // DGM TODO: Shall we stop? shall we try to go on,
                            // as we are not sure that we will actually need
                            // this much points?
                            assert(false);
                        }
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            if (!getOnlyPointsWithValidScalar ||
                                ScalarField::ValidValue(
                                        m_theAssociatedCloud
                                                ->getPointScalarValue(
                                                        p->theIndex))) {
                                nNSS.pointsInNeighbourhood.emplace_back(
                                        m_theAssociatedCloud
                                                ->getPointPersistentPtr(
                                                        p->theIndex),
                                        p->theIndex);
                            }
                        }
                    }
                }
 
                if (kMax ==
                    neighbourhoodLength)  // test: does the plane of equation 'Z
                                          // = +neighbourhoodLength' is inside
                                          // the octree box? (note that
                                          // neighbourhoodLength > 0)
                {
                    CellCode c2 =
                            c1 | (GenerateCellCodeForDim(nNSS.cellPos.z +
                                                         neighbourhoodLength)
                                  << 2);
 
                    unsigned index = getCellIndex(c2, bitDec);
                    if (index < m_numberOfProjectedPoints) {
                        // we increase 'nNSS.pointsInNeighbourhood' capacity
                        // with average cell size
                        try {
                            nNSS.pointsInNeighbourhood.reserve(
                                    nNSS.pointsInNeighbourhood.size() +
                                    static_cast<unsigned>(
                                            ceil(m_averageCellPopulation
                                                         [nNSS.level])));
                        } catch (
                                ... /*const std::bad_alloc&*/)  // out of memory
                        {
                            // DGM TODO: Shall we stop? shall we try to go on,
                            // as we are not sure that we will actually need
                            // this much points?
                            assert(false);
                        }
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            if (!getOnlyPointsWithValidScalar ||
                                ScalarField::ValidValue(
                                        m_theAssociatedCloud
                                                ->getPointScalarValue(
                                                        p->theIndex))) {
                                nNSS.pointsInNeighbourhood.emplace_back(
                                        m_theAssociatedCloud
                                                ->getPointPersistentPtr(
                                                        p->theIndex),
                                        p->theIndex);
                            }
                        }
                    }
                }
            }
        }
    }
}
 
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
void DgmOctree::getPointsInNeighbourCellsAround(
        NearestNeighboursSphericalSearchStruct& nNSS,
        int minNeighbourhoodLength,
        int maxNeighbourhoodLength) const {
    assert(minNeighbourhoodLength >= nNSS.alreadyVisitedNeighbourhoodSize);
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(nNSS.level);
    CellDescriptor cellDesc;
 
    if (minNeighbourhoodLength == 0)  // special case
    {
        // we don't look if the cell is inside the octree as it is generally the
        // case
        CellCode truncatedCellCode =
                GenerateTruncatedCellCode(nNSS.cellPos, nNSS.level);
        unsigned index = getCellIndex(truncatedCellCode, bitDec);
        if (index < m_numberOfProjectedPoints) {
            // add cell descriptor to cells list
            cellDesc.center = CCVector3(nNSS.cellCenter);
            cellDesc.index = 0;
            nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
            for (cellsContainer::const_iterator p =
                         m_thePointsAndTheirCellCodes.begin() + index;
                 (p != m_thePointsAndTheirCellCodes.end()) &&
                 ((p->theCode >> bitDec) == truncatedCellCode);
                 ++p) {
                PointDescriptor newPoint(
                        m_theAssociatedCloud->getPointPersistentPtr(
                                p->theIndex),
                        p->theIndex);
                nNSS.pointsInSphericalNeighbourhood.push_back(newPoint);
            }
        }
        if (maxNeighbourhoodLength == 0) return;
        ++minNeighbourhoodLength;
    }
 
    // get distance form cell to octree neighbourhood borders
    int limits[6];
    if (!getCellDistanceFromBorders(nNSS.cellPos, nNSS.level,
                                    maxNeighbourhoodLength, limits))
        return;
 
    int& iMinAbs = limits[0];
    int& iMaxAbs = limits[1];
    int& jMinAbs = limits[2];
    int& jMaxAbs = limits[3];
    int& kMinAbs = limits[4];
    int& kMaxAbs = limits[5];
 
    unsigned old_index = 0;
    CellCode old_c2 = 0;
    Tuple3i currentCellPos;
 
    // first part: i in [-maxNL,-minNL] and (j,k) in [-maxNL,maxNL]
    if (iMinAbs >= minNeighbourhoodLength) {
        for (int v0 = nNSS.cellPos.x - iMinAbs;
             v0 <= nNSS.cellPos.x - minNeighbourhoodLength; ++v0) {
            CellCode c0 = GenerateCellCodeForDim(v0);
            currentCellPos.x = v0;
            for (int v1 = nNSS.cellPos.y - jMinAbs;
                 v1 <= nNSS.cellPos.y + jMaxAbs; ++v1) {
                CellCode c1 = c0 | (GenerateCellCodeForDim(v1) << 1);
                currentCellPos.y = v1;
                for (int v2 = nNSS.cellPos.z - kMinAbs;
                     v2 <= nNSS.cellPos.z + kMaxAbs; ++v2) {
                    CellCode c2 = c1 | (GenerateCellCodeForDim(v2) << 2);
 
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c2
                                     ? getCellIndex(
                                               c2, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c2, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.z = v2;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c2;
                    }
                }
            }
        }
        iMinAbs = minNeighbourhoodLength - 1;  // minNeighbourhoodLength>1
    }
 
    // second part: i in [minNL,maxNL] and (j,k) in [-maxNL,maxNL]
    if (iMaxAbs >= minNeighbourhoodLength) {
        for (int v0 = nNSS.cellPos.x + minNeighbourhoodLength;
             v0 <= nNSS.cellPos.x + iMaxAbs; ++v0) {
            CellCode c0 = GenerateCellCodeForDim(v0);
            currentCellPos.x = v0;
            for (int v1 = nNSS.cellPos.y - jMinAbs;
                 v1 <= nNSS.cellPos.y + jMaxAbs; ++v1) {
                CellCode c1 = c0 | (GenerateCellCodeForDim(v1) << 1);
                currentCellPos.y = v1;
                for (int v2 = nNSS.cellPos.z - kMinAbs;
                     v2 <= nNSS.cellPos.z + kMaxAbs; ++v2) {
                    CellCode c2 = c1 | (GenerateCellCodeForDim(v2) << 2);
 
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c2
                                     ? getCellIndex(
                                               c2, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c2, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.z = v2;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c2;
                    }
                }
            }
        }
        iMaxAbs = minNeighbourhoodLength - 1;  // minNeighbourhoodLength>1
    }
 
    // third part: j in [-maxNL,-minNL] and (i,k) in [-maxNL,maxNL]
    if (jMinAbs >= minNeighbourhoodLength) {
        for (int v1 = nNSS.cellPos.y - jMinAbs;
             v1 <= nNSS.cellPos.y - minNeighbourhoodLength; ++v1) {
            CellCode c1 = (GenerateCellCodeForDim(v1) << 1);
            currentCellPos.y = v1;
            for (int v0 = nNSS.cellPos.x - iMinAbs;
                 v0 <= nNSS.cellPos.x + iMaxAbs; ++v0) {
                CellCode c0 = c1 | GenerateCellCodeForDim(v0);
                currentCellPos.x = v0;
                for (int v2 = nNSS.cellPos.z - kMinAbs;
                     v2 <= nNSS.cellPos.z + kMaxAbs; ++v2) {
                    CellCode c2 = c0 | (GenerateCellCodeForDim(v2) << 2);
 
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c2
                                     ? getCellIndex(
                                               c2, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c2, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.z = v2;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c2;
                    }
                }
            }
        }
        jMinAbs = minNeighbourhoodLength - 1;  // minNeighbourhoodLength>1
    }
 
    // fourth part: j in [minNL,maxNL] and (i,k) in [-maxNL,maxNL]
    if (jMaxAbs >= minNeighbourhoodLength) {
        for (int v1 = nNSS.cellPos.y + minNeighbourhoodLength;
             v1 <= nNSS.cellPos.y + jMaxAbs; ++v1) {
            CellCode c1 = (GenerateCellCodeForDim(v1) << 1);
            currentCellPos.y = v1;
            for (int v0 = nNSS.cellPos.x - iMinAbs;
                 v0 <= nNSS.cellPos.x + iMaxAbs; ++v0) {
                CellCode c0 = c1 | GenerateCellCodeForDim(v0);
                currentCellPos.x = v0;
                for (int v2 = nNSS.cellPos.z - kMinAbs;
                     v2 <= nNSS.cellPos.z + kMaxAbs; ++v2) {
                    CellCode c2 = c0 | (GenerateCellCodeForDim(v2) << 2);
 
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c2
                                     ? getCellIndex(
                                               c2, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c2, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.z = v2;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c2);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c2;
                    }
                }
            }
        }
        jMaxAbs = minNeighbourhoodLength - 1;  // minNeighbourhoodLength>1
    }
 
    // fifth part: k in [-maxNL,-minNL] and (i,k) in [-maxNL,maxNL]
    if (kMinAbs >= minNeighbourhoodLength) {
        for (int v2 = nNSS.cellPos.z - kMinAbs;
             v2 <= nNSS.cellPos.z - minNeighbourhoodLength; ++v2) {
            CellCode c2 = (GenerateCellCodeForDim(v2) << 2);
            currentCellPos.z = v2;
            for (int v0 = nNSS.cellPos.x - iMinAbs;
                 v0 <= nNSS.cellPos.x + iMaxAbs; ++v0) {
                CellCode c0 = c2 | GenerateCellCodeForDim(v0);
                currentCellPos.x = v0;
                for (int v1 = nNSS.cellPos.y - jMinAbs;
                     v1 <= nNSS.cellPos.y + jMaxAbs; ++v1) {
                    CellCode c1 = c0 | (GenerateCellCodeForDim(v1) << 1);
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c1
                                     ? getCellIndex(
                                               c1, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c1, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.y = v1;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c1);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c1;
                    }
                }
            }
        }
        // kMinAbs = minNeighbourhoodLength-1; //minNeighbourhoodLength>1
    }
 
    // sixth and last part: k in [minNL,maxNL] and (i,k) in [-maxNL,maxNL]
    if (kMaxAbs >= minNeighbourhoodLength) {
        for (int v2 = nNSS.cellPos.z + minNeighbourhoodLength;
             v2 <= nNSS.cellPos.z + kMaxAbs; ++v2) {
            CellCode c2 = (GenerateCellCodeForDim(v2) << 2);
            currentCellPos.z = v2;
            for (int v0 = nNSS.cellPos.x - iMinAbs;
                 v0 <= nNSS.cellPos.x + iMaxAbs; ++v0) {
                CellCode c0 = c2 | GenerateCellCodeForDim(v0);
                currentCellPos.x = v0;
                for (int v1 = nNSS.cellPos.y - jMinAbs;
                     v1 <= nNSS.cellPos.y + jMaxAbs; ++v1) {
                    CellCode c1 = c0 | (GenerateCellCodeForDim(v1) << 1);
                    // look for corresponding cell
                    unsigned index =
                            (old_c2 < c1
                                     ? getCellIndex(
                                               c1, bitDec, old_index,
                                               m_numberOfProjectedPoints - 1)
                                     : getCellIndex(c1, bitDec, 0, old_index));
                    if (index < m_numberOfProjectedPoints) {
                        // add cell descriptor to cells list
                        currentCellPos.y = v1;
                        computeCellCenter(currentCellPos, nNSS.level,
                                          cellDesc.center);
                        cellDesc.index =
                                nNSS.pointsInSphericalNeighbourhood.size();
                        nNSS.cellsInNeighbourhood.push_back(cellDesc);
 
                        for (cellsContainer::const_iterator p =
                                     m_thePointsAndTheirCellCodes.begin() +
                                     index;
                             (p != m_thePointsAndTheirCellCodes.end()) &&
                             ((p->theCode >> bitDec) == c1);
                             ++p) {
                            PointDescriptor newPoint(
                                    m_theAssociatedCloud->getPointPersistentPtr(
                                            p->theIndex),
                                    p->theIndex);
                            nNSS.pointsInSphericalNeighbourhood.push_back(
                                    newPoint);
                        }
 
                        old_index = index;
                        old_c2 = c1;
                    }
                }
            }
        }
        // kMaxAbs = minNeighbourhoodLength-1; //minNeighbourhoodLength>1
    }
}
#endif
 
double DgmOctree::findTheNearestNeighborStartingFromCell(
        NearestNeighboursSearchStruct& nNSS) const {
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(nNSS.level);
 
    // cell size at the current level of subdivision
    const PointCoordinateType& cs = getCellSize(nNSS.level);
 
    // already visited cells (relative distance to the cell that includes the
    // query point)
    int visitedCellDistance = nNSS.alreadyVisitedNeighbourhoodSize;
    // minimum (a priori) relative distance to get eligible points (see
    // 'eligibleDist' below)
    int eligibleCellDistance = visitedCellDistance;
 
    // if we have not already looked for the first cell (the one including the
    // query point)
    if (visitedCellDistance == 0) {
        //'visitedCellDistance == 0' means that no cell has ever been processed!
        // No cell should be inside 'minimalCellsSetToVisit'
        assert(nNSS.minimalCellsSetToVisit.empty());
 
        // check for existence of an 'including' cell
        CellCode truncatedCellCode =
                GenerateTruncatedCellCode(nNSS.cellPos, nNSS.level);
        unsigned index = (truncatedCellCode == INVALID_CELL_CODE
                                  ? m_numberOfProjectedPoints
                                  : getCellIndex(truncatedCellCode, bitDec));
 
        visitedCellDistance = 1;
 
        // it this cell does exist...
        if (index < m_numberOfProjectedPoints) {
            // we add it to the 'cells to visit' set
            nNSS.minimalCellsSetToVisit.push_back(index);
            eligibleCellDistance = 1;
        }
        // otherwise, we may be very far from the nearest octree cell
        //(let's try to get there asap)
        else {
            // fill indexes for current level
            const int* _fillIndexes = m_fillIndexes + 6 * nNSS.level;
            int diagonalDistance = 0;
            for (int dim = 0; dim < 3; ++dim) {
                // distance to min border of octree along each axis
                int distToBorder = *_fillIndexes - nNSS.cellPos.u[dim];
                // if its negative, lets look the other side
                if (distToBorder < 0) {
                    // distance to max border of octree along each axis
                    distToBorder = nNSS.cellPos.u[dim] - _fillIndexes[3];
                }
 
                if (distToBorder > 0) {
                    visitedCellDistance =
                            std::max(distToBorder, visitedCellDistance);
                    diagonalDistance += distToBorder * distToBorder;
                }
 
                // next dimension
                ++_fillIndexes;
            }
 
            // the nearest octree cell
            diagonalDistance = static_cast<int>(
                    ceil(sqrt(static_cast<float>(diagonalDistance))));
            eligibleCellDistance = std::max(diagonalDistance, 1);
 
            if (nNSS.maxSearchSquareDistd > 0) {
                // Distance to the nearest point
                double minDist =
                        static_cast<double>(eligibleCellDistance - 1) * cs;
                // if we are already outside of the search limit, we can quit
                if (minDist * minDist > nNSS.maxSearchSquareDistd) {
                    return -1.0;
                }
            }
        }
 
        // update
        nNSS.alreadyVisitedNeighbourhoodSize = visitedCellDistance;
    }
 
    // for each dimension, we look for the min distance between the query point
    // and the cell border. This distance (minDistToBorder) corresponds to the
    // maximal radius of a sphere centered on the query point and totally
    // included inside the cell
    PointCoordinateType minDistToBorder = ComputeMinDistanceToCellBorder(
            nNSS.queryPoint, cs, nNSS.cellCenter);
 
    // cells for which we have already computed the distances from their points
    // to the query point
    unsigned alreadyProcessedCells = 0;
 
    // Min (squared) distance of neighbours
    double minSquareDist = -1.0;
 
    while (true) {
        // if we do have found points but that were too far to be eligible
        if (minSquareDist > 0) {
            // what would be the correct neighbourhood size to be sure of it?
            int newEligibleCellDistance = static_cast<int>(ceil(
                    (static_cast<PointCoordinateType>(sqrt(minSquareDist)) -
                     minDistToBorder) /
                    cs));
            eligibleCellDistance =
                    std::max(newEligibleCellDistance, eligibleCellDistance);
        }
 
        // we get the (new) cells around the current neighbourhood
        while (nNSS.alreadyVisitedNeighbourhoodSize <
               eligibleCellDistance)  // DGM: warning,
                                      // alreadyVisitedNeighbourhoodSize == 1
                                      // means that we have only visited the
                                      // first cell (distance=0)
        {
            getNeighborCellsAround(nNSS.cellPos, nNSS.minimalCellsSetToVisit,
                                   nNSS.alreadyVisitedNeighbourhoodSize,
                                   nNSS.level);
            ++nNSS.alreadyVisitedNeighbourhoodSize;
        }
 
        // we compute distances for the new points
        DgmOctree::cellIndexesContainer::const_iterator q;
        for (q = nNSS.minimalCellsSetToVisit.begin() + alreadyProcessedCells;
             q != nNSS.minimalCellsSetToVisit.end(); ++q) {
            // current cell index (== index of its first point)
            unsigned m = *q;
 
            // we scan the whole cell to see if it contains a closer point
            cellsContainer::const_iterator p =
                    m_thePointsAndTheirCellCodes.begin() + m;
            CellCode code = (p->theCode >> bitDec);
            while (m < m_numberOfProjectedPoints &&
                   (p->theCode >> bitDec) == code) {
                // square distance to query point
                double dist2 = (*m_theAssociatedCloud->getPointPersistentPtr(
                                        p->theIndex) -
                                nNSS.queryPoint)
                                       .norm2d();
                // we keep track of the closest one
                if (dist2 < minSquareDist || minSquareDist < 0) {
                    nNSS.theNearestPointIndex = p->theIndex;
                    minSquareDist = dist2;
                    if (dist2 == 0)  // no need to process any further
                        break;
                }
                ++m;
                ++p;
            }
        }
        alreadyProcessedCells =
                static_cast<unsigned>(nNSS.minimalCellsSetToVisit.size());
 
        // equivalent spherical neighbourhood radius (as we are actually looking
        // to 'square' neighbourhoods, we must check that the nearest points
        // inside such neighbourhoods are indeed near enough to fall inside the
        // biggest included sphere). Otherwise we must look further.
        double eligibleDist =
                static_cast<double>(eligibleCellDistance - 1) * cs +
                minDistToBorder;
        double squareEligibleDist = eligibleDist * eligibleDist;
 
        // if we have found an eligible point
        if (minSquareDist >= 0 && minSquareDist <= squareEligibleDist) {
            if (nNSS.maxSearchSquareDistd <= 0 ||
                minSquareDist <= nNSS.maxSearchSquareDistd)
                return minSquareDist;
            else
                return -1.0;
        } else {
            // no eligible point? Maybe we are already too far?
            if (nNSS.maxSearchSquareDistd > 0 &&
                squareEligibleDist >= nNSS.maxSearchSquareDistd)
                return -1.0;
        }
 
        // default strategy: increase neighbourhood size of +1 (for next step)
        ++eligibleCellDistance;
    }
 
    // we should never get here!
    assert(false);
 
    return -1.0;
}
 
// search for at least "minNumberOfNeighbors" points around a query point
unsigned DgmOctree::findNearestNeighborsStartingFromCell(
        NearestNeighboursSearchStruct& nNSS,
        bool getOnlyPointsWithValidScalar /*=false*/) const {
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(nNSS.level);
 
    // cell size at the current level of subdivision
    const PointCoordinateType& cs = getCellSize(nNSS.level);
 
    // already visited cells (relative distance to the cell that includes the
    // query point)
    int visitedCellDistance = nNSS.alreadyVisitedNeighbourhoodSize;
    // minimum (a priori) relative distance to get eligible points (see
    // 'eligibleDist' below)
    int eligibleCellDistance = visitedCellDistance;
 
    // shall we look inside the first cell (the one including the query point)?
    if (visitedCellDistance == 0) {
        // visitedCellDistance == 0 means that no cell has ever been processed!
        // No point should be inside 'pointsInNeighbourhood'
        assert(nNSS.pointsInNeighbourhood.empty());
 
        // check for existence of 'including' cell
        CellCode truncatedCellCode =
                GenerateTruncatedCellCode(nNSS.cellPos, nNSS.level);
        unsigned index = (truncatedCellCode == INVALID_CELL_CODE
                                  ? m_numberOfProjectedPoints
                                  : getCellIndex(truncatedCellCode, bitDec));
 
        visitedCellDistance = 1;
 
        // it this cell does exist...
        if (index < m_numberOfProjectedPoints) {
            // we grab the points inside
            cellsContainer::const_iterator p =
                    m_thePointsAndTheirCellCodes.begin() + index;
            while (p != m_thePointsAndTheirCellCodes.end() &&
                   (p->theCode >> bitDec) == truncatedCellCode) {
                if (!getOnlyPointsWithValidScalar ||
                    ScalarField::ValidValue(
                            m_theAssociatedCloud->getPointScalarValue(
                                    p->theIndex))) {
                    nNSS.pointsInNeighbourhood.emplace_back(
                            m_theAssociatedCloud->getPointPersistentPtr(
                                    p->theIndex),
                            p->theIndex);
                    ++p;
                }
            }
 
            eligibleCellDistance = 1;
        }
        // otherwise, we may be very far from the nearest octree cell
        //(let's try to get there asap)
        else {
            // fill indexes for current level
            const int* _fillIndexes = m_fillIndexes + 6 * nNSS.level;
            int diagonalDistance = 0;
            for (int dim = 0; dim < 3; ++dim) {
                // distance to min border of octree along each axis
                int distToBorder = *_fillIndexes - nNSS.cellPos.u[dim];
                // if its negative, lets look the other side
                if (distToBorder < 0) {
                    // distance to max border of octree along each axis
                    distToBorder = nNSS.cellPos.u[dim] - _fillIndexes[3];
                }
 
                if (distToBorder > 0) {
                    visitedCellDistance =
                            std::max(distToBorder, visitedCellDistance);
                    diagonalDistance += distToBorder * distToBorder;
                }
 
                // next dimension
                ++_fillIndexes;
            }
 
            // the nearest octree cell
            diagonalDistance = static_cast<int>(
                    ceil(sqrt(static_cast<float>(diagonalDistance))));
            eligibleCellDistance = std::max(diagonalDistance, 1);
 
            if (nNSS.maxSearchSquareDistd > 0) {
                // Distance of the nearest point
                double minDist =
                        static_cast<double>(eligibleCellDistance - 1) * cs;
                // if we are already outside of the search limit, we can quit
                if (minDist * minDist > nNSS.maxSearchSquareDistd) {
                    return 0;
                }
            }
        }
    }
 
    // for each dimension, we look for the min distance between the query point
    // and the cell border. This distance (minDistToBorder) corresponds to the
    // maximal radius of a sphere centered on the query point and totally
    // included inside the cell
    PointCoordinateType minDistToBorder = ComputeMinDistanceToCellBorder(
            nNSS.queryPoint, cs, nNSS.cellCenter);
 
    // eligible points found
    unsigned eligiblePoints = 0;
 
    // points for which we have already computed the distance to the query point
    unsigned alreadyProcessedPoints = 0;
 
    // Min (squared) distance of non eligible points
    double minSquareDist = -1.0;
 
    // while we don't have enough 'nearest neighbours'
    while (eligiblePoints < nNSS.minNumberOfNeighbors) {
        // if we do have found points but that were too far to be eligible
        if (minSquareDist > 0) {
            // what would be the correct neighbourhood size to be sure of it?
            int newEligibleCellDistance = static_cast<int>(ceil(
                    (static_cast<PointCoordinateType>(sqrt(minSquareDist)) -
                     minDistToBorder) /
                    cs));
            eligibleCellDistance =
                    std::max(newEligibleCellDistance, eligibleCellDistance);
        }
 
        // we get the (new) points lying in the added area
        while (visitedCellDistance <
               eligibleCellDistance)  // DGM: warning, visitedCellDistance == 1
                                      // means that we have only visited the
                                      // first cell (distance=0)
        {
            getPointsInNeighbourCellsAround(nNSS, visitedCellDistance,
                                            getOnlyPointsWithValidScalar);
            ++visitedCellDistance;
        }
 
        // we compute distances for the new points
        NeighboursSet::iterator q;
        for (q = nNSS.pointsInNeighbourhood.begin() + alreadyProcessedPoints;
             q != nNSS.pointsInNeighbourhood.end(); ++q)
            q->squareDistd = (*q->point - nNSS.queryPoint).norm2d();
        alreadyProcessedPoints =
                static_cast<unsigned>(nNSS.pointsInNeighbourhood.size());
 
        // equivalent spherical neighbourhood radius (as we are actually looking
        // to 'square' neighbourhoods, we must check that the nearest points
        // inside such neighbourhoods are indeed near enough to fall inside the
        // biggest included sphere). Otherwise we must look further.
        double eligibleDist =
                static_cast<double>(eligibleCellDistance - 1) * cs +
                minDistToBorder;
        double squareEligibleDist = eligibleDist * eligibleDist;
 
        // let's test all the previous 'not yet eligible' points and the new
        // ones
        unsigned j = eligiblePoints;
        for (q = nNSS.pointsInNeighbourhood.begin() + eligiblePoints;
             q != nNSS.pointsInNeighbourhood.end(); ++q, ++j) {
            // if the point is eligible
            if (q->squareDistd <= squareEligibleDist) {
                if (eligiblePoints < j)
                    std::swap(nNSS.pointsInNeighbourhood[eligiblePoints],
                              nNSS.pointsInNeighbourhood[j]);
                ++eligiblePoints;
            }
            // otherwise we track the nearest one
            else if (q->squareDistd < minSquareDist || j == eligiblePoints) {
                minSquareDist = q->squareDistd;
            }
        }
 
        // Maybe we are already too far?
        if (nNSS.maxSearchSquareDistd > 0 &&
            squareEligibleDist >= nNSS.maxSearchSquareDistd)
            break;
 
        // default strategy: increase neighbourhood size of +1 (for next step)
        ++eligibleCellDistance;
    }
 
    // update the neighbourhood size (for next call, if the query point lies in
    // the same cell)
    nNSS.alreadyVisitedNeighbourhoodSize = visitedCellDistance;
 
    // we sort the eligible points
    std::sort(nNSS.pointsInNeighbourhood.begin(),
              nNSS.pointsInNeighbourhood.begin() + eligiblePoints,
              PointDescriptor::distComp);
 
    // we return the number of eligible points found
    return eligiblePoints;
}
 
int DgmOctree::getPointsInSphericalNeighbourhood(
        const CCVector3& sphereCenter,
        PointCoordinateType radius,
        NeighboursSet& neighbours,
        unsigned char level /*=0*/) const {
    // cell size
    const PointCoordinateType& cs = getCellSize(level);
    PointCoordinateType halfCellSize = cs / 2;
 
    // squared radius
    double squareRadius =
            static_cast<double>(radius) * static_cast<double>(radius);
    // constant value for cell/sphere inclusion test
    double maxDiagFactor = squareRadius + (0.75 * cs + SQRT_3 * radius) * cs;
 
    // we are going to test all the cells that may intersect the sphere
    CCVector3 corner = sphereCenter - CCVector3(radius, radius, radius);
    Tuple3i cornerPos;
    getTheCellPosWhichIncludesThePoint(&corner, cornerPos, level);
 
    // don't need to look outside the octree limits!
    cornerPos.x = std::max<int>(cornerPos.x, 0);
    cornerPos.y = std::max<int>(cornerPos.y, 0);
    cornerPos.z = std::max<int>(cornerPos.z, 0);
 
    // corresponding cell limits
    CCVector3 boxMin(m_dimMin[0] + cs * cornerPos.x,
                     m_dimMin[1] + cs * cornerPos.y,
                     m_dimMin[2] + cs * cornerPos.z);
 
    // max number of cells for this dimension
    int maxCellCount = OCTREE_LENGTH(level);
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(level);
 
    CCVector3 cellMin = boxMin;
    Tuple3i cellPos(cornerPos.x, 0, 0);
    while (cellMin.x < sphereCenter.x + radius && cellPos.x < maxCellCount) {
        CCVector3 cellCenter(cellMin.x + halfCellSize, 0, 0);
 
        cellMin.y = boxMin.y;
        cellPos.y = cornerPos.y;
        while (cellMin.y < sphereCenter.y + radius &&
               cellPos.y < maxCellCount) {
            cellCenter.y = cellMin.y + halfCellSize;
 
            cellMin.z = boxMin.z;
            cellPos.z = cornerPos.z;
            while (cellMin.z < sphereCenter.z + radius &&
                   cellPos.z < maxCellCount) {
                cellCenter.z = cellMin.z + halfCellSize;
                // test this cell
                // 1st test: is it close enough to the sphere center?
                if ((cellCenter - sphereCenter).norm2d() <=
                    maxDiagFactor)  // otherwise cell is totally outside
                {
                    // 2nd test: does this cell exists?
                    CellCode truncatedCellCode =
                            GenerateTruncatedCellCode(cellPos, level);
                    unsigned cellIndex =
                            getCellIndex(truncatedCellCode, bitDec);
 
                    // if yes get the corresponding points
                    if (cellIndex < m_numberOfProjectedPoints) {
                        // we look for the first index in
                        // 'm_thePointsAndTheirCellCodes' corresponding to this
                        // cell
                        cellsContainer::const_iterator p =
                                m_thePointsAndTheirCellCodes.begin() +
                                cellIndex;
                        CellCode searchCode = (p->theCode >> bitDec);
 
                        // while the (partial) cell code matches this cell
                        for (; (p != m_thePointsAndTheirCellCodes.end()) &&
                               ((p->theCode >> bitDec) == searchCode);
                             ++p) {
                            const CCVector3* P =
                                    m_theAssociatedCloud->getPoint(p->theIndex);
                            double d2 = (*P - sphereCenter).norm2d();
                            // we keep the points falling inside the sphere
                            if (d2 <= squareRadius) {
                                neighbours.emplace_back(P, p->theIndex, d2);
                            }
                        }
                    }
                }
 
                // next cell
                cellMin.z += cs;
                ++cellPos.z;
            }
 
            // next cell
            cellMin.y += cs;
            ++cellPos.y;
        }
 
        // next cell
        cellMin.x += cs;
        ++cellPos.x;
    }
 
    return static_cast<int>(neighbours.size());
}
 
std::size_t DgmOctree::getPointsInBoxNeighbourhood(
        BoxNeighbourhood& params) const {
    // cell size
    const PointCoordinateType& cs = getCellSize(params.level);
 
    // we are going to test all the cells that may intersect this box
    // first we extract the box... bounding box ;)
    CCVector3 minCorner, maxCorner;
    if (params.axes) {
        // normalize axes (just in case)
        params.axes[0].normalize();
        params.axes[1].normalize();
        params.axes[2].normalize();
 
        const PointCoordinateType& dx = params.dimensions.x;
        const PointCoordinateType& dy = params.dimensions.y;
        const PointCoordinateType& dz = params.dimensions.z;
 
        CCVector3 corners[8] = {CCVector3(-dx / 2, -dy / 2, -dz / 2),
                                CCVector3(-dx / 2, -dy / 2, dz / 2),
                                CCVector3(-dx / 2, dy / 2, dz / 2),
                                CCVector3(-dx / 2, dy / 2, -dz / 2),
                                CCVector3(dx / 2, -dy / 2, -dz / 2),
                                CCVector3(dx / 2, -dy / 2, dz / 2),
                                CCVector3(dx / 2, dy / 2, dz / 2),
                                CCVector3(dx / 2, dy / 2, -dz / 2)};
 
        // position of the box vertices in the octree coordinate system
        for (unsigned char i = 0; i < 8; ++i) {
            corners[i] = corners[i].x * params.axes[0] +
                         corners[i].y * params.axes[1] +
                         corners[i].z * params.axes[2];
            if (i) {
                if (corners[i].x < minCorner.x)
                    minCorner.x = corners[i].x;
                else if (corners[i].x > maxCorner.x)
                    maxCorner.x = corners[i].x;
 
                if (corners[i].y < minCorner.y)
                    minCorner.y = corners[i].y;
                else if (corners[i].y > maxCorner.y)
                    maxCorner.y = corners[i].y;
 
                if (corners[i].z < minCorner.z)
                    minCorner.z = corners[i].z;
                else if (corners[i].z > maxCorner.z)
                    maxCorner.z = corners[i].z;
            } else {
                minCorner = maxCorner = corners[0];
            }
        }
 
        // up to now min and max corners where centered on (0,0,0)
        minCorner = params.center + minCorner;
        maxCorner = params.center + maxCorner;
    } else {
        minCorner = params.center - params.dimensions / 2;
        maxCorner = params.center + params.dimensions / 2;
    }
 
    Tuple3i minCornerPos;
    getTheCellPosWhichIncludesThePoint(&minCorner, minCornerPos, params.level);
    Tuple3i maxCornerPos;
    getTheCellPosWhichIncludesThePoint(&maxCorner, maxCornerPos, params.level);
 
    const int* minFillIndexes = getMinFillIndexes(params.level);
    const int* maxFillIndexes = getMaxFillIndexes(params.level);
 
    // don't need to look outside the octree limits!
    minCornerPos.x = std::max<int>(minCornerPos.x, minFillIndexes[0]);
    minCornerPos.y = std::max<int>(minCornerPos.y, minFillIndexes[1]);
    minCornerPos.z = std::max<int>(minCornerPos.z, minFillIndexes[2]);
 
    maxCornerPos.x = std::min<int>(maxCornerPos.x, maxFillIndexes[0]);
    maxCornerPos.y = std::min<int>(maxCornerPos.y, maxFillIndexes[1]);
    maxCornerPos.z = std::min<int>(maxCornerPos.z, maxFillIndexes[2]);
 
    // half cell diagonal
    CCVector3 boxHalfDimensions = params.dimensions / 2;
    CCVector3 maxHalfDist = boxHalfDimensions;
    if (params.axes) {
        PointCoordinateType halfDiag =
                static_cast<PointCoordinateType>(cs * sqrt(3.0) / 2.0);
        maxHalfDist += CCVector3(halfDiag, halfDiag, halfDiag);
    }
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(params.level);
 
    for (int i = minCornerPos.x; i <= maxCornerPos.x; ++i) {
        for (int j = minCornerPos.y; j <= maxCornerPos.y; ++j) {
            for (int k = minCornerPos.z; k <= maxCornerPos.z; ++k) {
                // additional inclusion test
                if (params.axes) {
                    CCVector3 cellCenter =
                            m_dimMin +
                            CCVector3(
                                    static_cast<PointCoordinateType>(i + 0.5),
                                    static_cast<PointCoordinateType>(j + 0.5),
                                    static_cast<PointCoordinateType>(k + 0.5)) *
                                    cs;
 
                    // project the cell center in the box C.S.
                    CCVector3 Q = cellCenter - params.center;
                    Q = CCVector3(params.axes[0].dot(Q), params.axes[1].dot(Q),
                                  params.axes[2].dot(Q));
 
                    // rough inclusion test
                    if (std::abs(Q.x) > maxHalfDist.x ||
                        std::abs(Q.y) > maxHalfDist.y ||
                        std::abs(Q.z) > maxHalfDist.z) {
                        // skip this cell
                        continue;
                    }
                }
 
                // test if this cell exists
                Tuple3i cellPos(i, j, k);
                CellCode truncatedCellCode =
                        GenerateTruncatedCellCode(cellPos, params.level);
                unsigned cellIndex = getCellIndex(truncatedCellCode, bitDec);
 
                // if yes, we can test the corresponding points
                if (cellIndex < m_numberOfProjectedPoints) {
                    // we look for the first index in
                    // 'm_thePointsAndTheirCellCodes' corresponding to this cell
                    cellsContainer::const_iterator p =
                            m_thePointsAndTheirCellCodes.begin() + cellIndex;
                    CellCode searchCode = (p->theCode >> bitDec);
 
                    // while the (partial) cell code matches this cell
                    for (; (p != m_thePointsAndTheirCellCodes.end()) &&
                           ((p->theCode >> bitDec) == searchCode);
                         ++p) {
                        const CCVector3* P =
                                m_theAssociatedCloud->getPoint(p->theIndex);
                        CCVector3 Q = *P - params.center;
 
                        if (params.axes) {
                            // project the point in the box C.S.
                            Q = CCVector3(params.axes[0].dot(Q),
                                          params.axes[1].dot(Q),
                                          params.axes[2].dot(Q));
                        }
 
                        // we keep the points that fall inside the box
                        if (std::abs(Q.x) <= boxHalfDimensions.x &&
                            std::abs(Q.y) <= boxHalfDimensions.y &&
                            std::abs(Q.z) <= boxHalfDimensions.z) {
                            params.neighbours.emplace_back(P, p->theIndex, 0);
                        }
                    }
                }
            }
        }
    }
 
    return params.neighbours.size();
}
 
std::size_t DgmOctree::getPointsInCylindricalNeighbourhood(
        CylindricalNeighbourhood& params) const {
    // cell size
    const PointCoordinateType& cs = getCellSize(params.level);
    PointCoordinateType halfCellSize = cs / 2;
 
    // squared radius
    double squareRadius = static_cast<double>(params.radius) *
                          static_cast<double>(params.radius);
    // constant value for cell/sphere inclusion test
    double maxDiagFactor =
            squareRadius + (0.75 * cs + SQRT_3 * params.radius) * cs;
    PointCoordinateType maxLengthFactor =
            params.maxHalfLength +
            static_cast<PointCoordinateType>(cs * SQRT_3 / 2);
    PointCoordinateType minLengthFactor =
            params.onlyPositiveDir ? 0 : -maxLengthFactor;
 
    PointCoordinateType minHalfLength =
            params.onlyPositiveDir ? 0 : -params.maxHalfLength;
 
    // we are going to test all the cells that may intersect this cylinder
    // dumb bounding-box estimation: place two spheres at the ends of the
    // cylinder
    CCVector3 minCorner;
    CCVector3 maxCorner;
    {
        CCVector3 C1 = params.center + params.dir * params.maxHalfLength;
        CCVector3 C2 = params.center + params.dir * minHalfLength;
        CCVector3 corner1 =
                C1 - CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner2 =
                C1 + CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner3 =
                C2 - CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner4 =
                C2 + CCVector3(params.radius, params.radius, params.radius);
 
        minCorner.x = std::min(std::min(corner1.x, corner2.x),
                               std::min(corner3.x, corner4.x));
        minCorner.y = std::min(std::min(corner1.y, corner2.y),
                               std::min(corner3.y, corner4.y));
        minCorner.z = std::min(std::min(corner1.z, corner2.z),
                               std::min(corner3.z, corner4.z));
 
        maxCorner.x = std::max(std::max(corner1.x, corner2.x),
                               std::max(corner3.x, corner4.x));
        maxCorner.y = std::max(std::max(corner1.y, corner2.y),
                               std::max(corner3.y, corner4.y));
        maxCorner.z = std::max(std::max(corner1.z, corner2.z),
                               std::max(corner3.z, corner4.z));
    }
 
    Tuple3i cornerPos;
    getTheCellPosWhichIncludesThePoint(&minCorner, cornerPos, params.level);
 
    const int* minFillIndexes = getMinFillIndexes(params.level);
    const int* maxFillIndexes = getMaxFillIndexes(params.level);
 
    // don't need to look outside the octree limits!
    cornerPos.x = std::max<int>(cornerPos.x, minFillIndexes[0]);
    cornerPos.y = std::max<int>(cornerPos.y, minFillIndexes[1]);
    cornerPos.z = std::max<int>(cornerPos.z, minFillIndexes[2]);
 
    // corresponding cell limits
    CCVector3 boxMin(
            m_dimMin[0] + cs * static_cast<PointCoordinateType>(cornerPos.x),
            m_dimMin[1] + cs * static_cast<PointCoordinateType>(cornerPos.y),
            m_dimMin[2] + cs * static_cast<PointCoordinateType>(cornerPos.z));
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(params.level);
 
    CCVector3 cellMin = boxMin;
    Tuple3i cellPos(cornerPos.x, 0, 0);
    while (cellMin.x < maxCorner.x && cellPos.x <= maxFillIndexes[0]) {
        CCVector3 cellCenter(cellMin.x + halfCellSize, 0, 0);
 
        cellMin.y = boxMin.y;
        cellPos.y = cornerPos.y;
        while (cellMin.y < maxCorner.y && cellPos.y <= maxFillIndexes[1]) {
            cellCenter.y = cellMin.y + halfCellSize;
 
            cellMin.z = boxMin.z;
            cellPos.z = cornerPos.z;
            while (cellMin.z < maxCorner.z && cellPos.z <= maxFillIndexes[2]) {
                cellCenter.z = cellMin.z + halfCellSize;
                // test this cell
                // 1st test: is it close enough to the cylinder axis?
                CCVector3 OC = (cellCenter - params.center);
                PointCoordinateType dot = OC.dot(params.dir);
                double d2 = (OC - params.dir * dot).norm2d();
                if (d2 <= maxDiagFactor && dot <= maxLengthFactor &&
                    dot >= minLengthFactor)  // otherwise cell is totally
                                             // outside
                {
                    // 2nd test: does this cell exists?
                    CellCode truncatedCellCode =
                            GenerateTruncatedCellCode(cellPos, params.level);
                    unsigned cellIndex =
                            getCellIndex(truncatedCellCode, bitDec);
 
                    // if yes get the corresponding points
                    if (cellIndex < m_numberOfProjectedPoints) {
                        // we look for the first index in
                        // 'm_thePointsAndTheirCellCodes' corresponding to this
                        // cell
                        cellsContainer::const_iterator p =
                                m_thePointsAndTheirCellCodes.begin() +
                                cellIndex;
                        CellCode searchCode = (p->theCode >> bitDec);
 
                        // while the (partial) cell code matches this cell
                        for (; (p != m_thePointsAndTheirCellCodes.end()) &&
                               ((p->theCode >> bitDec) == searchCode);
                             ++p) {
                            const CCVector3* P =
                                    m_theAssociatedCloud->getPoint(p->theIndex);
 
                            // we keep the points falling inside the sphere
                            CCVector3 OP = (*P - params.center);
                            dot = OP.dot(params.dir);
                            d2 = (OP - params.dir * dot).norm2d();
                            if (d2 <= squareRadius && dot >= minHalfLength &&
                                dot <= params.maxHalfLength) {
                                params.neighbours.emplace_back(
                                        P, p->theIndex,
                                        dot);  // we save the distance
                                               // relatively to the center
                                               // projected on the axis!
                            }
                        }
                    }
                }
 
                // next cell
                cellMin.z += cs;
                ++cellPos.z;
            }
 
            // next cell
            cellMin.y += cs;
            ++cellPos.y;
        }
 
        // next cell
        cellMin.x += cs;
        ++cellPos.x;
    }
 
    return params.neighbours.size();
}
 
std::size_t DgmOctree::getPointsInCylindricalNeighbourhoodProgressive(
        ProgressiveCylindricalNeighbourhood& params) const {
    // cell size
    const PointCoordinateType& cs = getCellSize(params.level);
    PointCoordinateType halfCellSize = cs / 2;
 
    // squared radius
    double squareRadius = static_cast<double>(params.radius) *
                          static_cast<double>(params.radius);
    // constant value for cell/sphere inclusion test
    double maxDiagFactor =
            squareRadius + (0.75 * cs + SQRT_3 * params.radius) * cs;
    PointCoordinateType maxLengthFactor =
            params.maxHalfLength +
            static_cast<PointCoordinateType>(cs * SQRT_3 / 2);
    PointCoordinateType minLengthFactor =
            params.onlyPositiveDir ? 0 : -maxLengthFactor;
 
    // increase the search cylinder's height
    params.currentHalfLength += params.radius;
    // no need to chop the max cylinder if the parts are too small!
    //(takes also into account any 'overflow' above maxHalfLength ;)
    if (params.maxHalfLength - params.currentHalfLength < params.radius / 2)
        params.currentHalfLength = params.maxHalfLength;
 
    PointCoordinateType currentHalfLengthMinus =
            params.onlyPositiveDir ? 0 : -params.currentHalfLength;
 
    // first process potential candidates from the previous pass
    {
        for (std::size_t k = 0; k < params.potentialCandidates.size();
             /*++k*/) {
            // potentialCandidates[k].squareDist = 'dot'!
            if (params.potentialCandidates[k].squareDistd >=
                        currentHalfLengthMinus &&
                params.potentialCandidates[k].squareDistd <=
                        params.currentHalfLength) {
                params.neighbours.push_back(params.potentialCandidates[k]);
                // and remove it from the potential list
                std::swap(params.potentialCandidates[k],
                          params.potentialCandidates.back());
                params.potentialCandidates.pop_back();
            } else {
                ++k;
            }
        }
    }
 
    // we are going to test all the cells that may intersect this cylinder
    // dumb bounding-box estimation: place two spheres at the ends of the
    // cylinder
    CCVector3 minCorner;
    CCVector3 maxCorner;
    {
        CCVector3 C1 = params.center + params.dir * params.currentHalfLength;
        CCVector3 C2 = params.center + params.dir * currentHalfLengthMinus;
        CCVector3 corner1 =
                C1 - CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner2 =
                C1 + CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner3 =
                C2 - CCVector3(params.radius, params.radius, params.radius);
        CCVector3 corner4 =
                C2 + CCVector3(params.radius, params.radius, params.radius);
 
        minCorner.x = std::min(std::min(corner1.x, corner2.x),
                               std::min(corner3.x, corner4.x));
        minCorner.y = std::min(std::min(corner1.y, corner2.y),
                               std::min(corner3.y, corner4.y));
        minCorner.z = std::min(std::min(corner1.z, corner2.z),
                               std::min(corner3.z, corner4.z));
 
        maxCorner.x = std::max(std::max(corner1.x, corner2.x),
                               std::max(corner3.x, corner4.x));
        maxCorner.y = std::max(std::max(corner1.y, corner2.y),
                               std::max(corner3.y, corner4.y));
        maxCorner.z = std::max(std::max(corner1.z, corner2.z),
                               std::max(corner3.z, corner4.z));
    }
 
    Tuple3i cornerPos;
    getTheCellPosWhichIncludesThePoint(&minCorner, cornerPos, params.level);
 
    const int* minFillIndexes = getMinFillIndexes(params.level);
    const int* maxFillIndexes = getMaxFillIndexes(params.level);
 
    // don't need to look outside the octree limits!
    cornerPos.x = std::max<int>(cornerPos.x, minFillIndexes[0]);
    cornerPos.y = std::max<int>(cornerPos.y, minFillIndexes[1]);
    cornerPos.z = std::max<int>(cornerPos.z, minFillIndexes[2]);
 
    // corresponding cell limits
    CCVector3 boxMin(
            m_dimMin[0] + cs * static_cast<PointCoordinateType>(cornerPos.x),
            m_dimMin[1] + cs * static_cast<PointCoordinateType>(cornerPos.y),
            m_dimMin[2] + cs * static_cast<PointCoordinateType>(cornerPos.z));
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(params.level);
 
    Tuple3i cellPos(cornerPos.x, 0, 0);
    CCVector3 cellMin = boxMin;
    while (cellMin.x < maxCorner.x && cellPos.x <= maxFillIndexes[0]) {
        CCVector3 cellCenter(cellMin.x + halfCellSize, 0, 0);
 
        cellMin.y = boxMin.y;
        cellPos.y = cornerPos.y;
        while (cellMin.y < maxCorner.y && cellPos.y <= maxFillIndexes[1]) {
            cellCenter.y = cellMin.y + halfCellSize;
 
            cellMin.z = boxMin.z;
            cellPos.z = cornerPos.z;
            while (cellMin.z < maxCorner.z && cellPos.z <= maxFillIndexes[2]) {
                cellCenter.z = cellMin.z + halfCellSize;
 
                // don't test already tested cells!
                if (cellPos.x < params.prevMinCornerPos.x ||
                    cellPos.x >= params.prevMaxCornerPos.x ||
                    cellPos.y < params.prevMinCornerPos.y ||
                    cellPos.y >= params.prevMaxCornerPos.y ||
                    cellPos.z < params.prevMinCornerPos.z ||
                    cellPos.z >= params.prevMaxCornerPos.z) {
                    // test this cell
                    // 1st test: is it close enough to the cylinder axis?
                    CCVector3 OC = (cellCenter - params.center);
                    PointCoordinateType dot = OC.dot(params.dir);
                    double d2 = (OC - params.dir * dot).norm2d();
                    if (d2 <= maxDiagFactor && dot <= maxLengthFactor &&
                        dot >= minLengthFactor)  // otherwise cell is totally
                                                 // outside
                    {
                        // 2nd test: does this cell exists?
                        CellCode truncatedCellCode = GenerateTruncatedCellCode(
                                cellPos, params.level);
                        unsigned cellIndex =
                                getCellIndex(truncatedCellCode, bitDec);
 
                        // if yes get the corresponding points
                        if (cellIndex < m_numberOfProjectedPoints) {
                            // we look for the first index in
                            // 'm_thePointsAndTheirCellCodes' corresponding to
                            // this cell
                            cellsContainer::const_iterator p =
                                    m_thePointsAndTheirCellCodes.begin() +
                                    cellIndex;
                            CellCode searchCode = (p->theCode >> bitDec);
 
                            // while the (partial) cell code matches this cell
                            for (; (p != m_thePointsAndTheirCellCodes.end()) &&
                                   ((p->theCode >> bitDec) == searchCode);
                                 ++p) {
                                const CCVector3* P =
                                        m_theAssociatedCloud->getPoint(
                                                p->theIndex);
 
                                // we keep the points falling inside the sphere
                                CCVector3 OP = (*P - params.center);
                                dot = OP.dot(params.dir);
                                d2 = (OP - params.dir * dot).norm2d();
                                if (d2 <= squareRadius) {
                                    // potential candidate?
                                    if (dot >= currentHalfLengthMinus &&
                                        dot <= params.currentHalfLength) {
                                        params.neighbours.emplace_back(
                                                P, p->theIndex,
                                                dot);  // we save the distance
                                                       // relatively to the
                                                       // center projected on
                                                       // the axis!
                                    } else if (params.currentHalfLength <
                                               params.maxHalfLength) {
                                        // we still keep it in the 'potential
                                        // candidates' list
                                        params.potentialCandidates.emplace_back(
                                                P, p->theIndex,
                                                dot);  // we save the distance
                                                       // relatively to the
                                                       // center projected on
                                                       // the axis!
                                    }
                                }
                            }
                        }
                    }
                }
 
                // next cell
                cellMin.z += cs;
                ++cellPos.z;
            }
 
            // next cell
            cellMin.y += cs;
            ++cellPos.y;
        }
 
        // next cell
        cellMin.x += cs;
        ++cellPos.x;
    }
 
    params.prevMinCornerPos = cornerPos;
    params.prevMaxCornerPos = cellPos;
 
    return params.neighbours.size();
}
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
static double s_skippedPoints = 0.0;
static double s_testedPoints = 0.0;
#endif
 
// search for all neighbors inside a sphere
// warning: there may be more points at the end of nNSS.pointsInNeighbourhood
// than the actual nearest neighbors!
int DgmOctree::findNeighborsInASphereStartingFromCell(
        NearestNeighboursSearchStruct& nNSS,
        double radius,
        bool sortValues) const {
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
    if (!nNSS.ready) {
        // current level cell size
        const PointCoordinateType& cs = getCellSize(nNSS.level);
 
        // we deduce the minimum cell neighbourhood size (integer) that includes
        // the search sphere for ANY point in the cell
        int minNeighbourhoodSize =
                static_cast<int>(ceil(radius / cs + SQRT_3 / 2.0));
 
        nNSS.cellsInNeighbourhood.reserve(minNeighbourhoodSize *
                                          minNeighbourhoodSize *
                                          minNeighbourhoodSize);
 
        if (nNSS.alreadyVisitedNeighbourhoodSize == 1 &&
            nNSS.cellsInNeighbourhood.empty() &&
            !nNSS.pointsInNeighbourhood.empty()) {
            // in this case, we assume the points already in
            // 'pointsInNeighbourhood' are the 1st cell points
            nNSS.cellsInNeighbourhood.push_back(
                    CellDescriptor(nNSS.cellCenter, 0));
            nNSS.pointsInSphericalNeighbourhood = nNSS.pointsInNeighbourhood;
        }
 
        getPointsInNeighbourCellsAround(nNSS,
                                        nNSS.alreadyVisitedNeighbourhoodSize,
                                        minNeighbourhoodSize);
 
        if (nNSS.pointsInNeighbourhood.size() <
            nNSS.pointsInSphericalNeighbourhood.size())
            nNSS.pointsInNeighbourhood.resize(
                    nNSS.pointsInSphericalNeighbourhood.size());
 
        // don't forget to update the visited neighbourhood size!
        nNSS.alreadyVisitedNeighbourhoodSize = minNeighbourhoodSize + 1;
 
        nNSS.ready = true;
    }
#else
    // current level cell size
    const PointCoordinateType& cs = getCellSize(nNSS.level);
 
    // we compute the minimal distance between the query point and all cell
    // borders
    const PointCoordinateType minDistToBorder = ComputeMinDistanceToCellBorder(
            nNSS.queryPoint, cs, nNSS.cellCenter);
 
    // we deduce the minimum cell neighbourhood size (integer) that includes the
    // search sphere
    const int minNeighbourhoodSize =
            1 +
            (radius > minDistToBorder
                     ? static_cast<int>(ceil((radius - minDistToBorder) / cs))
                     : 0);
 
    // if we don't have visited such a neighbourhood...
    if (nNSS.alreadyVisitedNeighbourhoodSize < minNeighbourhoodSize) {
        //... let's look for the corresponding points
        for (int i = nNSS.alreadyVisitedNeighbourhoodSize;
             i < minNeighbourhoodSize; ++i)
            getPointsInNeighbourCellsAround(nNSS, i);
 
        // don't forget to update the visited neighbourhood size!
        nNSS.alreadyVisitedNeighbourhoodSize = minNeighbourhoodSize;
    }
#endif
 
    // squared distances comparison is faster!
    const double squareRadius = radius * radius;
    unsigned numberOfEligiblePoints = 0;
 
#ifdef TEST_CELLS_FOR_SPHERICAL_NN
    // cell limit relatively to sphere tight bounding box
    // const PointCoordinateType& half_cs=getCellSize(nNSS.level+1); //half cell
    // size at current level CCVector3 limitMin =
    // nNSS.queryPoint-CCVector3(radius,radius,radius)-CCVector3(half_cs,half_cs,half_cs);
    // CCVector3 limitMax =
    // nNSS.queryPoint+CCVector3(radius,radius,radius)+CCVector3(half_cs,half_cs,half_cs);
 
    // cell by cell scan
    for (NeighbourCellsSet::iterator c = nNSS.cellsInNeighbourhood.begin();
         c != nNSS.cellsInNeighbourhood.end(); ++c) {
        // we check the cell bounding box
        /*if (limitMax.x < c->center.x || limitMin.x >  c->center.x
        || limitMax.y < c->center.y || limitMin.y >  c->center.y
        || limitMax.z < c->center.z || limitMin.z >  c->center.z)
        continue; //sphere totally outside the cell
        //*/
 
        // distance between the new cell center and the sphere center
        PointCoordinateType d2 = (c->center - nNSS.queryPoint).norm2();
 
        // is this cell totally out of the sphere?
        if (d2 <= nNSS.minOutD2) {
            NeighboursSet::iterator p =
                    nNSS.pointsInSphericalNeighbourhood.begin() + c->index;
            unsigned count =
                    ((c + 1) != nNSS.cellsInNeighbourhood.end()
                             ? (c + 1)->index
                             : nNSS.pointsInSphericalNeighbourhood.size()) -
                    c->index;
            if (!sortValues &&
                d2 <= nNSS.maxInD2)  // totally inside? (+ we can skip distances
                                     // computation)
            {
                //... we had them to the 'eligible points' part of the container
                std::copy(p, p + count,
                          nNSS.pointsInNeighbourhood.begin() +
                                  numberOfEligiblePoints);
                numberOfEligiblePoints += count;
#ifdef COMPUTE_NN_SEARCH_STATISTICS
                s_skippedPoints += static_cast<double>(count);
#endif
            } else {
                for (unsigned j = 0; j < count; ++j, ++p) {
                    p->squareDist = (*p->point - nNSS.queryPoint).norm2();
#ifdef COMPUTE_NN_SEARCH_STATISTICS
                    s_testedPoints += 1.0;
#endif
                    // if the distance is inferior to the sphere radius...
                    if (p->squareDistd <= squareRadius) {
                        //... we had it to the 'eligible points' part of the
                        // container
                        nNSS.pointsInNeighbourhood[numberOfEligiblePoints++] =
                                *p;
                    }
                }
            }
        }
        // else cell is totally outside
        {
#ifdef COMPUTE_NN_SEARCH_STATISTICS
            unsigned count =
                    ((c + 1) != nNSS.cellsInNeighbourhood.end()
                             ? (c + 1)->index
                             : nNSS.pointsInSphericalNeighbourhood.size()) -
                    c->index;
            s_skippedPoints += static_cast<double>(count);
#endif
        }
    }
 
#else  // TEST_CELLS_FOR_SPHERICAL_NN
 
    // point by point scan
    std::size_t i = 0;
 
    for (PointDescriptor& pDescr : nNSS.pointsInNeighbourhood) {
        pDescr.squareDistd = (*pDescr.point - nNSS.queryPoint).norm2d();
 
        // if the distance is inferior to the sphere radius...
        if (pDescr.squareDistd <= squareRadius) {
            //... we add it to the 'eligible points' part of the container
            if (i > numberOfEligiblePoints) {
                std::swap(nNSS.pointsInNeighbourhood[i],
                          nNSS.pointsInNeighbourhood[numberOfEligiblePoints]);
            }
 
            ++numberOfEligiblePoints;
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
            s_testedPoints += 1.0;
#endif
        }
 
        ++i;
    }
 
#endif  
 
    // eventually (if requested) we sort the eligible points
    if (sortValues && numberOfEligiblePoints > 0) {
        std::sort(nNSS.pointsInNeighbourhood.begin(),
                  nNSS.pointsInNeighbourhood.begin() + numberOfEligiblePoints,
                  PointDescriptor::distComp);
    }
 
    // return the number of eligible points
    return numberOfEligiblePoints;
}
 
unsigned char DgmOctree::findBestLevelForAGivenNeighbourhoodSizeExtraction(
        PointCoordinateType radius) const {
    static const PointCoordinateType c_neighbourhoodSizeExtractionFactor =
            static_cast<PointCoordinateType>(2.5);
    PointCoordinateType aim = std::max<PointCoordinateType>(
            0, radius / c_neighbourhoodSizeExtractionFactor);
 
    int level = 1;
    PointCoordinateType minValue = getCellSize(1) - aim;
    minValue *= minValue;
    for (int i = 2; i <= MAX_OCTREE_LEVEL; ++i) {
        // we need two points per cell ideally
        if (m_averageCellPopulation[i] < 1.5) break;
 
        // The level with cell size as near as possible to the aim
        PointCoordinateType cellSizeDelta = getCellSize(i) - aim;
        cellSizeDelta *= cellSizeDelta;
 
        if (cellSizeDelta < minValue) {
            level = i;
            minValue = cellSizeDelta;
        }
    }
 
    return static_cast<unsigned char>(level);
}
 
unsigned char DgmOctree::findBestLevelForComparisonWithOctree(
        const DgmOctree* theOtherOctree) const {
    unsigned ptsA = getNumberOfProjectedPoints();
    unsigned ptsB = theOtherOctree->getNumberOfProjectedPoints();
 
    unsigned char maxOctreeLevel = MAX_OCTREE_LEVEL;
 
    if (std::min(ptsA, ptsB) < 16)
        maxOctreeLevel =
                std::min(maxOctreeLevel,
                         static_cast<unsigned char>(5));  // very small clouds
    else if (std::max(ptsA, ptsB) < 2000000)
        maxOctreeLevel = std::min(
                maxOctreeLevel,
                static_cast<unsigned char>(10));  // average size clouds
 
    double estimatedTime[MAX_OCTREE_LEVEL]{};
    unsigned char bestLevel = 1;
 
    for (int i = 1; i < maxOctreeLevel; ++i)  // warning: i >= 1
    {
        int diffA = 0;
        int diffB = 0;
        int cellsA = 0;
        int cellsB = 0;
 
        bool success = diff(i, m_thePointsAndTheirCellCodes,
                            theOtherOctree->m_thePointsAndTheirCellCodes, diffA,
                            diffB, cellsA, cellsB);
 
        if (!success) {
            continue;
        }
 
        // we use a linear model for prediction
        estimatedTime[i] =
                ((static_cast<double>(ptsA) * ptsB) / cellsB) * 0.001 + diffA;
 
        if (estimatedTime[i] < estimatedTime[bestLevel]) {
            bestLevel = i;
        }
    }
 
    return bestLevel;
}
 
unsigned char DgmOctree::findBestLevelForAGivenPopulationPerCell(
        unsigned indicativeNumberOfPointsPerCell) const {
    for (unsigned char level = MAX_OCTREE_LEVEL; level > 0; --level) {
        if (m_averageCellPopulation[level] >
            indicativeNumberOfPointsPerCell)  // density can only increase. If
                                              // it's above the target, no need
                                              // to look further
        {
            // we take the closest match between this level and the previous one
            if (level == MAX_OCTREE_LEVEL ||
                (m_averageCellPopulation[level] -
                         indicativeNumberOfPointsPerCell <=
                 indicativeNumberOfPointsPerCell -
                         m_averageCellPopulation
                                 [level +
                                  1]))  // by definition
                                        // "m_averageCellPopulation[level + 1]
                                        // <= indicativeNumberOfPointsPerCell"
            {
                return level;
            } else {
                return level + 1;
            }
        }
    }
 
    return 1;
}
 
unsigned char DgmOctree::findBestLevelForAGivenCellNumber(
        unsigned indicativeNumberOfCells) const {
    // we look for the level giviing the number of points per cell as close to
    // the query
    unsigned char bestLevel = 1;
    // number of cells for this level
    int n = getCellNumber(bestLevel);
    // error relatively to the query
    int oldd = abs(n - static_cast<int>(indicativeNumberOfCells));
 
    n = getCellNumber(bestLevel + 1);
    int d = abs(n - static_cast<int>(indicativeNumberOfCells));
 
    while (d < oldd && bestLevel < MAX_OCTREE_LEVEL) {
        ++bestLevel;
        oldd = d;
        n = getCellNumber(bestLevel + 1);
        d = abs(n - static_cast<int>(indicativeNumberOfCells));
    }
 
    return bestLevel;
}
 
double DgmOctree::computeMeanOctreeDensity(unsigned char level) const {
    return static_cast<double>(m_numberOfProjectedPoints) /
           static_cast<double>(getCellNumber(level));
}
 
bool DgmOctree::getCellCodesAndIndexes(unsigned char level,
                                       cellsContainer& vec,
                                       bool truncatedCodes /*=false*/) const {
    try {
        // binary shift for cell code truncation
        unsigned char bitDec = GET_BIT_SHIFT(level);
 
        cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
        CellCode predCode =
                (p->theCode >> bitDec) +
                1;  // pred value must be different than the first element's
 
        for (unsigned i = 0; i < m_numberOfProjectedPoints; ++i, ++p) {
            CellCode currentCode = (p->theCode >> bitDec);
 
            if (predCode != currentCode)
                vec.emplace_back(i, truncatedCodes ? currentCode : p->theCode);
 
            predCode = currentCode;
        }
    } catch (const std::bad_alloc&) {
        // not enough memory
        return false;
    }
    return true;
}
 
bool DgmOctree::getCellCodes(unsigned char level,
                             cellCodesContainer& vec,
                             bool truncatedCodes /*=false*/) const {
    try {
        // binary shift for cell code truncation
        unsigned char bitDec = GET_BIT_SHIFT(level);
 
        cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
        CellCode predCode =
                (p->theCode >> bitDec) +
                1;  // pred value must be different than the first element's
 
        for (unsigned i = 0; i < m_numberOfProjectedPoints; ++i, ++p) {
            CellCode currentCode = (p->theCode >> bitDec);
 
            if (predCode != currentCode)
                vec.push_back(truncatedCodes ? currentCode : p->theCode);
 
            predCode = currentCode;
        }
    } catch (const std::bad_alloc&) {
        // not enough memory
        return false;
    }
    return true;
}
 
bool DgmOctree::getCellIndexes(unsigned char level,
                               cellIndexesContainer& vec) const {
    try {
        vec.resize(m_cellCount[level]);
    } catch (const std::bad_alloc&) {
        // not enough memory
        return false;
    }
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(level);
 
    cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
    CellCode predCode =
            (p->theCode >> bitDec) +
            1;  // pred value must be different than the first element's
 
    for (unsigned i = 0, j = 0; i < m_numberOfProjectedPoints; ++i, ++p) {
        CellCode currentCode = (p->theCode >> bitDec);
 
        if (predCode != currentCode) vec[j++] = i;
 
        predCode = currentCode;
    }
 
    return true;
}
 
bool DgmOctree::getPointsInCellByCellIndex(
        ReferenceCloud* cloud,
        unsigned cellIndex,
        unsigned char level,
        bool clearOutputCloud /* = true*/) const {
    assert(cloud && cloud->getAssociatedCloud() == m_theAssociatedCloud);
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(level);
 
    // we look for the first index in 'm_thePointsAndTheirCellCodes'
    // corresponding to this cell
    cellsContainer::const_iterator p =
            m_thePointsAndTheirCellCodes.begin() + cellIndex;
    CellCode searchCode = (p->theCode >> bitDec);
 
    if (clearOutputCloud) {
        cloud->clear();
    }
 
    // while the (partial) cell code matches this cell
    while ((p != m_thePointsAndTheirCellCodes.end()) &&
           ((p->theCode >> bitDec) == searchCode)) {
        if (!cloud->addPointIndex(p->theIndex)) return false;
        ++p;
    }
 
    return true;
}
 
ReferenceCloud* DgmOctree::getPointsInCellsWithSortedCellCodes(
        cellCodesContainer& cellCodes,
        unsigned char level,
        ReferenceCloud* subset,
        bool areCodesTruncated /*=false*/) const {
    assert(subset);
 
    // binary shift for cell code truncation
    unsigned char bitDec1 = GET_BIT_SHIFT(level);  // shift for this octree
                                                   // codes
    unsigned char bitDec2 =
            (areCodesTruncated ? 0 : bitDec1);  // shift for the input codes
 
    cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
    CellCode toExtractCode,
            currentCode =
                    (p->theCode >> bitDec1);  // pred value must be different
                                              // than the first element's
 
    subset->clear();
 
    cellCodesContainer::const_iterator q = cellCodes.begin();
    unsigned ind_p = 0;
    while (ind_p < m_numberOfProjectedPoints) {
        // we skip codes while the searched code is below the current one
        while (((toExtractCode = (*q >> bitDec2)) < currentCode) &&
               (q != cellCodes.end()))
            ++q;
 
        if (q == cellCodes.end()) break;
 
        // now we skip current codes to catch the search one!
        while ((ind_p < m_numberOfProjectedPoints) &&
               (currentCode <= toExtractCode)) {
            if (currentCode == toExtractCode)
                subset->addPointIndex(p->theIndex);
 
            ++p;
            if (++ind_p < m_numberOfProjectedPoints)
                currentCode = p->theCode >> bitDec1;
        }
    }
 
    return subset;
}
 
void DgmOctree::diff(const cellCodesContainer& codesA,
                     const cellCodesContainer& codesB,
                     cellCodesContainer& diffA,
                     cellCodesContainer& diffB) const {
    if (codesA.empty() && codesB.empty()) return;
 
    cellCodesContainer::const_iterator pA = codesA.begin();
    cellCodesContainer::const_iterator pB = codesB.begin();
 
    // cell codes should already be sorted!
    while (pA != codesA.end() && pB != codesB.end()) {
        if (*pA < *pB)
            diffA.push_back(*pA++);
        else if (*pA > *pB)
            diffB.push_back(*pB++);
        else {
            ++pA;
            ++pB;
        }
    }
 
    while (pA != codesA.end()) diffA.push_back(*pA++);
    while (pB != codesB.end()) diffB.push_back(*pB++);
}
 
bool DgmOctree::diff(unsigned char octreeLevel,
                     const cellsContainer& codesA,
                     const cellsContainer& codesB,
                     int& diffA,
                     int& diffB,
                     int& cellsA,
                     int& cellsB) const {
    diffA = 0;
    diffB = 0;
    cellsA = 0;
    cellsB = 0;
 
    if (codesA.empty() && codesB.empty()) {
        return false;
    }
 
    cellsContainer::const_iterator pA = codesA.begin();
    cellsContainer::const_iterator pB = codesB.begin();
 
    // binary shift for cell code truncation
    unsigned char bitDec = GET_BIT_SHIFT(octreeLevel);
 
    CellCode predCodeA = pA->theCode >> bitDec;
    CellCode predCodeB = pB->theCode >> bitDec;
 
    CellCode currentCodeA = 0;
    CellCode currentCodeB = 0;
 
    // cell codes should already be sorted!
    while ((pA != codesA.end()) && (pB != codesB.end())) {
        if (predCodeA < predCodeB) {
            ++diffA;
            ++cellsA;
            while ((pA != codesA.end()) &&
                   ((currentCodeA = (pA->theCode >> bitDec)) == predCodeA))
                ++pA;
            predCodeA = currentCodeA;
        } else if (predCodeA > predCodeB) {
            ++diffB;
            ++cellsB;
            while ((pB != codesB.end()) &&
                   ((currentCodeB = (pB->theCode >> bitDec)) == predCodeB))
                ++pB;
            predCodeB = currentCodeB;
        } else {
            while ((pA != codesA.end()) &&
                   ((currentCodeA = (pA->theCode >> bitDec)) == predCodeA))
                ++pA;
            predCodeA = currentCodeA;
            ++cellsA;
            while ((pB != codesB.end()) &&
                   ((currentCodeB = (pB->theCode >> bitDec)) == predCodeB))
                ++pB;
            predCodeB = currentCodeB;
            ++cellsB;
        }
    }
 
    while (pA != codesA.end()) {
        ++diffA;
        ++cellsA;
        while ((pA != codesA.end()) &&
               ((currentCodeA = (pA->theCode >> bitDec)) == predCodeA))
            ++pA;
        predCodeA = currentCodeA;
    }
    while (pB != codesB.end()) {
        ++diffB;
        ++cellsB;
        while ((pB != codesB.end()) &&
               ((currentCodeB = (pB->theCode >> bitDec)) == predCodeB))
            ++pB;
        predCodeB = currentCodeB;
    }
 
    return true;
}
 
int DgmOctree::extractCCs(unsigned char level,
                          bool sixConnexity,
                          GenericProgressCallback* progressCb) const {
    std::vector<CellCode> cellCodes;
    getCellCodes(level, cellCodes);
    return extractCCs(cellCodes, level, sixConnexity, progressCb);
}
 
struct IndexAndCodeExt {
#ifdef OCTREE_CODES_64_BITS
    using IndexType = unsigned long long;
#else
    using IndexType = unsigned;
#endif
 
    IndexType theIndex;
 
    DgmOctree::CellCode theCode;
 
    IndexAndCodeExt() : theIndex(0), theCode(0) {}
 
 
    static bool indexComp(const IndexAndCodeExt& a,
                          const IndexAndCodeExt& b) throw() {
        return a.theIndex < b.theIndex;
    }
};
 
int DgmOctree::extractCCs(const cellCodesContainer& cellCodes,
                          unsigned char level,
                          bool sixConnexity,
                          GenericProgressCallback* progressCb) const {
    std::size_t numberOfCells = cellCodes.size();
    if (numberOfCells == 0)  // no cells!
        return -1;
 
    // filled octree cells
    std::vector<IndexAndCodeExt> ccCells;
    try {
        ccCells.resize(numberOfCells);
    } catch (const std::bad_alloc&) {
        // not enough memory
        return -2;
    }
 
    // we compute the position of each cell (grid coordinates)
    Tuple3i indexMin, indexMax;
    {
        // binary shift for cell code truncation
        unsigned char bitDec = GET_BIT_SHIFT(level);
 
        for (std::size_t i = 0; i < numberOfCells; i++) {
            ccCells[i].theCode = (cellCodes[i] >> bitDec);
 
            Tuple3i cellPos;
            getCellPos(ccCells[i].theCode, level, cellPos, true);
 
            // we look for the actual min and max dimensions of the input cells
            // set (which may not be the whole set of octree cells!)
            if (i != 0) {
                for (unsigned char k = 0; k < 3; k++) {
                    if (cellPos.u[k] < indexMin.u[k])
                        indexMin.u[k] = cellPos.u[k];
                    else if (cellPos.u[k] > indexMax.u[k])
                        indexMax.u[k] = cellPos.u[k];
                }
            } else {
                indexMin.x = indexMax.x = cellPos.x;
                indexMin.y = indexMax.y = cellPos.y;
                indexMin.z = indexMax.z = cellPos.z;
            }
 
            // Warning: the cells will have to be sorted inside a slice
            // afterwards!
            ccCells[i].theIndex =
                    (static_cast<IndexAndCodeExt::IndexType>(cellPos.x)) +
                    (static_cast<IndexAndCodeExt::IndexType>(cellPos.y)
                     << level) +
                    (static_cast<IndexAndCodeExt::IndexType>(cellPos.z)
                     << (2 * level));
        }
    }
 
    // we deduce the size of the grid that totally includes input cells
    Tuple3i gridSize = indexMax - indexMin + Tuple3i(1, 1, 1);
 
    // we sort the cells
    ParallelSort(ccCells.begin(), ccCells.end(),
                 IndexAndCodeExt::indexComp);  // ascending index code order
 
    const int& di = gridSize.x;
    const int& dj = gridSize.y;
    const int& step = gridSize.z;
 
    // relative neighbos positions (either 6 or 26 total - but we only use half
    // of it)
    unsigned char neighborsInCurrentSlice = 0, neighborsInPrecedingSlice = 0;
    int currentSliceNeighborsShifts[4],
            precedingSliceNeighborsShifts[9];  // maximum size to simplify
                                               // code...
 
    if (sixConnexity)  // 6-connexity
    {
        neighborsInCurrentSlice = 2;
        currentSliceNeighborsShifts[0] = -(di + 2);
        currentSliceNeighborsShifts[1] = -1;
 
        neighborsInPrecedingSlice = 1;
        precedingSliceNeighborsShifts[0] = 0;
    } else  // 26-connexity
    {
        neighborsInCurrentSlice = 4;
        currentSliceNeighborsShifts[0] = -1 - (di + 2);
        currentSliceNeighborsShifts[1] = -(di + 2);
        currentSliceNeighborsShifts[2] = 1 - (di + 2);
        currentSliceNeighborsShifts[3] = -1;
 
        neighborsInPrecedingSlice = 9;
        precedingSliceNeighborsShifts[0] = -1 - (di + 2);
        precedingSliceNeighborsShifts[1] = -(di + 2);
        precedingSliceNeighborsShifts[2] = 1 - (di + 2);
        precedingSliceNeighborsShifts[3] = -1;
        precedingSliceNeighborsShifts[4] = 0;
        precedingSliceNeighborsShifts[5] = 1;
        precedingSliceNeighborsShifts[6] = -1 + (di + 2);
        precedingSliceNeighborsShifts[7] = (di + 2);
        precedingSliceNeighborsShifts[8] = 1 + (di + 2);
    }
 
    // shared structures (to avoid repeated allocations)
    std::vector<int> neighboursVal, neighboursMin;
    try {
        neighboursVal.reserve(neighborsInCurrentSlice +
                              neighborsInPrecedingSlice);
        neighboursMin.reserve(neighborsInCurrentSlice +
                              neighborsInPrecedingSlice);
    } catch (const std::bad_alloc&) {
        // not enough memory
        return -2;
    }
 
    // temporary virtual 'slices'
    int sliceSize =
            (di + 2) * (dj + 2);  // add a margin to avoid "boundary effects"
    std::vector<int> slice;
    std::vector<int> oldSlice;
    // equivalence table between 'on the fly' labels
    std::vector<int> equivalentLabels;
    std::vector<int> cellIndexToLabel;
 
    try {
        slice.resize(sliceSize);
        oldSlice.resize(sliceSize, 0);  // previous slice is empty by default
        equivalentLabels.resize(numberOfCells + 2, 0);
        cellIndexToLabel.resize(numberOfCells, 0);
    } catch (const std::bad_alloc&) {
        // not enough memory
        return -2;
    }
 
    // progress notification
    if (progressCb) {
        if (progressCb->textCanBeEdited()) {
            progressCb->setMethodTitle("Components Labeling");
            char buffer[256];
            sprintf(buffer, "Box: [%i*%i*%i]", gridSize.x, gridSize.y,
                    gridSize.z);
            progressCb->setInfo(buffer);
        }
        progressCb->update(0);
        progressCb->start();
    }
 
    // current label
    std::size_t currentLabel = 1;
 
    // process each slice
    {
        unsigned counter = 0;
        const IndexAndCodeExt::IndexType gridCoordMask =
                (static_cast<IndexAndCodeExt::IndexType>(1) << level) - 1;
        std::vector<IndexAndCodeExt>::const_iterator _ccCells = ccCells.begin();
        NormalizedProgress nprogress(progressCb, step);
 
        for (int k = indexMin.z; k < indexMin.z + step; k++) {
            // initialize the 'current' slice
            std::fill(slice.begin(), slice.end(), 0);
 
            // for each cell of the slice
            while (counter < numberOfCells &&
                   static_cast<int>(_ccCells->theIndex >> (level << 1)) == k) {
                int iind = static_cast<int>(_ccCells->theIndex & gridCoordMask);
                int jind = static_cast<int>((_ccCells->theIndex >> level) &
                                            gridCoordMask);
                int cellIndex = (iind - indexMin.x + 1) +
                                (jind - indexMin.y + 1) * (di + 2);
                ++_ccCells;
 
                // we look if the cell has neighbors inside the slice
                int* _slice = &(slice[cellIndex]);
                {
                    for (unsigned char n = 0; n < neighborsInCurrentSlice;
                         n++) {
                        assert(cellIndex + currentSliceNeighborsShifts[n] <
                               sliceSize);
                        const int& neighborLabel =
                                _slice[currentSliceNeighborsShifts[n]];
                        if (neighborLabel > 1)
                            neighboursVal.push_back(neighborLabel);
                    }
                }
 
                // and in the previous slice
                const int* _oldSlice = &(oldSlice[cellIndex]);
                {
                    for (unsigned char n = 0; n < neighborsInPrecedingSlice;
                         n++) {
                        assert(cellIndex + precedingSliceNeighborsShifts[n] <
                               sliceSize);
                        const int& neighborLabel =
                                _oldSlice[precedingSliceNeighborsShifts[n]];
                        if (neighborLabel > 1)
                            neighboursVal.push_back(neighborLabel);
                    }
                }
 
                // number of neighbors for current cell
                std::size_t p = neighboursVal.size();
 
                if (p == 0)  // no neighbor
                {
                    *_slice = static_cast<int>(
                            ++currentLabel);  // we create a new label
                } else if (p == 1)            // 1 neighbor
                {
                    *_slice = neighboursVal.back();  // we'll use its label
                    neighboursVal.pop_back();
                } else  // more than 1 neighbor?
                {
                    // we get the smallest label
                    ParallelSort(neighboursVal.begin(), neighboursVal.end());
 
                    int smallestLabel = neighboursVal[0];
 
                    // if they are not the same
                    if (smallestLabel != neighboursVal.back()) {
                        int lastLabel = 0;
                        neighboursMin.clear();
                        // we get the smallest equivalent label for each
                        // neighbor's branch
                        {
                            for (std::size_t n = 0; n < p; n++) {
                                // ... we start from its C.C. index
                                int label = neighboursVal[n];
                                // if it's not the same as the previous
                                // neighbor's
                                if (label != lastLabel) {
                                    // we update the 'before' value
                                    assert(label <
                                           static_cast<int>(numberOfCells) + 2);
                                    lastLabel = label;
 
                                    // we look for its real equivalent value
                                    while (equivalentLabels[label] > 1) {
                                        label = equivalentLabels[label];
                                        assert(label <
                                               static_cast<int>(numberOfCells) +
                                                       2);
                                    }
 
                                    neighboursMin.push_back(label);
                                }
                            }
                        }
 
                        // get the smallest one
                        ParallelSort(neighboursMin.begin(),
                                     neighboursMin.end());
 
                        smallestLabel = neighboursMin.front();
 
                        // update the equivalence table by the way
                        // for all other branches
                        lastLabel = smallestLabel;
                        {
                            for (std::size_t n = 1; n < neighboursMin.size();
                                 n++) {
                                int label = neighboursMin[n];
                                assert(label <
                                       static_cast<int>(numberOfCells) + 2);
                                // we don't process it if it's the same label as
                                // the previous neighbor
                                if (label != lastLabel) {
                                    equivalentLabels[label] = smallestLabel;
                                    lastLabel = label;
                                }
                            }
                        }
                    }
 
                    // update current cell label
                    *_slice = smallestLabel;
                    neighboursVal.clear();
                }
 
                cellIndexToLabel[counter++] = *_slice;
            }
 
            if (counter == numberOfCells) break;
 
            std::swap(slice, oldSlice);
 
            nprogress.oneStep();
        }
    }
 
    // release some memory
    slice.resize(0);
    oldSlice.resize(0);
 
    if (progressCb) {
        progressCb->stop();
    }
 
    if (currentLabel < 2) {
        // No component found
        return -3;
    }
 
    // path compression (http://en.wikipedia.org/wiki/Union_find)
    assert(currentLabel < numberOfCells + 2);
    {
        for (std::size_t i = 2; i <= currentLabel; i++) {
            int label = equivalentLabels[i];
            assert(label < static_cast<int>(numberOfCells) + 2);
            while (equivalentLabels[label] > 1)  // equivalentLabels[0] == 0 !!!
            {
                label = equivalentLabels[label];
                assert(label < static_cast<int>(numberOfCells) + 2);
            }
            equivalentLabels[i] = label;
        }
    }
 
    // update leaves
    {
        for (std::size_t i = 0; i < numberOfCells; i++) {
            int label = cellIndexToLabel[i];
            assert(label < static_cast<int>(numberOfCells) + 2);
            if (equivalentLabels[label] > 1)
                cellIndexToLabel[i] = equivalentLabels[label];
        }
    }
 
    // hack: we use "equivalentLabels" to count how many components will have to
    // be created
    int numberOfComponents = 0;
    {
        std::fill(equivalentLabels.begin(), equivalentLabels.end(), 0);
 
        for (std::size_t i = 0; i < numberOfCells; i++) {
            assert(cellIndexToLabel[i] > 1 &&
                   cellIndexToLabel[i] < static_cast<int>(numberOfCells) + 2);
            equivalentLabels[cellIndexToLabel[i]] = 1;
        }
 
        // we create (following) indexes for each components
        for (std::size_t i = 2; i < numberOfCells + 2; i++)
            if (equivalentLabels[i] == 1)
                equivalentLabels[i] =
                        ++numberOfComponents;  // labels start at '1'
    }
    assert(equivalentLabels[0] == 0);
    assert(equivalentLabels[1] == 0);
 
    // we flag each component's points with its label
    {
        if (progressCb) {
            if (progressCb->textCanBeEdited()) {
                char buffer[256];
                sprintf(buffer, "Components: %i", numberOfComponents);
                progressCb->setMethodTitle("Connected Components Extraction");
                progressCb->setInfo(buffer);
            }
            progressCb->update(0);
            progressCb->start();
        }
        NormalizedProgress nprogress(progressCb,
                                     static_cast<unsigned>(numberOfCells));
 
        ReferenceCloud Y(m_theAssociatedCloud);
        for (std::size_t i = 0; i < numberOfCells; i++) {
            assert(cellIndexToLabel[i] < static_cast<int>(numberOfCells) + 2);
 
            const int& label = equivalentLabels[cellIndexToLabel[i]];
            assert(label > 0);
            getPointsInCell(ccCells[i].theCode, level, &Y, true);
            Y.placeIteratorAtBeginning();
            ScalarType d = static_cast<ScalarType>(label);
            for (unsigned j = 0; j < Y.size(); ++j) {
                Y.setCurrentPointScalarValue(d);
                Y.forwardIterator();
            }
 
            nprogress.oneStep();
        }
 
        if (progressCb) {
            progressCb->stop();
        }
    }
 
    return numberOfComponents;
}
 
/*** Octree-based cloud traversal mechanism ***/
 
DgmOctree::octreeCell::octreeCell(const DgmOctree* _parentOctree)
    : parentOctree(_parentOctree),
      truncatedCode(0),
      index(0),
      points(nullptr),
      level(0) {
    if (parentOctree && parentOctree->m_theAssociatedCloud) {
        points = new ReferenceCloud(parentOctree->m_theAssociatedCloud);
    } else {
        assert(false);
    }
}
 
DgmOctree::octreeCell::octreeCell(const octreeCell& cell)
    : parentOctree(cell.parentOctree),
      level(cell.level),
      truncatedCode(cell.truncatedCode),
      index(cell.index),
      points(nullptr) {
    // copy constructor shouldn't be used (we can't properly share the 'points'
    // reference)
    assert(false);
}
 
DgmOctree::octreeCell::~octreeCell() {
    if (points) delete points;
}
 
#ifdef ENABLE_MT_OCTREE
 
/*** FOR THE MULTI THREADING WRAPPER ***/
struct octreeCellDesc {
    DgmOctree::CellCode truncatedCode;
    unsigned i1, i2;
    unsigned char level;
};
 
static DgmOctree* s_octree_MT = nullptr;
static DgmOctree::octreeCellFunc s_func_MT = nullptr;
static void** s_userParams_MT = nullptr;
static GenericProgressCallback* s_progressCb_MT = nullptr;
static NormalizedProgress* s_normProgressCb_MT = nullptr;
static bool s_cellFunc_MT_success = true;
 
void LaunchOctreeCellFunc_MT(const octreeCellDesc& desc) {
    // skip cell if process is aborted/has failed
    if (!s_cellFunc_MT_success) {
        return;
    }
 
    const DgmOctree::cellsContainer& pointsAndCodes =
            s_octree_MT->pointsAndTheirCellCodes();
 
    // cell descriptor
    DgmOctree::octreeCell cell(s_octree_MT);
    cell.level = desc.level;
    cell.index = desc.i1;
    cell.truncatedCode = desc.truncatedCode;
    if (cell.points->reserve(desc.i2 - desc.i1 + 1)) {
        for (unsigned i = desc.i1; i <= desc.i2; ++i) {
            cell.points->addPointIndex(pointsAndCodes[i].theIndex);
        }
 
        s_cellFunc_MT_success &=
                (*s_func_MT)(cell, s_userParams_MT, s_normProgressCb_MT);
    } else {
        s_cellFunc_MT_success = false;
    }
 
    if (!s_cellFunc_MT_success) {
        // TODO: display a message to make clear that the cancel order has been
        // acknowledged!
        if (s_progressCb_MT) {
            if (s_progressCb_MT->textCanBeEdited()) {
                s_progressCb_MT->setInfo("Cancelling...");
            }
        }
 
        // if (s_normProgressCb_MT)
        //{
        //  if (!s_normProgressCb_MT->oneStep())
        //  {
        //      s_cellFunc_MT_success = false;
        //      return;
        //  }
        // }
    }
}
 
#endif
 
unsigned DgmOctree::executeFunctionForAllCellsAtLevel(
        unsigned char level,
        octreeCellFunc func,
        void** additionalParameters,
        bool multiThread /*=false*/,
        GenericProgressCallback* progressCb /*=0*/,
        const char* functionTitle /*=0*/,
        int maxThreadCount /*=0*/) {
    if (m_thePointsAndTheirCellCodes.empty()) return 0;
 
#ifdef ENABLE_MT_OCTREE
 
    // cells that will be processed by QtConcurrent::map
    const unsigned cellsNumber = getCellNumber(level);
    std::vector<octreeCellDesc> cells;
 
    if (multiThread) {
        try {
            cells.reserve(cellsNumber);
        } catch (const std::bad_alloc&) {
            // not enough memory
            // we use standard way (DGM TODO: we should warn the user!)
            multiThread = false;
        }
    }
 
    if (!multiThread)
#endif
    {
        // we get the maximum cell population for this level
        unsigned maxCellPopulation = m_maxCellPopulation[level];
 
        // cell descriptor (initialize it with first cell/point)
        octreeCell cell(this);
        if (!cell.points->reserve(maxCellPopulation))  // not enough memory
            return 0;
        cell.level = level;
        cell.index = 0;
 
        // binary shift for cell code truncation
        unsigned char bitDec = GET_BIT_SHIFT(level);
 
        // iterator on cell codes
        cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
        // init with first cell
        cell.truncatedCode = (p->theCode >> bitDec);
        cell.points->addPointIndex(p->theIndex);  // can't fail (see above)
        ++p;
 
        // number of cells for this level
        unsigned cellCount = getCellNumber(level);
 
        // progress bar
        if (progressCb) {
            if (progressCb->textCanBeEdited()) {
                if (functionTitle) {
                    progressCb->setMethodTitle(functionTitle);
                }
                char buffer[512];
                sprintf(buffer,
                        "Octree level %i\nCells: %u\nMean population: %3.2f "
                        "(+/-%3.2f)\nMax population: %u",
                        level, cellCount, m_averageCellPopulation[level],
                        m_stdDevCellPopulation[level],
                        m_maxCellPopulation[level]);
                progressCb->setInfo(buffer);
            }
            progressCb->update(0);
            progressCb->start();
        }
        NormalizedProgress nprogress(progressCb, m_theAssociatedCloud->size());
 
        bool result = true;
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        s_skippedPoints = 0;
        s_testedPoints = 0;
        s_jumps = 0.0;
        s_binarySearchCount = 0.0;
#endif
 
        // for each point
        for (; p != m_thePointsAndTheirCellCodes.end(); ++p) {
            // check if it belongs to the current cell
            CellCode nextCode = (p->theCode >> bitDec);
            if (nextCode != cell.truncatedCode) {
                // if not, we call the user function on the previous cell
                result = (*func)(cell, additionalParameters, &nprogress);
 
                if (!result) break;
 
                // and we start a new cell
                cell.index += cell.points->size();
                cell.points->clear();
                cell.truncatedCode = nextCode;
            }
 
            cell.points->addPointIndex(p->theIndex);  // can't fail (see above)
        }
 
        // don't forget last cell!
        if (result) result = (*func)(cell, additionalParameters, &nprogress);
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        FILE* fp = fopen("octree_log.txt", "at");
        if (fp) {
            fprintf(fp, "Function: %s\n",
                    functionTitle ? functionTitle : "unknown");
            fprintf(fp, "Tested:  %f (%3.1f %%)\n", s_testedPoints,
                    100.0 * s_testedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "skipped: %f (%3.1f %%)\n", s_skippedPoints,
                    100.0 * s_skippedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "Binary search count: %.0f\n", s_binarySearchCount);
            if (s_binarySearchCount > 0.0)
                fprintf(fp, "Mean jumps: %f\n", s_jumps / s_binarySearchCount);
            fprintf(fp, "\n");
            fclose(fp);
        }
#endif
 
        // if something went wrong, we return 0
        return (result ? cellCount : 0);
    }
#ifdef ENABLE_MT_OCTREE
    else {
        assert(cells.capacity() == cellsNumber);
 
        // binary shift for cell code truncation
        unsigned char bitDec = GET_BIT_SHIFT(level);
 
        // iterator on cell codes
        cellsContainer::const_iterator p = m_thePointsAndTheirCellCodes.begin();
 
        // cell descriptor (init. with first point/cell)
        octreeCellDesc cellDesc;
        cellDesc.i1 = 0;
        cellDesc.i2 = 0;
        cellDesc.level = level;
        cellDesc.truncatedCode = (p->theCode >> bitDec);
        ++p;
 
        // sweep through the octree
        for (; p != m_thePointsAndTheirCellCodes.end(); ++p) {
            CellCode nextCode = (p->theCode >> bitDec);
 
            if (nextCode != cellDesc.truncatedCode) {
                cells.push_back(cellDesc);
                cellDesc.i1 = cellDesc.i2 + 1;
            }
 
            cellDesc.truncatedCode = nextCode;
            ++cellDesc.i2;
        }
        // don't forget the last cell!
        cells.push_back(cellDesc);
 
        // static wrap
        s_octree_MT = this;
        s_func_MT = func;
        s_userParams_MT = additionalParameters;
        s_cellFunc_MT_success = true;
        s_progressCb_MT = progressCb;
        if (s_normProgressCb_MT) {
            delete s_normProgressCb_MT;
            s_normProgressCb_MT = 0;
        }
 
        // progress notification
        if (progressCb) {
            if (progressCb->textCanBeEdited()) {
                if (functionTitle) {
                    progressCb->setMethodTitle(functionTitle);
                }
                char buffer[512];
                sprintf(buffer,
                        "Octree level %i\nCells: %i\nAverage population: %3.2f "
                        "(+/-%3.2f)\nMax population: %u",
                        level, static_cast<int>(cells.size()),
                        m_averageCellPopulation[level],
                        m_stdDevCellPopulation[level],
                        m_maxCellPopulation[level]);
                progressCb->setInfo(buffer);
            }
            progressCb->update(0);
            s_normProgressCb_MT = new NormalizedProgress(
                    progressCb, m_theAssociatedCloud->size());
            progressCb->start();
        }
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        s_skippedPoints = 0;
        s_testedPoints = 0;
        s_jumps = 0.0;
        s_binarySearchCount = 0.0;
#endif
 
        if (maxThreadCount == 0) {
            maxThreadCount = QThread::idealThreadCount();
        }
        QThreadPool::globalInstance()->setMaxThreadCount(maxThreadCount);
        QtConcurrent::blockingMap(cells, LaunchOctreeCellFunc_MT);
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        FILE* fp = fopen("octree_log.txt", "at");
        if (fp) {
            fprintf(fp, "Function: %s\n",
                    functionTitle ? functionTitle : "unknown");
            fprintf(fp, "Tested:  %f (%3.1f %%)\n", s_testedPoints,
                    100.0 * s_testedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "skipped: %f (%3.1f %%)\n", s_skippedPoints,
                    100.0 * s_skippedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "Binary search count: %.0f\n", s_binarySearchCount);
            if (s_binarySearchCount > 0.0)
                fprintf(fp, "Mean jumps: %f\n", s_jumps / s_binarySearchCount);
            fprintf(fp, "\n");
            fclose(fp);
        }
#endif
 
        s_octree_MT = nullptr;
        s_func_MT = nullptr;
        s_userParams_MT = nullptr;
 
        if (progressCb) {
            progressCb->stop();
            if (s_normProgressCb_MT) delete s_normProgressCb_MT;
            s_normProgressCb_MT = nullptr;
            s_progressCb_MT = nullptr;
        }
 
        // if something went wrong, we clear everything and return 0!
        if (!s_cellFunc_MT_success) cells.clear();
 
        return static_cast<unsigned>(cells.size());
    }
#endif
}
 
// Down-top traversal (for standard and mutli-threaded versions)
#define ENABLE_DOWN_TOP_TRAVERSAL
#define ENABLE_DOWN_TOP_TRAVERSAL_MT
 
unsigned DgmOctree::executeFunctionForAllCellsStartingAtLevel(
        unsigned char startingLevel,
        octreeCellFunc func,
        void** additionalParameters,
        unsigned minNumberOfPointsPerCell,
        unsigned maxNumberOfPointsPerCell,
        bool multiThread /* = true*/,
        GenericProgressCallback* progressCb /*=0*/,
        const char* functionTitle /*=0*/,
        int maxThreadCount /*=0*/) {
    if (m_thePointsAndTheirCellCodes.empty()) return 0;
 
    const unsigned cellsNumber = getCellNumber(startingLevel);
 
#ifdef ENABLE_MT_OCTREE
 
    // cells that will be processed by QtConcurrent::map
    std::vector<octreeCellDesc> cells;
    if (multiThread) {
        try {
            cells.reserve(cellsNumber);  // at least!
        } catch (const std::bad_alloc&) {
            // not enough memory?
            // we use standard way (DGM TODO: we should warn the user!)
            multiThread = false;
        }
    }
 
    if (!multiThread)
#endif
    {
        // we get the maximum cell population for this level
        unsigned maxCellPopulation = m_maxCellPopulation[startingLevel];
 
        // cell descriptor
        octreeCell cell(this);
        if (!cell.points->reserve(maxCellPopulation))  // not enough memory
            return 0;
        cell.level = startingLevel;
        cell.index = 0;
 
        // progress notification
        if (progressCb) {
            if (progressCb->textCanBeEdited()) {
                if (functionTitle) {
                    progressCb->setMethodTitle(functionTitle);
                }
                char buffer[1024];
                sprintf(buffer,
                        "Octree levels %i - %i\nCells: %i - %i\nAverage "
                        "population: %3.2f (+/-%3.2f) - %3.2f (+/-%3.2f)\nMax "
                        "population: %u - %u",
                        startingLevel, MAX_OCTREE_LEVEL,
                        getCellNumber(startingLevel),
                        getCellNumber(MAX_OCTREE_LEVEL),
                        m_averageCellPopulation[startingLevel],
                        m_stdDevCellPopulation[startingLevel],
                        m_averageCellPopulation[MAX_OCTREE_LEVEL],
                        m_stdDevCellPopulation[MAX_OCTREE_LEVEL],
                        m_maxCellPopulation[startingLevel],
                        m_maxCellPopulation[MAX_OCTREE_LEVEL]);
                progressCb->setInfo(buffer);
            }
            progressCb->update(0);
            progressCb->start();
        }
#ifndef ENABLE_DOWN_TOP_TRAVERSAL
        NormalizedProgress nprogress(progressCb, m_theAssociatedCloud->size());
#endif
 
        // binary shift for cell code truncation at current level
        unsigned char currentBitDec = GET_BIT_SHIFT(startingLevel);
 
#ifdef ENABLE_DOWN_TOP_TRAVERSAL
        bool firstSubCell = true;
#else
        unsigned char shallowSteps = 0;
#endif
 
        // pointer on the current octree element
        cellsContainer::const_iterator startingElement =
                m_thePointsAndTheirCellCodes.begin();
 
        bool result = true;
 
        // let's sweep through the octree
        while (cell.index < m_numberOfProjectedPoints) {
            // new cell
            cell.truncatedCode = (startingElement->theCode >> currentBitDec);
            // we can already 'add' (virtually) the first point to the current
            // cell description struct
            unsigned elements = 1;
 
            // progress notification
#ifndef ENABLE_DOWN_TOP_TRAVERSAL
            // if (cell.level == startingLevel)
            //{
            //  if (!nprogress.oneStep())
            //  {
            //      result=false;
            //      break;
            //  }
            // }
#else
            // in this mode, we can't update progress notification regularly...
            if (progressCb) {
                progressCb->update(
                        100.0f * static_cast<float>(cell.index) /
                        static_cast<float>(m_numberOfProjectedPoints));
                if (progressCb->isCancelRequested()) {
                    result = false;
                    break;
                }
            }
#endif
 
            // let's test the following points
            for (cellsContainer::const_iterator p = startingElement + 1;
                 p != m_thePointsAndTheirCellCodes.end(); ++p) {
                // next point code (at current level of subdivision)
                CellCode currentTruncatedCode = (p->theCode >> currentBitDec);
                // same code? Then it belongs to the same cell
                if (currentTruncatedCode == cell.truncatedCode) {
                    // if we have reached the user specified limit
                    if (elements == maxNumberOfPointsPerCell) {
                        bool keepGoing = true;
 
                        // we should go deeper in the octree (as long as the
                        // current element belongs to the same cell as the first
                        // cell element - in which case the cell will still be
                        // too big)
                        while (cell.level < MAX_OCTREE_LEVEL) {
                            // next level
                            ++cell.level;
                            currentBitDec -= 3;
                            cell.truncatedCode =
                                    (startingElement->theCode >> currentBitDec);
 
                            // not the same cell anymore?
                            if (cell.truncatedCode !=
                                (p->theCode >> currentBitDec)) {
                                // we must re-check all the previous inserted
                                // points at this new level to determine the end
                                // of this new cell
                                p = startingElement;
                                elements = 1;
                                while (((++p)->theCode >> currentBitDec) ==
                                       cell.truncatedCode)
                                    ++elements;
 
                                // and we must stop point collection here
                                keepGoing = false;
 
#ifdef ENABLE_DOWN_TOP_TRAVERSAL
                                // in this case, the next cell won't be the
                                // first sub-cell!
                                firstSubCell = false;
#endif
                                break;
                            }
                        }
 
                        // we should stop point collection here
                        if (!keepGoing) break;
                    }
 
                    // otherwise we 'add' the point to the cell descriptor
                    ++elements;
                } else  // code is different --> not the same cell anymore
                {
#ifndef ENABLE_DOWN_TOP_TRAVERSAL
                    // we may have to go shallower ... as long as the parent
                    // cell is different
                    assert(shallowSteps == 0);
                    CellCode cellTruncatedCode = cell.truncatedCode;
                    while (cell.level > startingLevel + shallowSteps) {
                        cellTruncatedCode >>= 3;
                        currentTruncatedCode >>= 3;
                        // this cell and the following share the same parent
                        if (cellTruncatedCode == currentTruncatedCode) break;
                        ++shallowSteps;
                    }
 
                    // we must stop point collection here
                    break;
#else
                    // we are at the end of the cell
                    bool keepGoing = false;
                    // can we continue collecting points?
                    if (cell.level > startingLevel) {
                        // this cell and the following share the same parent?
                        if ((cell.truncatedCode >> 3) ==
                            (currentTruncatedCode >> 3)) {
                            // if this cell is the first one, and we don't have
                            // enough points we can simply proceed with its
                            // parent cell
                            if (firstSubCell &&
                                elements < minNumberOfPointsPerCell) {
                                // previous level
                                --cell.level;
                                currentBitDec += 3;
                                cell.truncatedCode >>= 3;
 
                                // we 'add' the point to the cell descriptor
                                ++elements;
                                // and we can continue collecting points
                                keepGoing = true;
                            }
 
                            // as this cell and the next one share the same
                            // parent, the next cell won't be the first
                            // sub-cell!
                            firstSubCell = false;
                        } else {
                            // as this cell and the next one have different
                            // parents, the next cell is the first sub-cell!
                            firstSubCell = true;
                        }
                    } else {
                        // at the ceiling level, all cells are considered as
                        // 'frist' sub-cells
                        firstSubCell = true;
                    }
 
                    // we must stop point collection here
                    if (!keepGoing) break;
#endif
                }
            }
 
            // we can now really 'add' the points to the cell descriptor
            cell.points->clear();
            // DGM: already done earlier
            /*if (!cell.points->reserve(elements)) //not enough memory
            {
            result=false;
            break;
            }
            */
            for (unsigned i = 0; i < elements; ++i) {
                cell.points->addPointIndex((startingElement++)->theIndex);
            }
 
            // call user method on current cell
            result = (*func)(cell, additionalParameters,
#ifndef ENABLE_DOWN_TOP_TRAVERSAL
                             &nProgress
#else
                             nullptr
#endif
            );
 
            if (!result) break;
 
            // proceed to next cell
            cell.index += elements;
 
#ifndef ENABLE_DOWN_TOP_TRAVERSAL
            if (shallowSteps) {
                // we should go shallower
                assert(cell.level - shallowSteps >= startingLevel);
                cell.level -= shallowSteps;
                currentBitDec += 3 * shallowSteps;
                shallowSteps = 0;
            }
#endif
        }
 
        if (progressCb) {
            progressCb->stop();
        }
 
        // if something went wrong, we return 0
        return (result ? cellsNumber : 0);
    }
#ifdef ENABLE_MT_OCTREE
    else {
        assert(cells.capacity() == cellsNumber);
 
        // cell descriptor (init. with first point/cell)
        octreeCellDesc cellDesc;
        cellDesc.i1 = 0;
        cellDesc.i2 = 0;
        cellDesc.level = startingLevel;
 
        // binary shift for cell code truncation at current level
        unsigned char currentBitDec = GET_BIT_SHIFT(startingLevel);
 
#ifdef ENABLE_DOWN_TOP_TRAVERSAL_MT
        bool firstSubCell = true;
#else
        unsigned char shallowSteps = 0;
#endif
        // pointer on the current octree element
        cellsContainer::const_iterator startingElement =
                m_thePointsAndTheirCellCodes.begin();
 
        // we compute some statistics on the fly
        unsigned long long popSum = 0;
        unsigned long long popSum2 = 0;
        unsigned long long maxPop = 0;
 
        // let's sweep through the octree
        while (cellDesc.i1 < m_numberOfProjectedPoints) {
            // new cell
            cellDesc.truncatedCode =
                    (startingElement->theCode >> currentBitDec);
            // we can already 'add' (virtually) the first point to the current
            // cell description struct
            unsigned elements = 1;
 
            // let's test the following points
            for (cellsContainer::const_iterator p = startingElement + 1;
                 p != m_thePointsAndTheirCellCodes.end(); ++p) {
                // next point code (at current level of subdivision)
                CellCode currentTruncatedCode = (p->theCode >> currentBitDec);
                // same code? Then it belongs to the same cell
                if (currentTruncatedCode == cellDesc.truncatedCode) {
                    // if we have reached the user specified limit
                    if (elements == maxNumberOfPointsPerCell) {
                        bool keepGoing = true;
 
                        // we should go deeper in the octree (as long as the
                        // current element belongs to the same cell as the first
                        // cell element - in which case the cell will still be
                        // too big)
                        while (cellDesc.level < MAX_OCTREE_LEVEL) {
                            // next level
                            ++cellDesc.level;
                            currentBitDec -= 3;
                            cellDesc.truncatedCode =
                                    (startingElement->theCode >> currentBitDec);
 
                            // not the same cell anymore?
                            if (cellDesc.truncatedCode !=
                                (p->theCode >> currentBitDec)) {
                                // we must re-check all the previously inserted
                                // points at this new level to determine the end
                                // of this new cell
                                p = startingElement;
                                elements = 1;
                                while (((++p)->theCode >> currentBitDec) ==
                                       cellDesc.truncatedCode)
                                    ++elements;
 
                                // and we must stop point collection here
                                keepGoing = false;
 
#ifdef ENABLE_DOWN_TOP_TRAVERSAL_MT
                                // in this case, the next cell won't be the
                                // first sub-cell!
                                firstSubCell = false;
#endif
                                break;
                            }
                        }
 
                        // we should stop point collection here
                        if (!keepGoing) break;
                    }
 
                    // otherwise we 'add' the point to the cell descriptor
                    ++elements;
                } else  // code is different --> not the same cell anymore
                {
#ifndef ENABLE_DOWN_TOP_TRAVERSAL_MT
                    // we may have to go shallower ... as long as the parent
                    // cell is different
                    assert(shallowSteps == 0);
                    CellCode cellTruncatedCode = cellDesc.truncatedCode;
                    while (cellDesc.level > startingLevel + shallowSteps) {
                        cellTruncatedCode >>= 3;
                        currentTruncatedCode >>= 3;
                        // this cell and the following share the same parent
                        if (cellTruncatedCode == currentTruncatedCode) break;
                        ++shallowSteps;
                    }
 
                    // we must stop point collection here
                    break;
#else
                    // we are at the end of the cell
                    bool keepGoing = false;
                    // can we continue collecting points?
                    if (cellDesc.level > startingLevel) {
                        // this cell and the following share the same parent?
                        if ((cellDesc.truncatedCode >> 3) ==
                            (currentTruncatedCode >> 3)) {
                            // if this cell is the first one, and we don't have
                            // enough points we can simply proceed with its
                            // parent cell
                            if (firstSubCell &&
                                elements < minNumberOfPointsPerCell) {
                                // previous level
                                --cellDesc.level;
                                currentBitDec += 3;
                                cellDesc.truncatedCode >>= 3;
 
                                // we 'add' the point to the cell descriptor
                                ++elements;
                                // and we can continue collecting points
                                keepGoing = true;
                            }
 
                            // as this cell and the next one share the same
                            // parent, the next cell won't be the first
                            // sub-cell!
                            firstSubCell = false;
                        } else {
                            // as this cell and the next one have different
                            // parents, the next cell is the first sub-cell!
                            firstSubCell = true;
                        }
                    } else {
                        // at the ceiling level, all cells are considered as
                        // 'frist' sub-cells
                        firstSubCell = true;
                    }
 
                    // we must stop point collection here
                    if (!keepGoing) break;
#endif
                }
            }
 
            // we can now 'add' this cell to the list
            cellDesc.i2 = cellDesc.i1 + (elements - 1);
            cells.push_back(cellDesc);
            popSum += static_cast<unsigned long long>(elements);
            popSum2 += static_cast<unsigned long long>(elements * elements);
            if (maxPop < elements) maxPop = elements;
 
            // proceed to next cell
            cellDesc.i1 += elements;
            startingElement += elements;
 
#ifndef ENABLE_DOWN_TOP_TRAVERSAL_MT
            if (shallowSteps) {
                // we should go shallower
                assert(cellDesc.level - shallowSteps >= startingLevel);
                cellDesc.level -= shallowSteps;
                currentBitDec += 3 * shallowSteps;
                shallowSteps = 0;
            }
#endif
        }
 
        // statistics
        double mean = static_cast<double>(popSum) / cells.size();
        double stddev = sqrt(static_cast<double>(popSum2 - popSum * popSum)) /
                        cells.size();
 
        // static wrap
        s_octree_MT = this;
        s_func_MT = func;
        s_userParams_MT = additionalParameters;
        s_cellFunc_MT_success = true;
        if (s_normProgressCb_MT) delete s_normProgressCb_MT;
        s_normProgressCb_MT = nullptr;
 
        // progress notification
        if (progressCb) {
            if (progressCb->textCanBeEdited()) {
                if (functionTitle) {
                    progressCb->setMethodTitle(functionTitle);
                }
                char buffer[1024];
                sprintf(buffer,
                        "Octree levels %i - %i\nCells: %i\nAverage population: "
                        "%3.2f (+/-%3.2f)\nMax population: %llu",
                        startingLevel, MAX_OCTREE_LEVEL,
                        static_cast<int>(cells.size()), mean, stddev, maxPop);
                progressCb->setInfo(buffer);
            }
            if (s_normProgressCb_MT) {
                delete s_normProgressCb_MT;
            }
            s_normProgressCb_MT = new NormalizedProgress(
                    progressCb, static_cast<unsigned>(cells.size()));
            progressCb->update(0);
            progressCb->start();
        }
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        s_skippedPoints = 0;
        s_testedPoints = 0;
        s_jumps = 0.0;
        s_binarySearchCount = 0.0;
#endif
 
        if (maxThreadCount == 0) {
            maxThreadCount = QThread::idealThreadCount();
        }
        QThreadPool::globalInstance()->setMaxThreadCount(maxThreadCount);
        QtConcurrent::blockingMap(cells, LaunchOctreeCellFunc_MT);
 
#ifdef COMPUTE_NN_SEARCH_STATISTICS
        FILE* fp = fopen("octree_log.txt", "at");
        if (fp) {
            fprintf(fp, "Function: %s\n",
                    functionTitle ? functionTitle : "unknown");
            fprintf(fp, "Tested:  %f (%3.1f %%)\n", s_testedPoints,
                    100.0 * s_testedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "skipped: %f (%3.1f %%)\n", s_skippedPoints,
                    100.0 * s_skippedPoints /
                            std::max(1.0, s_testedPoints + s_skippedPoints));
            fprintf(fp, "Binary search count: %.0f\n", s_binarySearchCount);
            if (s_binarySearchCount > 0.0)
                fprintf(fp, "Mean jumps: %f\n", s_jumps / s_binarySearchCount);
            fprintf(fp, "\n");
            fclose(fp);
        }
#endif
 
        s_octree_MT = nullptr;
        s_func_MT = nullptr;
        s_userParams_MT = nullptr;
 
        if (progressCb) {
            progressCb->stop();
            if (s_normProgressCb_MT) delete s_normProgressCb_MT;
            s_normProgressCb_MT = nullptr;
        }
 
        // if something went wrong, we clear everything and return 0!
        if (!s_cellFunc_MT_success) cells.resize(0);
 
        return static_cast<unsigned>(cells.size());
    }
#endif
}
 
bool DgmOctree::rayCast(const CCVector3& rayAxis,
                        const CCVector3& rayOrigin,
                        double maxRadiusOrFov,
                        bool isFOV,
                        RayCastProcess process,
                        std::vector<PointDescriptor>& output) const {
    if (m_thePointsAndTheirCellCodes.empty()) {
        // nothing to do
        assert(false);
        return false;
    }
 
    CCVector3 margin(0, 0, 0);
    double maxSqRadius = 0;
    if (!isFOV) {
        margin = CCVector3(1, 1, 1) *
                 static_cast<PointCoordinateType>(maxRadiusOrFov);
        maxSqRadius = maxRadiusOrFov * maxRadiusOrFov;
    }
 
    // first test with the total bounding box
    Ray<PointCoordinateType> ray(rayAxis, rayOrigin);
    if (!AABB<PointCoordinateType>(m_dimMin - margin, m_dimMax + margin)
                 .intersects(ray)) {
        // no intersection
        output.resize(0);
        return true;
    }
 
    // no need to go too deep
    const unsigned char maxLevel = findBestLevelForAGivenPopulationPerCell(10);
 
    // starting level of subdivision
    unsigned char level = 1;
    // binary shift for cell code truncation at current level
    unsigned char currentBitDec = GET_BIT_SHIFT(level);
    // current cell code
    CellCode currentCode = INVALID_CELL_CODE;
    // whether the current cell should be skipped or not
    bool skipThisCell = false;
    // smallest FOV (i.e. nearest point)
    double smallestOrderDist = -1.0;
 
#ifdef CV_DEBUG
    m_theAssociatedCloud->enableScalarField();
#endif
 
    // ray with origin expressed in the local coordinate system!
    Ray<PointCoordinateType> rayLocal(rayAxis, rayOrigin - m_dimMin);
 
    // let's sweep through the octree
    for (cellsContainer::const_iterator it =
                 m_thePointsAndTheirCellCodes.begin();
         it != m_thePointsAndTheirCellCodes.end(); ++it) {
        CellCode truncatedCode = (it->theCode >> currentBitDec);
 
        // new cell?
        if (truncatedCode != (currentCode >> currentBitDec)) {
            // look for the biggest 'parent' cell that englobes this cell and
            // the previous one (if any)
            while (level > 1) {
                unsigned char bitDec = GET_BIT_SHIFT(level - 1);
                if ((it->theCode >> bitDec) == (currentCode >> bitDec)) {
                    // same parent cell, we can stop here
                    break;
                }
                --level;
            }
 
            currentCode = it->theCode;
 
            // now try to go deeper with the new cell
            while (level < maxLevel) {
                Tuple3i cellPos;
                getCellPos(it->theCode, level, cellPos, false);
 
                // first test with the total bounding box
                const PointCoordinateType& halfCellSize =
                        getCellSize(level) / 2;
                CCVector3 cellCenter((2 * cellPos.x + 1) * halfCellSize,
                                     (2 * cellPos.y + 1) * halfCellSize,
                                     (2 * cellPos.z + 1) * halfCellSize);
 
                CCVector3 halfCell =
                        CCVector3(halfCellSize, halfCellSize, halfCellSize);
 
                if (isFOV) {
                    double radialSqDist, sqDistToOrigin;
                    rayLocal.squareDistances(cellCenter, radialSqDist,
                                             sqDistToOrigin);
 
                    double dx = sqrt(sqDistToOrigin);
                    double dy = std::max<double>(
                            0, sqrt(radialSqDist) - SQRT_3 * halfCellSize);
                    double fov_rad = atan2(dy, dx);
 
                    skipThisCell = (fov_rad > maxRadiusOrFov);
                } else {
                    skipThisCell = !AABB<PointCoordinateType>(
                                            cellCenter - halfCell - margin,
                                            cellCenter + halfCell + margin)
                                            .intersects(rayLocal);
                }
 
                if (skipThisCell)
                    break;
                else
                    ++level;
            }
            currentBitDec = GET_BIT_SHIFT(level);
        }
 
#ifdef CV_DEBUG
        m_theAssociatedCloud->setPointScalarValue(it->theIndex, level);
#endif
 
        if (!skipThisCell) {
            // test the point
            const CCVector3* P = m_theAssociatedCloud->getPoint(it->theIndex);
 
            double radialSqDist = ray.radialSquareDistance(*P);
            double orderDist = -1.0;
            bool isElligible = false;
 
            if (isFOV) {
                double sqDist = ray.squareDistanceToOrigin(*P);
                double fov_rad = atan2(sqrt(radialSqDist), sqrt(sqDist));
                isElligible = (fov_rad <= maxRadiusOrFov);
                orderDist = fov_rad;
#ifdef CV_DEBUG
                // m_theAssociatedCloud->setPointScalarValue(it->theIndex,
                // fov_rad);
                // m_theAssociatedCloud->setPointScalarValue(it->theIndex,
                // sqrt(sqDist));
#endif
            } else {
                isElligible = (radialSqDist <= maxSqRadius);
#ifdef CV_DEBUG
                // m_theAssociatedCloud->setPointScalarValue(it->theIndex,
                // sqrt(radialSqDist));
#endif
            }
 
            if (isElligible) {
                try {
                    switch (process) {
                        case RC_NEAREST_POINT:
 
                            if (orderDist < 0) {
                                // to give better chances to points that are
                                // closer to the viewer)
                                double sqDist = ray.squareDistanceToOrigin(*P);
                                orderDist = sqrt(radialSqDist * sqDist);
                            }
 
                            // keep only the 'nearest' point
                            if (output.empty()) {
                                output.resize(1,
                                              PointDescriptor(P, it->theIndex,
                                                              radialSqDist));
                                smallestOrderDist = orderDist;
                            } else {
                                if (orderDist < smallestOrderDist) {
                                    output.back() = PointDescriptor(
                                            P, it->theIndex, radialSqDist);
                                    smallestOrderDist = orderDist;
                                }
                            }
                            break;
 
                        case RC_CLOSE_POINTS:
 
                            // store all the points that are close enough to the
                            // ray
                            output.emplace_back(P, it->theIndex, radialSqDist);
                            break;
 
                        default:
                            assert(false);
                            return false;
                    }
                } catch (const std::bad_alloc&) {
                    // not enough memory
                    return false;
                }
            }
        }
    }
 
    return true;
}