KdTree.tpp

/*---------------------------------------------------------------------------*\
 *
 *  bitpit
 *
 *  Copyright (C) 2015-2021 OPTIMAD engineering Srl
 *
 *  -------------------------------------------------------------------------
 *  License
 *  This file is part of bitpit.
 *
 *  bitpit is free software: you can redistribute it and/or modify it
 *  under the terms of the GNU Lesser General Public License v3 (LGPL)
 *  as published by the Free Software Foundation.
 *
 *  bitpit is distributed in the hope that it will be useful, but WITHOUT
 *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 *  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
 *  License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with bitpit. If not, see <http://www.gnu.org/licenses/>.
 *
\*---------------------------------------------------------------------------*/
 
// ========================================================================== //
//                         - SORTING ALGORITHMS -                             //
//                                                                            //
// Functions for data sorting.                                                //
// ========================================================================== //
// INFO                                                                       //
// Author    : Alessandro Alaia                                               //
// Version   : v2.0                                                           //
//                                                                            //
// All rights reserved.                                                       //
// ========================================================================== //
 
namespace bitpit{
 
// ========================================================================== //
// TEMPLATE IMPLEMENTATIONS FOR KDNODE                                        //
// ========================================================================== //
 
// Constructor(s) =========================================================== //
 
// -------------------------------------------------------------------------- //
template<class T, class T1>
KdNode<T, T1>::KdNode(
    void
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// none
 
// Counters
// none
 
// ========================================================================== //
// SET DEFAULT VALUES                                                         //
// ========================================================================== //
reset();
 
return; }
 
// Methods ================================================================== //
 
// -------------------------------------------------------------------------- //
template<class T, class T1>
void KdNode<T, T1>::reset(
    void
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// none
 
// Counters
// none
 
// ========================================================================== //
// SET DEFAULT VALUES                                                         //
// ========================================================================== //
lchild_ = -1;
rchild_ = -1;
 
object_ = nullptr;
 
label = T1();
 
return; }
 
// ========================================================================== //
// TEMPLATE IMPLEMENTATIONS FOR KDTREE                                        //
// ========================================================================== //
 
// Constructors ============================================================= //
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
KdTree<d, T, T1>::KdTree(
    int      maxstack
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// none
 
// Counters
// none
 
// ========================================================================== //
// SET DEFAULT VALUES                                                         //
// ========================================================================== //
MAXSTK = maxstack;
n_nodes = 0;
 
// ========================================================================== //
// RESIZE NODE LIST                                                           //
// ========================================================================== //
increaseStack();
 
return; }
 
// Methods ================================================================== //
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
void KdTree<d, T, T1>::clear(
        void
        ) {
 
    // ========================================================================== //
    // VARIABLES DECLARATION                                                      //
    // ========================================================================== //
 
    // Local variables
    // none
 
    // Counters
    // none
 
    // ========================================================================== //
    // CLEAR CONTENT                                                              //
    // ========================================================================== //
    for (int i = 0; i < n_nodes; ++i) {
        nodes[i].reset();
    }
 
    n_nodes = 0;
 
    return;
}
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
int KdTree<d, T, T1>::exist(
    const T      *P_
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
bool             check = false;
int              index = -1;
int              prev_ = -1, next_ = 0;
int              lev = 0, dim;
 
// Counters
// none
 
// ========================================================================== //
// EXIT FOR EMPTY TREE                                                        //
// ========================================================================== //
if (n_nodes == 0) { return(index); };
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Find node on leaf -------------------------------------------------------- //
while ((next_ >= 0) && (!check)) {
    check = ((*P_) == (*(nodes[next_].object_)));
    prev_ = next_;
    dim = lev % d;
    if ((*P_)[dim] <= (*(nodes[next_].object_))[dim]) {
        next_ = nodes[next_].lchild_;
    }
    else {
        next_ = nodes[next_].rchild_;
    }
    lev++;
} //next
if (check) {
    index = prev_;
}
 
return(index); };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
int KdTree<d, T, T1>::exist(
    const T         *P_,
    T1              &label
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
bool             check = false;
int              index = -1;
int              prev_ = -1, next_ = 0;
int              lev = 0, dim;
 
// Counters
// none
 
// ========================================================================== //
// EXIT FOR EMPTY TREE                                                        //
// ========================================================================== //
if (n_nodes == 0) { return(index); };
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Find node on leaf -------------------------------------------------------- //
while ((next_ >= 0) && (!check)) {
    check = ((*P_) == (*(nodes[next_].object_)));
    prev_ = next_;
    dim = lev % d;
    if ((*P_)[dim] <= (*(nodes[next_].object_))[dim]) {
        next_ = nodes[next_].lchild_;
    }
    else {
        next_ = nodes[next_].rchild_;
    }
    lev++;
} //next
if (check) {
    index = prev_;
    label = nodes[prev_].label;
}
 
return(index); };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1 >
template< class T2>
int KdTree<d, T, T1>::hNeighbor(
    const T         *P_,
    T2               h,
    bool             debug,
    int              next_,
    int              lev
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
int              index_l = -1, index_r = -1;
int              prev_ = next_;
int              dim;
 
// Counters
// none
 
// ========================================================================== //
// EXIT FOR EMPTY TREE                                                        //
// ========================================================================== //
if (n_nodes == 0) { return(-1); };
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Check if root is in the h-neighbor of P_ --------------------------------- //
//if (debug) { cout << "visiting: " << prev_ << endl; }
if (distance<T2>(*(nodes[prev_].object_), *P_) <= h) { return(prev_); }
 
// Move on next branch ------------------------------------------------------ //
dim = lev % d;
if (((*(nodes[prev_].object_))[dim] >= (*P_)[dim] - h)
 && (nodes[prev_].lchild_ >= 0)) {
// if (nodes[prev_].lchild_ >= 0) {
    next_ = nodes[prev_].lchild_;
    index_l = hNeighbor(P_, h, debug, next_, lev+1);
}
if (((*(nodes[prev_].object_))[dim] <= (*P_)[dim] + h)
 && (nodes[prev_].rchild_ >= 0)) {
// if (nodes[prev_].rchild_ >= 0) {
    next_ = nodes[prev_].rchild_;
    index_r = hNeighbor(P_, h, debug, next_, lev+1);
}
 
//if (debug) { cout << "result is: " << max(index_l, index_r) << endl; }
return(std::max(index_l, index_r)); };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1 >
template< class T2>
int KdTree<d, T, T1>::hNeighbor(
    const T         *P_,
    T1              &label,
    T2               h,
    int              next_,
    int              lev
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// bool             check = false;
int              index_l = -1, index_r = -1;
// int              dim;
 
// Counters
// none
 
// // ========================================================================== //
// // EXIT FOR EMPTY TREE                                                        //
// // ========================================================================== //
// if (n_nodes == 0) { return(index); };
 
// // ========================================================================== //
// // MOVE ON TREE BRANCHES                                                      //
// // ========================================================================== //
 
// // Find node on leaf -------------------------------------------------------- //
// while ((next_ >= 0) && (!check)) {
    // check = (distance<T2>(*P_, *(nodes[prev_].object_)) <= h);
    // prev_ = next_;
    // dim = lev % d;
    // if ((*P_)[dim] <= (*(nodes[next_].object_))[dim]) {
        // next_ = nodes[next_].lchild_;
    // }
    // else {
        // next_ = nodes[next_].rchild_;
    // }
    // lev++;
// } //next
// if (check) {
    // index = prev_;
    // label = nodes[prev_].label;
// }
 
return(std::max(index_l, index_r)); };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
void KdTree<d, T, T1>::insert(
    T               *P_
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
bool             left;
int              prev_ = -1, next_ = 0;
int              lev = 0, dim;
 
// Counters
// none
 
// ========================================================================== //
// EXIT CONDITION FOR EMPTY TREE                                              //
// ========================================================================== //
if (n_nodes == 0) {
    nodes[0].object_ = P_;
    n_nodes++;
    return;
};
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Find node on leaf -------------------------------------------------------- //
while (next_ >= 0) {
    prev_ = next_;
    dim = lev % d;
    if ((*P_)[dim] <= (*(nodes[next_].object_))[dim]) {
        next_ = nodes[next_].lchild_;
        left = true;
    }
    else {
        next_ = nodes[next_].rchild_;
        left = false;
    }
    lev++;
} //next
 
// Insert new element ------------------------------------------------------- //
 
// Increase stack size
if (n_nodes+1 > nodes.size()) {
    increaseStack();
}
 
// Update parent
if (left) {
    nodes[prev_].lchild_ = n_nodes;
}
else {
    nodes[prev_].rchild_ = n_nodes;
}
 
// Insert children
nodes[n_nodes].object_ = P_;
n_nodes++;
 
return; };
 
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
void KdTree<d, T, T1>::insert(
    T               *P_,
    T1              &label
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
bool             left;
int              prev_ = -1, next_ = 0;
int              lev = 0, dim;
 
// Counters
// none
 
// ========================================================================== //
// QUIT FOR EMPTY TREE                                                        //
// ========================================================================== //
if (n_nodes == 0) {
    nodes[0].object_ = P_;
    nodes[0].label   = label;
    n_nodes++;
    return;
};
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Find node on leaf -------------------------------------------------------- //
while (next_ >= 0) {
    prev_ = next_;
    dim = lev % d;
    if ((*P_)[dim] <= (*(nodes[next_].object_))[dim]) {
        next_ = nodes[next_].lchild_;
        left = true;
    }
    else {
        next_ = nodes[next_].rchild_;
        left = false;
    }
    lev++;
} //next
 
// Insert new element ------------------------------------------------------- //
 
// Increase stack size
if (n_nodes+1 > (long) nodes.size()) {
    increaseStack();
}
 
// Update parent
if (left) {
    nodes[prev_].lchild_ = n_nodes;
}
else {
    nodes[prev_].rchild_ = n_nodes;
}
 
// Insert children
nodes[n_nodes].object_ = P_;
nodes[n_nodes].label   = label;
n_nodes++;
 
return; };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
void KdTree<d, T, T1>::increaseStack(
    void
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// none
 
// Counters
// none
 
// ========================================================================== //
// INCREASE STACK SIZE                                                        //
// ========================================================================== //
nodes.resize(nodes.size() + MAXSTK);
 
return; };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1>
void KdTree<d, T, T1>::decreaseStack(
    void
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
// none
 
// Counters
// none
 
// ========================================================================== //
// INCREASE STACK SIZE                                                        //
// ========================================================================== //
nodes.resize(max(MAXSTK, nodes.size() - MAXSTK));
 
return; };
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1 >
template< class T2>
T2 KdTree<d, T, T1>::distance(
    const T               &P1,
    const T               &P2
) const {
 
// Get the type of the coordinates
typedef typename std::remove_const<typename std::remove_reference<decltype(std::declval<T>()[0])>::type>::type CoordType;
 
// Evaluate the ndistance
std::array<CoordType, d> delta;
for (int i = 0; i < d; ++i) {
    delta[i] = P1[i] - P2[i];
}
 
return norm2(delta);
 
};
 
// -------------------------------------------------------------------------- //
template<int d, class T, class T1 >
template< class T2>
void KdTree<d, T, T1>::hNeighbors(
    const T         *P_,
    T2               h,
    std::vector<T1> *L_,
    std::vector<T1> *EX_,
    int              next_,
    int              lev
) {
 
// ========================================================================== //
// VARIABLES DECLARATION                                                      //
// ========================================================================== //
 
// Local variables
int              prev_ = next_;
int              dim;
 
// Counters
// none
 
// ========================================================================== //
// EXIT FOR EMPTY TREE                                                        //
// ========================================================================== //
if (n_nodes == 0) { return; };
 
// ========================================================================== //
// MOVE ON TREE BRANCHES                                                      //
// ========================================================================== //
 
// Check if root is in the h-neighbor of P_ --------------------------------- //
if (distance<T2>(*(nodes[prev_].object_), *P_) <= h) {
 
    if (EX_ != NULL){
        if (std::find(EX_->begin(), EX_->end(), nodes[prev_].label) == EX_->end() ) {
            L_->push_back(nodes[prev_].label);
        }
    }
    else{
        L_->push_back(nodes[prev_].label);
    }
}
 
 
 
// Move on next branch ------------------------------------------------------ //
dim = lev % d;
if (((*(nodes[prev_].object_))[dim] >= (*P_)[dim] - h)
 && (nodes[prev_].lchild_ >= 0)) {
    next_ = nodes[prev_].lchild_;
    hNeighbors(P_, h, L_, EX_, next_, lev+1);
}
if (((*(nodes[prev_].object_))[dim] <= (*P_)[dim] + h)
 && (nodes[prev_].rchild_ >= 0)) {
    next_ = nodes[prev_].rchild_;
    hNeighbors(P_, h, L_, EX_, next_, lev+1);
}
 
return ;
 
};
 
}