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

//----------------------------------------------------------------------
// File:                        kd_fix_rad_search.cpp
// Programmer:          Sunil Arya and David Mount
// Description:         Standard kd-tree fixed-radius kNN search
// Last modified:       05/03/05 (Version 1.1)
//----------------------------------------------------------------------
// 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 1.1  05/03/05
//              Initial release
//----------------------------------------------------------------------

#include "kd_fix_rad_search.h"                  // kd fixed-radius search decls

//----------------------------------------------------------------------
//      Approximate fixed-radius k nearest neighbor search
//              The squared radius is provided, and this procedure finds the
//              k nearest neighbors within the radius, and returns the total
//              number of points lying within the radius.
//
//              The method used for searching the kd-tree is a variation of the
//              nearest neighbor search used in kd_search.cpp, except that the
//              radius of the search ball is known.  We refer the reader to that
//              file for the explanation of the recursive search procedure.
//----------------------------------------------------------------------

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

int                             ANNkdFRDim;                             // dimension of space
ANNpoint                ANNkdFRQ;                               // query point
ANNdist                 ANNkdFRSqRad;                   // squared radius search bound
double                  ANNkdFRMaxErr;                  // max tolerable squared error
ANNpointArray   ANNkdFRPts;                             // the points
ANNmin_k*               ANNkdFRPointMK;                 // set of k closest points
int                             ANNkdFRPtsVisited;              // total points visited
int                             ANNkdFRPtsInRange;              // number of points in the range

//----------------------------------------------------------------------
//      annkFRSearch - fixed radius search for k nearest neighbors
//----------------------------------------------------------------------

int ANNkd_tree::annkFRSearch(
        ANNpoint                        q,                              // the query point
        ANNdist                         sqRad,                  // squared radius search bound
        int                                     k,                              // number of near neighbors to return
        ANNidxArray                     nn_idx,                 // nearest neighbor indices (returned)
        ANNdistArray            dd,                             // the approximate nearest neighbor
        double                          eps)                    // the error bound
{
        ANNkdFRDim = dim;                                       // copy arguments to static equivs
        ANNkdFRQ = q;
        ANNkdFRSqRad = sqRad;
        ANNkdFRPts = pts;
        ANNkdFRPtsVisited = 0;                          // initialize count of points visited
        ANNkdFRPtsInRange = 0;                          // ...and points in the range

        ANNkdFRMaxErr = ANN_POW(1.0 + eps);
        ANN_FLOP(2)                                                     // increment floating op count

        ANNkdFRPointMK = new ANNmin_k(k);       // create set for closest k points
                                                                                // search starting at the root
        root->ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim));

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

        delete ANNkdFRPointMK;                          // deallocate closest point set
        return ANNkdFRPtsInRange;                       // return final point count
}

//----------------------------------------------------------------------
//      kd_split::ann_FR_search - search a splitting node
//              Note: This routine is similar in structure to the standard kNN
//              search.  It visits the subtree that is closer to the query point
//              first.  For fixed-radius search, there is no benefit in visiting
//              one subtree before the other, but we maintain the same basic
//              code structure for the sake of uniformity.
//----------------------------------------------------------------------

void ANNkd_split::ann_FR_search(ANNdist box_dist)
{
                                                                                // check dist calc term condition
        if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return;

                                                                                // distance to cutting plane
        ANNcoord cut_diff = ANNkdFRQ[cut_dim] - cut_val;

        if (cut_diff < 0) {                                     // left of cutting plane
                child[ANN_LO]->ann_FR_search(box_dist);// visit closer child first

                ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdFRQ[cut_dim];
                if (box_diff < 0)                               // within bounds - ignore
                        box_diff = 0;
                                                                                // distance to further box
                box_dist = (ANNdist) ANN_SUM(box_dist,
                                ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                                                                                // visit further child if in range
                if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
                        child[ANN_HI]->ann_FR_search(box_dist);

        }
        else {                                                          // right of cutting plane
                child[ANN_HI]->ann_FR_search(box_dist);// visit closer child first

                ANNcoord box_diff = ANNkdFRQ[cut_dim] - cd_bnds[ANN_HI];
                if (box_diff < 0)                               // within bounds - ignore
                        box_diff = 0;
                                                                                // distance to further box
                box_dist = (ANNdist) ANN_SUM(box_dist,
                                ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));

                                                                                // visit further child if close enough
                if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
                        child[ANN_LO]->ann_FR_search(box_dist);

        }
        ANN_FLOP(13)                                            // increment floating ops
        ANN_SPL(1)                                                      // one more splitting node visited
}

//----------------------------------------------------------------------
//      kd_leaf::ann_FR_search - search points in a leaf node
//              Note: The unreadability of this code is the result of
//              some fine tuning to replace indexing by pointer operations.
//----------------------------------------------------------------------

void ANNkd_leaf::ann_FR_search(ANNdist box_dist)
{
        register ANNdist dist;                          // distance to data point
        register ANNcoord* pp;                          // data coordinate pointer
        register ANNcoord* qq;                          // query coordinate pointer
        register ANNcoord t;
        register int d;

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

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

                for(d = 0; d < ANNkdFRDim; d++) {
                        ANN_COORD(1)                            // one more coordinate hit
                        ANN_FLOP(5)                                     // 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))) > ANNkdFRSqRad) {
                                break;
                        }
                }

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