Geant4  10.02.p02
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 93883 2015-11-03 08:25:04Z gcosmo $
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 template<typename PointT>
112  G4KDNode_Base* G4KDTree::Insert(const PointT& pos)
113  {
114  G4KDNode_Base* node = 0;
115  if (!fRoot)
116  {
117  fRoot = new G4KDNodeCopy<PointT>(this, pos, 0);
118  node = fRoot;
119  fNbNodes = 0;
120  fNbNodes++;
121  fNbActiveNodes++;
122  }
123  else
124  {
125  if ((node = fRoot->Insert<PointT>(pos)))
126  {
127  fNbNodes++;
128  fNbActiveNodes++;
129  }
130  }
131 
132  if (fRect == 0)
133  {
134  fRect = new HyperRect(fDim);
135  fRect->SetMinMax(pos, pos);
136  }
137  else
138  {
139  fRect->Extend(pos);
140  }
141 
142  return node;
143  }
144 
145 //__________________________________________________________________
146 template<typename Position>
147  int G4KDTree::__NearestInRange(G4KDNode_Base* node,
148  const Position& pos,
149  const double& range_sq,
150  const double& range,
151  G4KDTreeResult& list,
152  int ordered,
153  G4KDNode_Base *source_node)
154  {
155  if (!node) return 0;
156 
157  double dist_sq(DBL_MAX), dx(DBL_MAX);
158  int ret(-1), added_res(0);
159 
160  if (node->IsValid() && node != source_node)
161  {
162  bool do_break = false;
163  dist_sq = 0;
164  for (size_t i = 0; i < fDim; i++)
165  {
166  dist_sq += sqr((*node)[i] - pos[i]);
167  if (dist_sq > range_sq)
168  {
169  do_break = true;
170  break;
171  }
172  }
173  if (!do_break && dist_sq <= range_sq)
174  {
175  list.Insert(dist_sq, node);
176  added_res = 1;
177  }
178  }
179 
180  dx = pos[node->GetAxis()] - (*node)[node->GetAxis()];
181 
182  ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos,
183  range_sq, range, list, ordered, source_node);
184  if (ret >= 0 && std::fabs(dx) <= range)
185  {
186  added_res += ret;
187  ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(),
188  pos, range_sq, range, list, ordered, source_node);
189  }
190 
191  if (ret == -1)
192  {
193  return -1;
194  }
195  added_res += ret;
196 
197  return added_res;
198  }
199 
200 //__________________________________________________________________
201 template<typename Position>
202  void G4KDTree::__NearestToPosition(G4KDNode_Base *node,
203  const Position &pos,
204  G4KDNode_Base *&result,
205  double *result_dist_sq,
206  HyperRect* rect)
207  {
208  int dir = node->GetAxis();
209  double dummy(0.), dist_sq(-1.);
210  G4KDNode_Base* nearer_subtree(0), *farther_subtree(0);
211  double *nearer_hyperrect_coord(0), *farther_hyperrect_coord(0);
212 
213  /* Decide whether to go left or right in the tree */
214  dummy = pos[dir] - (*node)[dir];
215  if (dummy <= 0)
216  {
217  nearer_subtree = node->GetLeft();
218  farther_subtree = node->GetRight();
219 
220  nearer_hyperrect_coord = rect->GetMax() + dir;
221  farther_hyperrect_coord = rect->GetMin() + dir;
222  }
223  else
224  {
225  nearer_subtree = node->GetRight();
226  farther_subtree = node->GetLeft();
227  nearer_hyperrect_coord = rect->GetMin() + dir;
228  farther_hyperrect_coord = rect->GetMax() + dir;
229  }
230 
231  if (nearer_subtree)
232  {
233  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
234  dummy = *nearer_hyperrect_coord;
235  *nearer_hyperrect_coord = (*node)[dir];
236  /* Recurse down into nearer subtree */
237  __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
238  /* Undo the slice */
239  *nearer_hyperrect_coord = dummy;
240  }
241 
242  /* Check the distance of the point at the current node, compare it
243  * with our best so far */
244  if (node->IsValid()) // TODO
245  {
246  dist_sq = 0;
247  bool do_break = false;
248  for (size_t i = 0; i < fDim; i++)
249  {
250  dist_sq += sqr((*node)[i] - pos[i]);
251  if (dist_sq > *result_dist_sq)
252  {
253  do_break = true;
254  break;
255  }
256  }
257  if (!do_break && dist_sq < *result_dist_sq)
258  {
259  result = node;
260  *result_dist_sq = dist_sq;
261  }
262  }
263 
264  if (farther_subtree)
265  {
266  /* Get the hyperrect of the farther subtree */
267  dummy = *farther_hyperrect_coord;
268  *farther_hyperrect_coord = (*node)[dir];
269  /* Check if we have to recurse down by calculating the closest
270  * point of the hyperrect and see if it's closer than our
271  * minimum distance in result_dist_sq. */
272  if (rect->CompareDistSqr(pos, result_dist_sq))
273  {
274  /* Recurse down into farther subtree */
275  __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
276  }
277  /* Undo the slice on the hyperrect */
278  *farther_hyperrect_coord = dummy;
279  }
280  }
281 
282 template<typename Position>
283  G4KDTreeResultHandle G4KDTree::Nearest(const Position& pos)
284  {
285  // G4cout << "Nearest(pos)" << G4endl ;
286 
287  if (!fRect) return 0;
288 
289  G4KDNode_Base *result(0);
290  double dist_sq = DBL_MAX;
291 
292  /* Duplicate the bounding hyperrectangle, we will work on the copy */
293  HyperRect* newrect = new HyperRect(*fRect);
294 
295  /* Our first estimate is the root node */
296  /* Search for the nearest neighbour recursively */
297  __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);
298 
299  /* Free the copy of the hyperrect */
300  delete newrect;
301 
302  /* Store the result */
303  if (result)
304  {
305  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
306  rset->Insert(dist_sq, result);
307  rset->Rewind();
308  return rset;
309  }
310  else
311  {
312  return 0;
313  }
314  }
315 
316 //__________________________________________________________________
317 template<typename Position>
318  void G4KDTree::__NearestToNode(G4KDNode_Base* source_node,
319  G4KDNode_Base* node,
320  const Position& pos,
321  std::vector<G4KDNode_Base*>& result,
322  double *result_dist_sq,
323  HyperRect* rect,
324  int& nbresult)
325  {
326  int dir = node->GetAxis();
327  double dummy, dist_sq;
328  G4KDNode_Base *nearer_subtree(0), *farther_subtree(0);
329  double *nearer_hyperrect_coord(0), *farther_hyperrect_coord(0);
330 
331  /* Decide whether to go left or right in the tree */
332  dummy = pos[dir] - (*node)[dir];
333  if (dummy <= 0)
334  {
335  nearer_subtree = node->GetLeft();
336  farther_subtree = node->GetRight();
337  nearer_hyperrect_coord = rect->GetMax() + dir;
338  farther_hyperrect_coord = rect->GetMin() + dir;
339  }
340  else
341  {
342  nearer_subtree = node->GetRight();
343  farther_subtree = node->GetLeft();
344  nearer_hyperrect_coord = rect->GetMin() + dir;
345  farther_hyperrect_coord = rect->GetMax() + dir;
346  }
347 
348  if (nearer_subtree)
349  {
350  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
351  dummy = *nearer_hyperrect_coord;
352  *nearer_hyperrect_coord = (*node)[dir];
353  /* Recurse down into nearer subtree */
354  __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq,
355  rect, nbresult);
356  /* Undo the slice */
357  *nearer_hyperrect_coord = dummy;
358  }
359 
360  /* Check the distance of the point at the current node, compare it
361  * with our best so far */
362  if (node->IsValid() && node != source_node)
363  {
364  dist_sq = 0;
365  bool do_break = false;
366  for (size_t i = 0; i < fDim; i++)
367  {
368  dist_sq += sqr((*node)[i] - pos[i]);
369  if (dist_sq > *result_dist_sq)
370  {
371  do_break = true;
372  break;
373  }
374  }
375  if (!do_break)
376  {
377  if (dist_sq < *result_dist_sq)
378  {
379  result.clear();
380  nbresult = 1;
381  result.push_back(node);
382  *result_dist_sq = dist_sq;
383  }
384  else if (dist_sq == *result_dist_sq)
385  {
386  result.push_back(node);
387  nbresult++;
388  }
389  }
390  }
391 
392  if (farther_subtree)
393  {
394  /* Get the hyperrect of the farther subtree */
395  dummy = *farther_hyperrect_coord;
396  *farther_hyperrect_coord = (*node)[dir];
397  /* Check if we have to recurse down by calculating the closest
398  * point of the hyperrect and see if it's closer than our
399  * minimum distance in result_dist_sq. */
400  // if (hyperrect_dist_sq(rect, pos) < *result_dist_sq)
401  if (rect->CompareDistSqr(pos, result_dist_sq))
402  {
403  /* Recurse down into farther subtree */
404  __NearestToNode(source_node, farther_subtree, pos, result,
405  result_dist_sq, rect, nbresult);
406  }
407  /* Undo the slice on the hyperrect */
408  *farther_hyperrect_coord = dummy;
409  }
410  }
411 
412 template<typename Position>
413  G4KDTreeResultHandle G4KDTree::NearestInRange(const Position& pos,
414  const double& range)
415  {
416  int ret(-1);
417 
418  const double range_sq = sqr(range);
419 
420  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
421  if ((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
422  {
423  rset = 0;
424  return rset;
425  }
426  rset->Sort();
427  rset->Rewind();
428  return rset;
429  }