Geant4  10.01.p03
G4KDTree.icc
Go to the documentation of this file.
1 //
2 // ********************************************************************
3 // * License and Disclaimer *
4 // * *
5 // * The Geant4 software is copyright of the Copyright Holders of *
6 // * the Geant4 Collaboration. It is provided under the terms and *
7 // * conditions of the Geant4 Software License, included in the file *
8 // * LICENSE and available at http://cern.ch/geant4/license . These *
9 // * include a list of copyright holders. *
10 // * *
11 // * Neither the authors of this software system, nor their employing *
12 // * institutes,nor the agencies providing financial support for this *
13 // * work make any representation or warranty, express or implied, *
14 // * regarding this software system or assume any liability for its *
15 // * use. Please see the license in the file LICENSE and URL above *
16 // * for the full disclaimer and the limitation of liability. *
17 // * *
18 // * This code implementation is the result of the scientific and *
19 // * technical work of the GEANT4 collaboration. *
20 // * By using, copying, modifying or distributing the software (or *
21 // * any work based on the software) you agree to acknowledge its *
22 // * use in resulting scientific publications, and indicate your *
23 // * acceptance of all terms of the Geant4 Software license. *
24 // ********************************************************************
25 //
26 // $Id: G4KDTree.icc 87443 2014-12-04 12:26:31Z gunter $
27 //
28 // Author: Mathieu Karamitros (kara (AT) cenbg . in2p3 . fr)
29 //
30 // History:
31 // -----------
32 // 10 Oct 2011 M.Karamitros created
33 //
34 // -------------------------------------------------------------------
35 /*
36  * Based on ``kdtree'', a library for working with kd-trees.
37  * Copyright (C) 2007-2009 John Tsiombikas <nuclear@siggraph.org>
38  * The original open-source version of this code
39  * may be found at http://code.google.com/p/kdtree/
40  *
41  * Redistribution and use in source and binary forms, with or without
42  * modification, are permitted provided that the following conditions are
43  * met:
44  * 1. Redistributions of source code must retain the above copyright
45  * notice, this
46  * list of conditions and the following disclaimer.
47  * 2. Redistributions in binary form must reproduce the above copyright
48  * notice,
49  * this list of conditions and the following disclaimer in the
50  * documentation
51  * and/or other materials provided with the distribution.
52  * 3. The name of the author may not be used to endorse or promote products
53  * derived from this software without specific prior written permission.
54  *
55  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
56  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
57  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
58  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
59  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
60  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
61  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
62  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
63  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
64  */
65 /* single nearest neighbor search written by Tamas Nepusz
66  * <tamas@cs.rhul.ac.uk>
67  */
68 
69 template<typename PointT>
70  G4KDNode_Base* G4KDTree::InsertMap(PointT* point)
71  {
72  G4KDNode<PointT>* node = new G4KDNode<PointT>(this, point, 0);
73  this->__InsertMap(node);
74  return node;
75  }
76 
77 template<typename PointT>
78  G4KDNode_Base* G4KDTree::Insert(PointT* pos)
79  {
80  G4KDNode_Base* node = 0;
81  if (!fRoot)
82  {
83  fRoot = new G4KDNode<PointT>(this, pos, 0);
84  node = fRoot;
85  fNbNodes = 0;
86  fNbNodes++;
87  fNbActiveNodes++;
88  }
89  else
90  {
91  if ((node = fRoot->Insert<PointT>(pos)))
92  {
93  fNbNodes++;
94  fNbActiveNodes++;
95  }
96  }
97 
98  if (fRect == 0)
99  {
100  fRect = new HyperRect(fDim);
101  fRect->SetMinMax(*pos, *pos);
102  }
103  else
104  {
105  fRect->Extend(*pos);
106  }
107 
108  return node;
109  }
110 
111 //__________________________________________________________________
112 template<typename Position>
113  int G4KDTree::__NearestInRange(G4KDNode_Base* node,
114  const Position& pos,
115  const double& range_sq,
116  const double& range,
117  G4KDTreeResult& list,
118  int ordered,
119  G4KDNode_Base *source_node)
120  {
121  if (!node) return 0;
122 
123  double dist_sq(DBL_MAX), dx(DBL_MAX);
124  int ret(-1), added_res(0);
125 
126  if (node->IsValid() && node != source_node)
127  {
128  bool do_break = false;
129  dist_sq = 0;
130  for (size_t i = 0; i < fDim; i++)
131  {
132  dist_sq += sqr((*node)[i] - pos[i]);
133  if (dist_sq > range_sq)
134  {
135  do_break = true;
136  break;
137  }
138  }
139  if (!do_break && dist_sq <= range_sq)
140  {
141  list.Insert(dist_sq, node);
142  added_res = 1;
143  }
144  }
145 
146  dx = pos[node->GetAxis()] - (*node)[node->GetAxis()];
147 
148  ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos,
149  range_sq, range, list, ordered, source_node);
150  if (ret >= 0 && std::fabs(dx) <= range)
151  {
152  added_res += ret;
153  ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(),
154  pos, range_sq, range, list, ordered, source_node);
155  }
156 
157  if (ret == -1)
158  {
159  return -1;
160  }
161  added_res += ret;
162 
163  return added_res;
164  }
165 
166 //__________________________________________________________________
167 template<typename Position>
168  void G4KDTree::__NearestToPosition(G4KDNode_Base *node,
169  const Position &pos,
170  G4KDNode_Base *&result,
171  double *result_dist_sq,
172  HyperRect* rect)
173  {
174  int dir = node->GetAxis();
175  double dummy(0.), dist_sq(-1.);
176  G4KDNode_Base* nearer_subtree(0), *farther_subtree(0);
177  double *nearer_hyperrect_coord(0), *farther_hyperrect_coord(0);
178 
179  /* Decide whether to go left or right in the tree */
180  dummy = pos[dir] - (*node)[dir];
181  if (dummy <= 0)
182  {
183  nearer_subtree = node->GetLeft();
184  farther_subtree = node->GetRight();
185 
186  nearer_hyperrect_coord = rect->GetMax() + dir;
187  farther_hyperrect_coord = rect->GetMin() + dir;
188  }
189  else
190  {
191  nearer_subtree = node->GetRight();
192  farther_subtree = node->GetLeft();
193  nearer_hyperrect_coord = rect->GetMin() + dir;
194  farther_hyperrect_coord = rect->GetMax() + dir;
195  }
196 
197  if (nearer_subtree)
198  {
199  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
200  dummy = *nearer_hyperrect_coord;
201  *nearer_hyperrect_coord = (*node)[dir];
202  /* Recurse down into nearer subtree */
203  __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
204  /* Undo the slice */
205  *nearer_hyperrect_coord = dummy;
206  }
207 
208  /* Check the distance of the point at the current node, compare it
209  * with our best so far */
210  if (node->IsValid()) // TODO
211  {
212  dist_sq = 0;
213  bool do_break = false;
214  for (size_t i = 0; i < fDim; i++)
215  {
216  dist_sq += sqr((*node)[i] - pos[i]);
217  if (dist_sq > *result_dist_sq)
218  {
219  do_break = true;
220  break;
221  }
222  }
223  if (!do_break && dist_sq < *result_dist_sq)
224  {
225  result = node;
226  *result_dist_sq = dist_sq;
227  }
228  }
229 
230  if (farther_subtree)
231  {
232  /* Get the hyperrect of the farther subtree */
233  dummy = *farther_hyperrect_coord;
234  *farther_hyperrect_coord = (*node)[dir];
235  /* Check if we have to recurse down by calculating the closest
236  * point of the hyperrect and see if it's closer than our
237  * minimum distance in result_dist_sq. */
238  if (rect->CompareDistSqr(pos, result_dist_sq))
239  {
240  /* Recurse down into farther subtree */
241  __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
242  }
243  /* Undo the slice on the hyperrect */
244  *farther_hyperrect_coord = dummy;
245  }
246  }
247 
248 template<typename Position>
249  G4KDTreeResultHandle G4KDTree::Nearest(const Position& pos)
250  {
251  // G4cout << "Nearest(pos)" << G4endl ;
252 
253  if (!fRect) return 0;
254 
255  G4KDNode_Base *result(0);
256  double dist_sq = DBL_MAX;
257 
258  /* Duplicate the bounding hyperrectangle, we will work on the copy */
259  HyperRect* newrect = new HyperRect(*fRect);
260 
261  /* Our first guesstimate is the root node */
262  /* Search for the nearest neighbour recursively */
263  __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);
264 
265  /* Free the copy of the hyperrect */
266  delete newrect;
267 
268  /* Store the result */
269  if (result)
270  {
271  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
272  rset->Insert(dist_sq, result);
273  rset->Rewind();
274  return rset;
275  }
276  else
277  {
278  return 0;
279  }
280  }
281 
282 //__________________________________________________________________
283 template<typename Position>
284  void G4KDTree::__NearestToNode(G4KDNode_Base* source_node,
285  G4KDNode_Base* node,
286  const Position& pos,
287  std::vector<G4KDNode_Base*>& result,
288  double *result_dist_sq,
289  HyperRect* rect,
290  int& nbresult)
291  {
292  int dir = node->GetAxis();
293  double dummy, dist_sq;
294  G4KDNode_Base *nearer_subtree(0), *farther_subtree(0);
295  double *nearer_hyperrect_coord(0), *farther_hyperrect_coord(0);
296 
297  /* Decide whether to go left or right in the tree */
298  dummy = pos[dir] - (*node)[dir];
299  if (dummy <= 0)
300  {
301  nearer_subtree = node->GetLeft();
302  farther_subtree = node->GetRight();
303  nearer_hyperrect_coord = rect->GetMax() + dir;
304  farther_hyperrect_coord = rect->GetMin() + dir;
305  }
306  else
307  {
308  nearer_subtree = node->GetRight();
309  farther_subtree = node->GetLeft();
310  nearer_hyperrect_coord = rect->GetMin() + dir;
311  farther_hyperrect_coord = rect->GetMax() + dir;
312  }
313 
314  if (nearer_subtree)
315  {
316  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
317  dummy = *nearer_hyperrect_coord;
318  *nearer_hyperrect_coord = (*node)[dir];
319  /* Recurse down into nearer subtree */
320  __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq,
321  rect, nbresult);
322  /* Undo the slice */
323  *nearer_hyperrect_coord = dummy;
324  }
325 
326  /* Check the distance of the point at the current node, compare it
327  * with our best so far */
328  if (node->IsValid() && node != source_node)
329  {
330  dist_sq = 0;
331  bool do_break = false;
332  for (size_t i = 0; i < fDim; i++)
333  {
334  dist_sq += sqr((*node)[i] - pos[i]);
335  if (dist_sq > *result_dist_sq)
336  {
337  do_break = true;
338  break;
339  }
340  }
341  if (!do_break)
342  {
343  if (dist_sq < *result_dist_sq)
344  {
345  result.clear();
346  nbresult = 1;
347  result.push_back(node);
348  *result_dist_sq = dist_sq;
349  }
350  else if (dist_sq == *result_dist_sq)
351  {
352  result.push_back(node);
353  nbresult++;
354  }
355  }
356  }
357 
358  if (farther_subtree)
359  {
360  /* Get the hyperrect of the farther subtree */
361  dummy = *farther_hyperrect_coord;
362  *farther_hyperrect_coord = (*node)[dir];
363  /* Check if we have to recurse down by calculating the closest
364  * point of the hyperrect and see if it's closer than our
365  * minimum distance in result_dist_sq. */
366  // if (hyperrect_dist_sq(rect, pos) < *result_dist_sq)
367  if (rect->CompareDistSqr(pos, result_dist_sq))
368  {
369  /* Recurse down into farther subtree */
370  __NearestToNode(source_node, farther_subtree, pos, result,
371  result_dist_sq, rect, nbresult);
372  }
373  /* Undo the slice on the hyperrect */
374  *farther_hyperrect_coord = dummy;
375  }
376  }
377 
378 template<typename Position>
379  G4KDTreeResultHandle G4KDTree::NearestInRange(const Position& pos,
380  const double& range)
381  {
382  int ret(-1);
383 
384  const double range_sq = sqr(range);
385 
386  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
387  if ((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
388  {
389  rset = 0;
390  return rset;
391  }
392  rset->Sort();
393  rset->Rewind();
394  return rset;
395  }