Method findNearest

deps/opencv/modules/core/src/datastructs.cpp:3756–3896

int KDTree::findNearest(InputArray _vec, int K, int emax,
                        OutputArray _neighborsIdx, OutputArray _neighbors,
                        OutputArray _dist, OutputArray _labels) const

{
    Mat vecmat = _vec.getMat();
    CV_Assert( vecmat.isContinuous() && vecmat.type() == CV_32F && vecmat.total() == (size_t)points.cols );
    const float* vec = vecmat.ptr<float>();
    K = std::min(K, points.rows);
    int ptdims = points.cols;

    CV_Assert(K > 0 && (normType == NORM_L2 || normType == NORM_L1));

    AutoBuffer<uchar> _buf((K+1)*(sizeof(float) + sizeof(int)));
    int* idx = (int*)(uchar*)_buf;
    float* dist = (float*)(idx + K + 1);
    int i, j, ncount = 0, e = 0;

    int qsize = 0, maxqsize = 1 << 10;
    AutoBuffer<uchar> _pqueue(maxqsize*sizeof(PQueueElem));
    PQueueElem* pqueue = (PQueueElem*)(uchar*)_pqueue;
    emax = std::max(emax, 1);

    for( e = 0; e < emax; )
    {
        float d, alt_d = 0.f;
        int nidx;

        if( e == 0 )
            nidx = 0;
        else
        {
            // take the next node from the priority queue
            if( qsize == 0 )
                break;
            nidx = pqueue[0].idx;
            alt_d = pqueue[0].dist;
            if( --qsize > 0 )
            {
                std::swap(pqueue[0], pqueue[qsize]);
                d = pqueue[0].dist;
                for( i = 0;;)
                {
                    int left = i*2 + 1, right = i*2 + 2;
                    if( left >= qsize )
                        break;
                    if( right < qsize && pqueue[right].dist < pqueue[left].dist )
                        left = right;
                    if( pqueue[left].dist >= d )
                        break;
                    std::swap(pqueue[i], pqueue[left]);
                    i = left;
                }
            }

            if( ncount == K && alt_d > dist[ncount-1] )
                continue;
        }