source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/ann_1.1.1/src/kd_pr_search.cpp @ 37

//----------------------------------------------------------------------
// File:                        kd_pr_search.cpp
// Programmer:          Sunil Arya and David Mount
// Description:         Priority search for kd-trees
// Last modified:       01/04/05 (Version 1.0)
//----------------------------------------------------------------------
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
// David Mount.  All Rights Reserved.
// 
// This software and related documentation is part of the Approximate
// Nearest Neighbor Library (ANN).  This software is provided under
// the provisions of the Lesser GNU Public License (LGPL).  See the
// file ../ReadMe.txt for further information.
// 
// The University of Maryland (U.M.) and the authors make no
// representations about the suitability or fitness of this software for
// any purpose.  It is provided "as is" without express or implied
// warranty.
//----------------------------------------------------------------------
// History:
//      Revision 0.1  03/04/98
//              Initial release
//----------------------------------------------------------------------

#include "kd_pr_search.h"                               // kd priority search declarations

//----------------------------------------------------------------------
//      Approximate nearest neighbor searching by priority search.
//              The kd-tree is searched for an approximate nearest neighbor.
//              The point is returned through one of the arguments, and the
//              distance returned is the SQUARED distance to this point.
//
//              The method used for searching the kd-tree is called priority
//              search.  (It is described in Arya and Mount, ``Algorithms for
//              fast vector quantization,'' Proc. of DCC '93: Data Compression
//              Conference}, eds. J. A. Storer and M. Cohn, IEEE Press, 1993,
//              381--390.)
//
//              The cell of the kd-tree containing the query point is located,
//              and cells are visited in increasing order of distance from the
//              query point.  This is done by placing each subtree which has
//              NOT been visited in a priority queue, according to the closest
//              distance of the corresponding enclosing rectangle from the
//              query point.  The search stops when the distance to the nearest
//              remaining rectangle exceeds the distance to the nearest point
//              seen by a factor of more than 1/(1+eps). (Implying that any
//              point found subsequently in the search cannot be closer by more
//              than this factor.)
//
//              The main entry point is annkPriSearch() which sets things up and
//              then call the recursive routine ann_pri_search().  This is a
//              recursive routine which performs the processing for one node in
//              the kd-tree.  There are two versions of this virtual procedure,
//              one for splitting nodes and one for leaves. When a splitting node
//              is visited, we determine which child to continue the search on
//              (the closer one), and insert the other child into the priority
//              queue.  When a leaf is visited, we compute the distances to the
//              points in the buckets, and update information on the closest
//              points.
//
//              Some trickery is used to incrementally update the distance from
//              a kd-tree rectangle to the query point.  This comes about from
//              the fact that which each successive split, only one component
//              (along the dimension that is split) of the squared distance to
//              the child rectangle is different from the squared distance to
//              the parent rectangle.
//----------------------------------------------------------------------

//----------------------------------------------------------------------
//              To keep argument lists short, a number of global variables
//              are maintained which are common to all the recursive calls.
//              These are given below.
//----------------------------------------------------------------------

double                  ANNprEps;                               // the error bound
int                             ANNprDim;                               // dimension of space
ANNpoint                ANNprQ;                                 // query point
double                  ANNprMaxErr;                    // max tolerable squared error
ANNpointArray   ANNprPts;                               // the points
ANNpr_queue             *ANNprBoxPQ;                    // priority queue for boxes
ANNmin_k                *ANNprPointMK;                  // set of k closest points

//----------------------------------------------------------------------
//      annkPriSearch - priority search for k nearest neighbors
//----------------------------------------------------------------------

void ANNkd_tree::annkPriSearch(
        ANNpoint                        q,                              // query point
        int                                     k,                              // number of near neighbors to return
        ANNidxArray                     nn_idx,                 // nearest neighbor indices (returned)
        ANNdistArray            dd,                             // dist to near neighbors (returned)
        double                          eps)                    // error bound (ignored)
{
                                                                                // max tolerable squared error
        ANNprMaxErr = ANN_POW(1.0 + eps);
        ANN_FLOP(2)                                                     // increment floating ops

        ANNprDim = dim;                                         // copy arguments to static equivs
        ANNprQ = q;
        ANNprPts = pts;
        ANNptsVisited = 0;                                      // initialize count of points visited

        ANNprPointMK = new ANNmin_k(k);         // create set for closest k points

                                                                                // distance to root box
        ANNdist box_dist = annBoxDistance(q,
                                bnd_box_lo, bnd_box_hi, dim);

        ANNprBoxPQ = new ANNpr_queue(n_pts);// create priority queue for boxes
        ANNprBoxPQ->insert(box_dist, root); // insert root in priority queue

        while (ANNprBoxPQ->non_empty() &&
                (!(ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited))) {
                ANNkd_ptr np;                                   // next box from prior queue

                                                                                // extract closest box from queue
                ANNprBoxPQ->extr_min(box_dist, (void *&) np);

                ANN_FLOP(2)                                             // increment floating ops
                if (box_dist*ANNprMaxErr >= ANNprPointMK->max_key())
                        break;

                np->ann_pri_search(box_dist);   // search this subtree.
        }

        for (int i = 0; i < k; i++) {           // extract the k-th closest points
                dd[i] = ANNprPointMK->ith_smallest_key(i);
                nn_idx[i] = ANNprPointMK->ith_smallest_info(i);
        }

        delete ANNprPointMK;                            // deallocate closest point set
        delete ANNprBoxPQ;                                      // deallocate priority queue
}

//----------------------------------------------------------------------
//      kd_split::ann_pri_search - search a splitting node
//----------------------------------------------------------------------

void ANNkd_split::ann_pri_search(ANNdist box_dist)
{
        ANNdist new_dist;                                       // distance to child visited later
                                                                                // distance to cutting plane
        ANNcoord cut_diff = ANNprQ[cut_dim] - cut_val;

        if (cut_diff < 0) {                                     // left of cutting plane
                ANNcoord box_diff = cd_bnds[ANN_LO] - ANNprQ[cut_dim];
                if (box_diff < 0)                               // within bounds - ignore
                        box_diff = 0;
                                                                                // distance to further box
                new_dist = (ANNdist) ANN_SUM(box_dist,
                                ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                if (child[ANN_HI] != KD_TRIVIAL)// enqueue if not trivial
                        ANNprBoxPQ->insert(new_dist, child[ANN_HI]);
                                                                                // continue with closer child
                child[ANN_LO]->ann_pri_search(box_dist);
        }
        else {                                                          // right of cutting plane
                ANNcoord box_diff = ANNprQ[cut_dim] - cd_bnds[ANN_HI];
                if (box_diff < 0)                               // within bounds - ignore
                        box_diff = 0;
                                                                                // distance to further box
                new_dist = (ANNdist) ANN_SUM(box_dist,
                                ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                if (child[ANN_LO] != KD_TRIVIAL)// enqueue if not trivial
                        ANNprBoxPQ->insert(new_dist, child[ANN_LO]);
                                                                                // continue with closer child
                child[ANN_HI]->ann_pri_search(box_dist);
        }
        ANN_SPL(1)                                                      // one more splitting node visited
        ANN_FLOP(8)                                                     // increment floating ops
}

//----------------------------------------------------------------------
//      kd_leaf::ann_pri_search - search points in a leaf node
//
//              This is virtually identical to the ann_search for standard search.
//----------------------------------------------------------------------

void ANNkd_leaf::ann_pri_search(ANNdist box_dist)
{
        register ANNdist dist;                          // distance to data point
        register ANNcoord* pp;                          // data coordinate pointer
        register ANNcoord* qq;                          // query coordinate pointer
        register ANNdist min_dist;                      // distance to k-th closest point
        register ANNcoord t;
        register int d;

        min_dist = ANNprPointMK->max_key(); // k-th smallest distance so far

        for (int i = 0; i < n_pts; i++) {       // check points in bucket

                pp = ANNprPts[bkt[i]];                  // first coord of next data point
                qq = ANNprQ;                                    // first coord of query point
                dist = 0;

                for(d = 0; d < ANNprDim; d++) {
                        ANN_COORD(1)                            // one more coordinate hit
                        ANN_FLOP(4)                                     // increment floating ops

                        t = *(qq++) - *(pp++);          // compute length and adv coordinate
                                                                                // exceeds dist to k-th smallest?
                        if( (dist = ANN_SUM(dist, ANN_POW(t))) > min_dist) {
                                break;
                        }
                }

                if (d >= ANNprDim &&                                    // among the k best?
                   (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem
                                                                                                // add it to the list
                        ANNprPointMK->insert(dist, bkt[i]);
                        min_dist = ANNprPointMK->max_key();
                }
        }
        ANN_LEAF(1)                                                     // one more leaf node visited
        ANN_PTS(n_pts)                                          // increment points visited
        ANNptsVisited += n_pts;                         // increment number of points visited
}