include/Geant4/G4KDTree.icc

0001 //
0002 // ********************************************************************
0003 // * License and Disclaimer                                           *
0004 // *                                                                  *
0005 // * The  Geant4 software  is  copyright of the Copyright Holders  of *
0006 // * the Geant4 Collaboration.  It is provided  under  the terms  and *
0007 // * conditions of the Geant4 Software License,  included in the file *
0008 // * LICENSE and available at  http://cern.ch/geant4/license .  These *
0009 // * include a list of copyright holders.                             *
0010 // *                                                                  *
0011 // * Neither the authors of this software system, nor their employing *
0012 // * institutes,nor the agencies providing financial support for this *
0013 // * work  make  any representation or  warranty, express or implied, *
0014 // * regarding  this  software system or assume any liability for its *
0015 // * use.  Please see the license in the file  LICENSE  and URL above *
0016 // * for the full disclaimer and the limitation of liability.         *
0017 // *                                                                  *
0018 // * This  code  implementation is the result of  the  scientific and *
0019 // * technical work of the GEANT4 collaboration.                      *
0020 // * By using,  copying,  modifying or  distributing the software (or *
0021 // * any work based  on the software)  you  agree  to acknowledge its *
0022 // * use  in  resulting  scientific  publications,  and indicate your *
0023 // * acceptance of all terms of the Geant4 Software license.          *
0024 // ********************************************************************
0025 //
0026 //
0027 // Author: Mathieu Karamitros (kara (AT) cenbg . in2p3 . fr)
0028 //
0029 // History:
0030 // -----------
0031 // 10 Oct 2011 M.Karamitros created
0032 //
0033 // -------------------------------------------------------------------
0034 /*
0035  * Based on ``kdtree'', a library for working with kd-trees.
0036  * Copyright (C) 2007-2009 John Tsiombikas <nuclear@siggraph.org>
0037  * The original open-source version of this code
0038  * may be found at http://code.google.com/p/kdtree/
0039  *
0040  * Redistribution and use in source and binary forms, with or without
0041  * modification, are permitted provided that the following conditions are
0042  * met:
0043  * 1. Redistributions of source code must retain the above copyright
0044  * notice, this
0045  * list of conditions and the following disclaimer.
0046  * 2. Redistributions in binary form must reproduce the above copyright
0047  * notice,
0048  *  this list of conditions and the following disclaimer in the
0049  * documentation
0050  *  and/or other materials provided with the distribution.
0051  * 3. The name of the author may not be used to endorse or promote products
0052  *  derived from this software without specific prior written permission.
0053  *
0054  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
0055  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
0056  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
0057  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
0058  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
0059  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
0060  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
0061  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
0062  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
0063  */
0064 /* single nearest neighbor search written by Tamas Nepusz
0065  * <tamas@cs.rhul.ac.uk>
0066  */
0067
0068 template<typename PointT>
0069   G4KDNode_Base* G4KDTree::InsertMap(PointT* point)
0070   {
0071     auto  node = new G4KDNode<PointT>(this, point, 0);
0072     this->__InsertMap(node);
0073     return node;
0074   }
0075
0076 template<typename PointT>
0077   G4KDNode_Base* G4KDTree::Insert(PointT* pos)
0078   {
0079     G4KDNode_Base* node = nullptr;
0080     if (!fRoot)
0081     {
0082       fRoot = new G4KDNode<PointT>(this, pos, nullptr);
0083       node = fRoot;
0084       fNbNodes = 0;
0085       fNbNodes++;
0086       fNbActiveNodes++;
0087     }
0088     else
0089     {
0090       if ((node = fRoot->Insert<PointT>(pos)))
0091       {
0092         fNbNodes++;
0093         fNbActiveNodes++;
0094       }
0095     }
0096
0097     if (fRect == nullptr)
0098     {
0099       fRect = new HyperRect(fDim);
0100       fRect->SetMinMax(*pos, *pos);
0101     }
0102     else
0103     {
0104       fRect->Extend(*pos);
0105     }
0106
0107     return node;
0108   }
0109
0110 template<typename PointT>
0111   G4KDNode_Base* G4KDTree::Insert(const PointT& pos)
0112   {
0113     G4KDNode_Base* node = nullptr;
0114     if (!fRoot)
0115     {
0116       fRoot = new G4KDNodeCopy<PointT>(this, pos, 0);
0117       node = fRoot;
0118       fNbNodes = 0;
0119       fNbNodes++;
0120       fNbActiveNodes++;
0121     }
0122     else
0123     {
0124       if ((node = fRoot->Insert<PointT>(pos)))
0125       {
0126         fNbNodes++;
0127         fNbActiveNodes++;
0128       }
0129     }
0130
0131     if (fRect == nullptr)
0132     {
0133       fRect = new HyperRect(fDim);
0134       fRect->SetMinMax(pos, pos);
0135     }
0136     else
0137     {
0138       fRect->Extend(pos);
0139     }
0140
0141     return node;
0142   }
0143
0144 //__________________________________________________________________
0145 template<typename Position>
0146   G4int G4KDTree::__NearestInRange(G4KDNode_Base* node,
0147                                    const Position& pos,
0148                                    const double& range_sq,
0149                                    const double& range,
0150                                    G4KDTreeResult& list,
0151                                    G4int ordered,
0152                                    G4KDNode_Base *source_node)
0153   {
0154     if (!node) return 0;
0155
0156     G4double dist_sq(DBL_MAX), dx(DBL_MAX);
0157     G4int ret(-1), added_res(0);
0158
0159     if (node->IsValid() && node != source_node)
0160     {
0161       G4bool do_break = false;
0162       dist_sq = 0;
0163       for (std::size_t i = 0; i < fDim; ++i)
0164       {
0165         dist_sq += sqr((*node)[i] - pos[(G4int)i]);
0166         if (dist_sq > range_sq)
0167         {
0168           do_break = true;
0169           break;
0170         }
0171       }
0172       if (!do_break && dist_sq <= range_sq)
0173       {
0174         list.Insert(dist_sq, node);
0175         added_res = 1;
0176       }
0177     }
0178
0179     dx = pos[node->GetAxis()] - (*node)[node->GetAxis()];
0180
0181     ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos,
0182                            range_sq, range, list, ordered, source_node);
0183     if (ret >= 0 && std::fabs(dx) <= range)
0184     {
0185       added_res += ret;
0186       ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(),
0187                              pos, range_sq, range, list, ordered, source_node);
0188     }
0189
0190     if (ret == -1)
0191     {
0192       return -1;
0193     }
0194     added_res += ret;
0195
0196     return added_res;
0197   }
0198
0199 //__________________________________________________________________
0200 template<typename Position>
0201   void G4KDTree::__NearestToPosition(G4KDNode_Base *node,
0202                                      const Position &pos,
0203                                      G4KDNode_Base *&result,
0204                                      G4double *result_dist_sq,
0205                                      HyperRect* rect)
0206   {
0207     G4int dir = node->GetAxis();
0208     G4double dummy(0.), dist_sq(-1.);
0209     G4KDNode_Base* nearer_subtree(nullptr), *farther_subtree(nullptr);
0210     G4double *nearer_hyperrect_coord(nullptr), *farther_hyperrect_coord(nullptr);
0211
0212     /* Decide whether to go left or right in the tree */
0213     dummy = pos[dir] - (*node)[dir];
0214     if (dummy <= 0)
0215     {
0216       nearer_subtree = node->GetLeft();
0217       farther_subtree = node->GetRight();
0218
0219       nearer_hyperrect_coord = rect->GetMax() + dir;
0220       farther_hyperrect_coord = rect->GetMin() + dir;
0221     }
0222     else
0223     {
0224       nearer_subtree = node->GetRight();
0225       farther_subtree = node->GetLeft();
0226       nearer_hyperrect_coord = rect->GetMin() + dir;
0227       farther_hyperrect_coord = rect->GetMax() + dir;
0228     }
0229
0230     if (nearer_subtree)
0231     {
0232       /* Slice the hyperrect to get the hyperrect of the nearer subtree */
0233       dummy = *nearer_hyperrect_coord;
0234       *nearer_hyperrect_coord = (*node)[dir];
0235       /* Recurse down into nearer subtree */
0236       __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
0237       /* Undo the slice */
0238       *nearer_hyperrect_coord = dummy;
0239     }
0240
0241     /* Check the distance of the point at the current node, compare it
0242      * with our best so far */
0243     if (node->IsValid()) // TODO
0244     {
0245       dist_sq = 0;
0246       G4bool do_break = false;
0247       for (std::size_t i = 0; i < fDim; ++i)
0248       {
0249         dist_sq += sqr((*node)[i] - pos[(G4int)i]);
0250         if (dist_sq > *result_dist_sq)
0251         {
0252           do_break = true;
0253           break;
0254         }
0255       }
0256       if (!do_break && dist_sq < *result_dist_sq)
0257       {
0258         result = node;
0259         *result_dist_sq = dist_sq;
0260       }
0261     }
0262
0263     if (farther_subtree)
0264     {
0265       /* Get the hyperrect of the farther subtree */
0266       dummy = *farther_hyperrect_coord;
0267       *farther_hyperrect_coord = (*node)[dir];
0268       /* Check if we have to recurse down by calculating the closest
0269        * point of the hyperrect and see if it's closer than our
0270        * minimum distance in result_dist_sq. */
0271       if (rect->CompareDistSqr(pos, result_dist_sq))
0272       {
0273         /* Recurse down into farther subtree */
0274         __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
0275       }
0276       /* Undo the slice on the hyperrect */
0277       *farther_hyperrect_coord = dummy;
0278     }
0279   }
0280
0281 template<typename Position>
0282   G4KDTreeResultHandle G4KDTree::Nearest(const Position& pos)
0283   {
0284     //    G4cout << "Nearest(pos)" << G4endl ;
0285
0286     if (!fRect) return nullptr;
0287
0288     G4KDNode_Base *result(nullptr);
0289     G4double dist_sq = DBL_MAX;
0290
0291     /* Duplicate the bounding hyperrectangle, we will work on the copy */
0292     auto  newrect = new HyperRect(*fRect);
0293
0294     /* Our first estimate is the root node */
0295     /* Search for the nearest neighbour recursively */
0296     __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);
0297
0298     /* Free the copy of the hyperrect */
0299     delete newrect;
0300
0301     /* Store the result */
0302     if (result)
0303     {
0304       G4KDTreeResultHandle rset = new G4KDTreeResult(this);
0305       rset->Insert(dist_sq, result);
0306       rset->Rewind();
0307       return rset;
0308     }
0309
0310     return nullptr;
0311   }
0312
0313 //__________________________________________________________________
0314 template<typename Position>
0315   void G4KDTree::__NearestToNode(G4KDNode_Base* source_node,
0316                                  G4KDNode_Base* node,
0317                                  const Position& pos,
0318                                  std::vector<G4KDNode_Base*>& result,
0319                                  G4double *result_dist_sq,
0320                                  HyperRect* rect,
0321                                  G4int& nbresult)
0322   {
0323     G4int dir = node->GetAxis();
0324     G4double dummy, dist_sq;
0325     G4KDNode_Base *nearer_subtree(nullptr), *farther_subtree(nullptr);
0326     G4double *nearer_hyperrect_coord(nullptr), *farther_hyperrect_coord(nullptr);
0327
0328     /* Decide whether to go left or right in the tree */
0329     dummy = pos[dir] - (*node)[dir];
0330     if (dummy <= 0)
0331     {
0332       nearer_subtree = node->GetLeft();
0333       farther_subtree = node->GetRight();
0334       nearer_hyperrect_coord = rect->GetMax() + dir;
0335       farther_hyperrect_coord = rect->GetMin() + dir;
0336     }
0337     else
0338     {
0339       nearer_subtree = node->GetRight();
0340       farther_subtree = node->GetLeft();
0341       nearer_hyperrect_coord = rect->GetMin() + dir;
0342       farther_hyperrect_coord = rect->GetMax() + dir;
0343     }
0344
0345     if (nearer_subtree)
0346     {
0347       /* Slice the hyperrect to get the hyperrect of the nearer subtree */
0348       dummy = *nearer_hyperrect_coord;
0349       *nearer_hyperrect_coord = (*node)[dir];
0350       /* Recurse down into nearer subtree */
0351       __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq,
0352                       rect, nbresult);
0353       /* Undo the slice */
0354       *nearer_hyperrect_coord = dummy;
0355     }
0356
0357     /* Check the distance of the point at the current node, compare it
0358      * with our best so far */
0359     if (node->IsValid() && node != source_node)
0360     {
0361       dist_sq = 0;
0362       G4bool do_break = false;
0363       for (std::size_t i = 0; i < fDim; ++i)
0364       {
0365         dist_sq += sqr((*node)[i] - pos[i]);
0366         if (dist_sq > *result_dist_sq)
0367         {
0368           do_break = true;
0369           break;
0370         }
0371       }
0372       if (!do_break)
0373       {
0374         if (dist_sq < *result_dist_sq)
0375         {
0376           result.clear();
0377           nbresult = 1;
0378           result.push_back(node);
0379           *result_dist_sq = dist_sq;
0380         }
0381         else if (dist_sq == *result_dist_sq)
0382         {
0383           result.push_back(node);
0384           nbresult++;
0385         }
0386       }
0387     }
0388
0389     if (farther_subtree)
0390     {
0391       /* Get the hyperrect of the farther subtree */
0392       dummy = *farther_hyperrect_coord;
0393       *farther_hyperrect_coord = (*node)[dir];
0394       /* Check if we have to recurse down by calculating the closest
0395        * point of the hyperrect and see if it's closer than our
0396        * minimum distance in result_dist_sq. */
0397       //        if (hyperrect_dist_sq(rect, pos) < *result_dist_sq)
0398       if (rect->CompareDistSqr(pos, result_dist_sq))
0399       {
0400         /* Recurse down into farther subtree */
0401         __NearestToNode(source_node, farther_subtree, pos, result,
0402                         result_dist_sq, rect, nbresult);
0403       }
0404       /* Undo the slice on the hyperrect */
0405       *farther_hyperrect_coord = dummy;
0406     }
0407   }
0408
0409 template<typename Position>
0410   G4KDTreeResultHandle G4KDTree::NearestInRange(const Position& pos,
0411                                                 const G4double& range)
0412   {
0413     G4int ret(-1);
0414
0415     const G4double range_sq = sqr(range);
0416
0417     G4KDTreeResultHandle rset = new G4KDTreeResult(this);
0418     if ((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
0419     {
0420       rset = nullptr;
0421       return rset;
0422     }
0423     rset->Sort();
0424     rset->Rewind();
0425     return rset;
0426   }