Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2026-09-27 09:15:00 

0001 /**
0002  *  @file   LCContent/include/LCUtility/KDTreeLinkerAlgoT.h
0003  *
0004  *  @brief  Header file for the kd tree linker algo template class
0005  *
0006  *  $Log: $
0007  */
0008 #ifndef LC_KD_TREE_LINKER_ALGO_TEMPLATED_H
0009 #define LC_KD_TREE_LINKER_ALGO_TEMPLATED_H
0010 
0011 #include "LCUtility/KDTreeLinkerToolsT.h"
0012 
0013 #include <vector>
0014 
0015 namespace lc_content {
0016 
0017 /**
0018  *  @brief  Class that implements the KDTree partition of 2D space and a closest point search algorithm
0019  */
0020 template <typename DATA, unsigned DIM = 2>
0021 class KDTreeLinkerAlgo {
0022 public:
0023   /**
0024    *  @brief  Default constructor
0025    */
0026   KDTreeLinkerAlgo();
0027 
0028   /**
0029    *  @brief  Destructor calls clear
0030    */
0031   ~KDTreeLinkerAlgo();
0032 
0033   /**
0034    *  @brief  Build the KD tree from the "eltList" in the space define by "region"
0035    *
0036    *  @param  eltList
0037    *  @param  region
0038    */
0039   void build(std::vector<KDTreeNodeInfoT<DATA, DIM>>& eltList, const KDTreeBoxT<DIM>& region);
0040 
0041   /**
0042    *  @brief  Search in the KDTree for all points that would be contained in the given searchbox
0043    *          The founded points are stored in resRecHitList
0044    *
0045    *  @param  searchBox
0046    *  @param  resRecHitList
0047    */
0048   void search(const KDTreeBoxT<DIM>& searchBox, std::vector<KDTreeNodeInfoT<DATA, DIM>>& resRecHitList);
0049 
0050   /**
0051    *  @brief  findNearestNeighbour
0052    *
0053    *  @param  point
0054    *  @param  result
0055    *  @param  distance
0056    */
0057   void findNearestNeighbour(const KDTreeNodeInfoT<DATA, DIM>& point, const KDTreeNodeInfoT<DATA, DIM>*& result,
0058                             float& distance);
0059 
0060   /**
0061    *  @brief  Whether the tree is empty
0062    *
0063    *  @return boolean
0064    */
0065   bool empty();
0066 
0067   /**
0068    *  @brief  Return the number of nodes + leaves in the tree (nElements should be (size() +1) / 2)
0069    *
0070    *  @return the number of nodes + leaves in the tree
0071    */
0072   int size();
0073 
0074   /**
0075    *  @brief  Clear all allocated structures
0076    */
0077   void clear();
0078 
0079 private:
0080   /**
0081    *  @brief  Get the next node from the node pool
0082    *
0083    *  @return the next node from the node pool
0084    */
0085   KDTreeNodeT<DATA, DIM>* getNextNode();
0086 
0087   /**
0088    *  @brief  Fast median search with Wirth algorithm in eltList between low and high indexes.
0089    *
0090    *  @param  low
0091    *  @param  high
0092    *  @param  treeDepth
0093    */
0094   int medianSearch(int low, int high, int treeDepth);
0095 
0096   /**
0097    *  @brief  Recursive kdtree builder. Is called by build()
0098    *
0099    *  @param  low
0100    *  @param  high
0101    *  @param  depth
0102    *  @param  region
0103    */
0104   KDTreeNodeT<DATA, DIM>* recBuild(int low, int high, int depth, const KDTreeBoxT<DIM>& region);
0105 
0106   /**
0107    *  @brief  Recursive kdtree search. Is called by search()
0108    *
0109    *  @param  current
0110    *  @param  trackBox
0111    */
0112   void recSearch(const KDTreeNodeT<DATA, DIM>* current, const KDTreeBoxT<DIM>& trackBox);
0113 
0114   /**
0115    *  @brief  Recursive nearest neighbour search. Is called by findNearestNeighbour()
0116    *
0117    *  @param  depth
0118    *  @param  current
0119    *  @param  point
0120    *  @param  best_match
0121    *  @param  best_dist
0122    */
0123   void recNearestNeighbour(unsigned depth, const KDTreeNodeT<DATA, DIM>* current,
0124                            const KDTreeNodeInfoT<DATA, DIM>& point, const KDTreeNodeT<DATA, DIM>*& best_match,
0125                            float& best_dist);
0126 
0127   /**
0128    *  @brief  Add all elements of an subtree to the closest elements. Used during the recSearch().
0129    *
0130    *  @param  current
0131    */
0132   void addSubtree(const KDTreeNodeT<DATA, DIM>* current);
0133 
0134   /**
0135    *  @brief  dist2
0136    *
0137    *  @param  a
0138    *  @param  b
0139    *
0140    *  @return dist2
0141    */
0142   float dist2(const KDTreeNodeInfoT<DATA, DIM>& a, const KDTreeNodeInfoT<DATA, DIM>& b) const;
0143 
0144   /**
0145    *  @brief  Frees the KDTree.
0146    */
0147   void clearTree();
0148 
0149   KDTreeNodeT<DATA, DIM>* root_;     ///< The KDTree root
0150   KDTreeNodeT<DATA, DIM>* nodePool_; ///< Node pool allows us to do just 1 call to new for each tree building
0151   int nodePoolSize_;                 ///< The node pool size
0152   int nodePoolPos_;                  ///< The node pool position
0153 
0154   std::vector<KDTreeNodeInfoT<DATA, DIM>>* closestNeighbour; ///< The closest neighbour
0155   std::vector<KDTreeNodeInfoT<DATA, DIM>>* initialEltList;   ///< The initial element list
0156 };
0157 
0158 //------------------------------------------------------------------------------------------------------------------------------------------
0159 //------------------------------------------------------------------------------------------------------------------------------------------
0160 
0161 template <typename DATA, unsigned DIM>
0162 inline KDTreeLinkerAlgo<DATA, DIM>::KDTreeLinkerAlgo()
0163     : root_(nullptr), nodePool_(nullptr), nodePoolSize_(-1), nodePoolPos_(-1), closestNeighbour(nullptr),
0164       initialEltList(nullptr) {}
0165 
0166 //------------------------------------------------------------------------------------------------------------------------------------------
0167 
0168 template <typename DATA, unsigned DIM>
0169 inline KDTreeLinkerAlgo<DATA, DIM>::~KDTreeLinkerAlgo() {
0170   this->clear();
0171 }
0172 
0173 //------------------------------------------------------------------------------------------------------------------------------------------
0174 
0175 template <typename DATA, unsigned DIM>
0176 inline void KDTreeLinkerAlgo<DATA, DIM>::build(std::vector<KDTreeNodeInfoT<DATA, DIM>>& eltList,
0177                                                const KDTreeBoxT<DIM>& region) {
0178   if (eltList.size()) {
0179     initialEltList = &eltList;
0180     const size_t mysize = initialEltList->size();
0181 
0182     nodePoolSize_ = mysize * 2 - 1;
0183     nodePool_ = new KDTreeNodeT<DATA, DIM>[nodePoolSize_];
0184 
0185     // Here we build the KDTree
0186     root_ = this->recBuild(0, mysize, 0, region);
0187     initialEltList = nullptr;
0188   }
0189 }
0190 
0191 //------------------------------------------------------------------------------------------------------------------------------------------
0192 
0193 template <typename DATA, unsigned DIM>
0194 inline int KDTreeLinkerAlgo<DATA, DIM>::medianSearch(int low, int high, int treeDepth) {
0195   // We should have at least 1 element to calculate the median...
0196   // assert(low < high);
0197 
0198   const int nbrElts = high - low;
0199   int median = nbrElts / 2 - (1 - 1 * (nbrElts & 1));
0200   median += low;
0201 
0202   int l = low;
0203   int m = high - 1;
0204 
0205   while (l < m) {
0206     KDTreeNodeInfoT<DATA, DIM> elt = (*initialEltList)[median];
0207     int i = l;
0208     int j = m;
0209 
0210     do {
0211       // The even depth is associated to dim1 dimension, the odd one to dim2 dimension
0212       const unsigned thedim = treeDepth % DIM;
0213       while ((*initialEltList)[i].dims[thedim] < elt.dims[thedim])
0214         ++i;
0215       while ((*initialEltList)[j].dims[thedim] > elt.dims[thedim])
0216         --j;
0217 
0218       if (i <= j) {
0219         std::swap((*initialEltList)[i], (*initialEltList)[j]);
0220         i++;
0221         j--;
0222       }
0223     } while (i <= j);
0224 
0225     if (j < median)
0226       l = i;
0227     if (i > median)
0228       m = j;
0229   }
0230 
0231   return median;
0232 }
0233 
0234 //------------------------------------------------------------------------------------------------------------------------------------------
0235 
0236 template <typename DATA, unsigned DIM>
0237 inline void KDTreeLinkerAlgo<DATA, DIM>::search(const KDTreeBoxT<DIM>& trackBox,
0238                                                 std::vector<KDTreeNodeInfoT<DATA, DIM>>& recHits) {
0239   if (root_) {
0240     closestNeighbour = &recHits;
0241     this->recSearch(root_, trackBox);
0242     closestNeighbour = nullptr;
0243   }
0244 }
0245 
0246 //------------------------------------------------------------------------------------------------------------------------------------------
0247 
0248 template <typename DATA, unsigned DIM>
0249 inline void KDTreeLinkerAlgo<DATA, DIM>::recSearch(const KDTreeNodeT<DATA, DIM>* current,
0250                                                    const KDTreeBoxT<DIM>& trackBox) {
0251   // By construction, current can't be null
0252   // assert(current != 0);
0253   // By Construction, a node can't have just 1 son.
0254   // assert (!(((current->left == 0) && (current->right != 0)) || ((current->left != 0) && (current->right == 0))));
0255 
0256   if ((current->left == nullptr) && (current->right == nullptr)) {
0257     // Leaf case
0258     // If point inside the rectangle/area
0259     bool isInside = true;
0260 
0261     for (unsigned i = 0; i < DIM; ++i) {
0262       const auto thedim = current->info.dims[i];
0263       isInside = isInside && thedim >= trackBox.dimmin[i] && thedim <= trackBox.dimmax[i];
0264     }
0265 
0266     if (isInside)
0267       closestNeighbour->push_back(current->info);
0268   } else {
0269     // Node case
0270     // If region( v->left ) is fully contained in the rectangle
0271     bool isFullyContained = true;
0272     bool hasIntersection = true;
0273 
0274     for (unsigned i = 0; i < DIM; ++i) {
0275       const auto regionmin = current->left->region.dimmin[i];
0276       const auto regionmax = current->left->region.dimmax[i];
0277       isFullyContained = isFullyContained && (regionmin >= trackBox.dimmin[i] && regionmax <= trackBox.dimmax[i]);
0278       hasIntersection = hasIntersection && (regionmin < trackBox.dimmax[i] && regionmax > trackBox.dimmin[i]);
0279     }
0280 
0281     if (isFullyContained) {
0282       this->addSubtree(current->left);
0283     } else if (hasIntersection) {
0284       this->recSearch(current->left, trackBox);
0285     }
0286 
0287     // if region( v->right ) is fully contained in the rectangle
0288     isFullyContained = true;
0289     hasIntersection = true;
0290 
0291     for (unsigned i = 0; i < DIM; ++i) {
0292       const auto regionmin = current->right->region.dimmin[i];
0293       const auto regionmax = current->right->region.dimmax[i];
0294       isFullyContained = isFullyContained && (regionmin >= trackBox.dimmin[i] && regionmax <= trackBox.dimmax[i]);
0295       hasIntersection = hasIntersection && (regionmin < trackBox.dimmax[i] && regionmax > trackBox.dimmin[i]);
0296     }
0297 
0298     if (isFullyContained) {
0299       this->addSubtree(current->right);
0300     } else if (hasIntersection) {
0301       this->recSearch(current->right, trackBox);
0302     }
0303   }
0304 }
0305 
0306 //------------------------------------------------------------------------------------------------------------------------------------------
0307 
0308 template <typename DATA, unsigned DIM>
0309 inline void KDTreeLinkerAlgo<DATA, DIM>::findNearestNeighbour(const KDTreeNodeInfoT<DATA, DIM>& point,
0310                                                               const KDTreeNodeInfoT<DATA, DIM>*& result,
0311                                                               float& distance) {
0312   if (nullptr != result || distance != std::numeric_limits<float>::max()) {
0313     result = nullptr;
0314     distance = std::numeric_limits<float>::max();
0315   }
0316 
0317   if (root_) {
0318     const KDTreeNodeT<DATA, DIM>* best_match = nullptr;
0319     this->recNearestNeighbour(0, root_, point, best_match, distance);
0320 
0321     if (distance != std::numeric_limits<float>::max()) {
0322       result = &(best_match->info);
0323       distance = std::sqrt(distance);
0324     }
0325   }
0326 }
0327 
0328 //------------------------------------------------------------------------------------------------------------------------------------------
0329 
0330 template <typename DATA, unsigned DIM>
0331 inline void KDTreeLinkerAlgo<DATA, DIM>::recNearestNeighbour(unsigned int depth, const KDTreeNodeT<DATA, DIM>* current,
0332                                                              const KDTreeNodeInfoT<DATA, DIM>& point,
0333                                                              const KDTreeNodeT<DATA, DIM>*& best_match,
0334                                                              float& best_dist) {
0335   const unsigned int current_dim = depth % DIM;
0336 
0337   if (current->left == nullptr && current->right == nullptr) {
0338     best_match = current;
0339     best_dist = this->dist2(point, best_match->info);
0340     return;
0341   } else {
0342     const float dist_to_axis = point.dims[current_dim] - current->info.dims[current_dim];
0343 
0344     if (dist_to_axis < 0.f) {
0345       this->recNearestNeighbour(depth + 1, current->left, point, best_match, best_dist);
0346     } else {
0347       this->recNearestNeighbour(depth + 1, current->right, point, best_match, best_dist);
0348     }
0349 
0350     // If we're here we're returned so best_dist is filled. Compare to this node and see if it's a better match. If it
0351     // is, update result
0352     const float dist_current = this->dist2(point, current->info);
0353 
0354     if (dist_current < best_dist) {
0355       best_dist = dist_current;
0356       best_match = current;
0357     }
0358 
0359     // Now we see if the radius to best crosses the splitting axis
0360     if (best_dist > dist_to_axis * dist_to_axis) {
0361       // if it does we traverse the other side of the axis to check for a new best
0362       const KDTreeNodeT<DATA, DIM>* check_best = best_match;
0363       float check_dist = best_dist;
0364 
0365       if (dist_to_axis < 0.f) {
0366         this->recNearestNeighbour(depth + 1, current->right, point, check_best, check_dist);
0367       } else {
0368         this->recNearestNeighbour(depth + 1, current->left, point, check_best, check_dist);
0369       }
0370 
0371       if (check_dist < best_dist) {
0372         best_dist = check_dist;
0373         best_match = check_best;
0374       }
0375     }
0376     return;
0377   }
0378 }
0379 
0380 //------------------------------------------------------------------------------------------------------------------------------------------
0381 
0382 template <typename DATA, unsigned DIM>
0383 inline void KDTreeLinkerAlgo<DATA, DIM>::addSubtree(const KDTreeNodeT<DATA, DIM>* current) {
0384   // By construction, current can't be null
0385   // assert(current != 0);
0386 
0387   if ((current->left == nullptr) && (current->right == nullptr)) {
0388     // Leaf case
0389     closestNeighbour->push_back(current->info);
0390   } else {
0391     // Node case
0392     this->addSubtree(current->left);
0393     this->addSubtree(current->right);
0394   }
0395 }
0396 
0397 //------------------------------------------------------------------------------------------------------------------------------------------
0398 
0399 template <typename DATA, unsigned DIM>
0400 inline float KDTreeLinkerAlgo<DATA, DIM>::dist2(const KDTreeNodeInfoT<DATA, DIM>& a,
0401                                                 const KDTreeNodeInfoT<DATA, DIM>& b) const {
0402   double d = 0.;
0403 
0404   for (unsigned i = 0; i < DIM; ++i) {
0405     const double diff = a.dims[i] - b.dims[i];
0406     d += diff * diff;
0407   }
0408 
0409   return (float)d;
0410 }
0411 
0412 //------------------------------------------------------------------------------------------------------------------------------------------
0413 
0414 template <typename DATA, unsigned DIM>
0415 inline void KDTreeLinkerAlgo<DATA, DIM>::clearTree() {
0416   delete[] nodePool_;
0417   nodePool_ = nullptr;
0418   root_ = nullptr;
0419   nodePoolSize_ = -1;
0420   nodePoolPos_ = -1;
0421 }
0422 
0423 //------------------------------------------------------------------------------------------------------------------------------------------
0424 
0425 template <typename DATA, unsigned DIM>
0426 inline bool KDTreeLinkerAlgo<DATA, DIM>::empty() {
0427   return (nodePoolPos_ == -1);
0428 }
0429 
0430 //------------------------------------------------------------------------------------------------------------------------------------------
0431 
0432 template <typename DATA, unsigned DIM>
0433 inline int KDTreeLinkerAlgo<DATA, DIM>::size() {
0434   return (nodePoolPos_ + 1);
0435 }
0436 
0437 //------------------------------------------------------------------------------------------------------------------------------------------
0438 
0439 template <typename DATA, unsigned DIM>
0440 inline void KDTreeLinkerAlgo<DATA, DIM>::clear() {
0441   if (root_)
0442     this->clearTree();
0443 }
0444 
0445 //------------------------------------------------------------------------------------------------------------------------------------------
0446 
0447 template <typename DATA, unsigned DIM>
0448 inline KDTreeNodeT<DATA, DIM>* KDTreeLinkerAlgo<DATA, DIM>::getNextNode() {
0449   ++nodePoolPos_;
0450 
0451   // The tree size is exactly 2 * nbrElts - 1 and this is the total allocated memory.
0452   // If we have used more than that....there is a big problem.
0453   // assert(nodePoolPos_ < nodePoolSize_);
0454 
0455   return &(nodePool_[nodePoolPos_]);
0456 }
0457 
0458 //------------------------------------------------------------------------------------------------------------------------------------------
0459 
0460 template <typename DATA, unsigned DIM>
0461 inline KDTreeNodeT<DATA, DIM>* KDTreeLinkerAlgo<DATA, DIM>::recBuild(int low, int high, int depth,
0462                                                                      const KDTreeBoxT<DIM>& region) {
0463   const int portionSize = high - low;
0464 
0465   // By construction, portionSize > 0 can't happen.
0466   // assert(portionSize > 0);
0467 
0468   if (portionSize == 1) {
0469     // Leaf case
0470     KDTreeNodeT<DATA, DIM>* leaf = this->getNextNode();
0471     leaf->setAttributs(region, (*initialEltList)[low]);
0472     return leaf;
0473   } else {
0474     // The even depth is associated to dim1 dimension, the odd one to dim2 dimension
0475     int medianId = this->medianSearch(low, high, depth);
0476 
0477     // We create the node
0478     KDTreeNodeT<DATA, DIM>* node = this->getNextNode();
0479     node->setAttributs(region);
0480     node->info = (*initialEltList)[medianId];
0481 
0482     // Here we split into 2 halfplanes the current plane
0483     KDTreeBoxT<DIM> leftRegion = region;
0484     KDTreeBoxT<DIM> rightRegion = region;
0485 
0486     const unsigned thedim = depth % DIM;
0487     auto medianVal = (*initialEltList)[medianId].dims[thedim];
0488     leftRegion.dimmax[thedim] = medianVal;
0489     rightRegion.dimmin[thedim] = medianVal;
0490 
0491     ++depth;
0492     ++medianId;
0493 
0494     // We recursively build the son nodes
0495     node->left = this->recBuild(low, medianId, depth, leftRegion);
0496     node->right = this->recBuild(medianId, high, depth, rightRegion);
0497     return node;
0498   }
0499 }
0500 
0501 } // namespace lc_content
0502 
0503 #endif // LC_KD_TREE_LINKER_ALGO_TEMPLATED_H
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!