Statistics
| Branch: | Revision:

root / rgbdslam / gicp / ann_1.1.1 / src / bd_pr_search.cpp @ 9240aaa3

History | View | Annotate | Download (2.67 KB)

1
//----------------------------------------------------------------------
2
// File:                        bd_pr_search.cpp
3
// Programmer:                David Mount
4
// Description:                Priority search for bd-trees
5
// Last modified:        01/04/05 (Version 1.0)
6
//----------------------------------------------------------------------
7
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
8
// David Mount.  All Rights Reserved.
9
// 
10
// This software and related documentation is part of the Approximate
11
// Nearest Neighbor Library (ANN).  This software is provided under
12
// the provisions of the Lesser GNU Public License (LGPL).  See the
13
// file ../ReadMe.txt for further information.
14
// 
15
// The University of Maryland (U.M.) and the authors make no
16
// representations about the suitability or fitness of this software for
17
// any purpose.  It is provided "as is" without express or implied
18
// warranty.
19
//----------------------------------------------------------------------
20
//History:
21
//        Revision 0.1  03/04/98
22
//                Initial release
23
//----------------------------------------------------------------------
24

    
25
#include "bd_tree.h"                                        // bd-tree declarations
26
#include "kd_pr_search.h"                                // kd priority search declarations
27

    
28
//----------------------------------------------------------------------
29
//        Approximate priority searching for bd-trees.
30
//                See the file kd_pr_search.cc for general information on the
31
//                approximate nearest neighbor priority search algorithm.  Here
32
//                we include the extensions for shrinking nodes.
33
//----------------------------------------------------------------------
34

    
35
//----------------------------------------------------------------------
36
//        bd_shrink::ann_search - search a shrinking node
37
//----------------------------------------------------------------------
38

    
39
void ANNbd_shrink::ann_pri_search(ANNdist box_dist)
40
{
41
        ANNdist inner_dist = 0;                                                // distance to inner box
42
        for (int i = 0; i < n_bnds; i++) {                        // is query point in the box?
43
                if (bnds[i].out(ANNprQ)) {                                // outside this bounding side?
44
                                                                                                // add to inner distance
45
                        inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNprQ));
46
                }
47
        }
48
        if (inner_dist <= box_dist) {                                // if inner box is closer
49
                if (child[ANN_OUT] != KD_TRIVIAL)                // enqueue outer if not trivial
50
                        ANNprBoxPQ->insert(box_dist,child[ANN_OUT]);
51
                                                                                                // continue with inner child
52
                child[ANN_IN]->ann_pri_search(inner_dist);
53
        }
54
        else {                                                                                // if outer box is closer
55
                if (child[ANN_IN] != KD_TRIVIAL)                // enqueue inner if not trivial
56
                        ANNprBoxPQ->insert(inner_dist,child[ANN_IN]);
57
                                                                                                // continue with outer child
58
                child[ANN_OUT]->ann_pri_search(box_dist);
59
        }
60
        ANN_FLOP(3*n_bnds)                                                        // increment floating ops
61
        ANN_SHR(1)                                                                        // one more shrinking node
62
}