Geant4  10.00.p01
G4KDTree.cc
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.cc 70171 2013-05-24 13:34:18Z 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 #include "globals.hh"
70 #include <stdio.h>
71 #include <stdlib.h>
72 #include <cmath>
73 #include "G4KDTree.hh"
74 #include "G4KDMap.hh"
75 #include "G4KDNode.hh"
76 #include "G4KDTreeResult.hh"
77 #include <list>
78 #include <iostream>
79 
80 using namespace std;
81 
82 //______________________________________________________________________
83 struct HyperRect
84 {
85 public:
86  HyperRect(int dim, const double *min, const double *max)
87  {
88  fDim = dim;
89  fMin = new double[fDim];
90  fMax = new double[fDim];
91  size_t size = fDim * sizeof(double);
92  memcpy(fMin, min, size);
93  memcpy(fMax, max, size);
94  }
95 
96 
98  {
99  delete[] fMin;
100  delete[] fMax;
101  }
102 
103  HyperRect(const HyperRect& rect)
104  {
105  fDim = rect.fDim;
106  fMin = new double[fDim];
107  fMax = new double[fDim];
108  size_t size = fDim * sizeof(double);
109  memcpy(fMin, rect.fMin, size);
110  memcpy(fMax, rect.fMax, size);
111  }
112 
113  void Extend(const double *pos)
114  {
115  int i;
116 
117  for (i=0; i < fDim; i++)
118  {
119  if (pos[i] < fMin[i])
120  {
121  fMin[i] = pos[i];
122  }
123  if (pos[i] > fMax[i])
124  {
125  fMax[i] = pos[i];
126  }
127  }
128  }
129 
130  bool CompareDistSqr(const double *pos, const double* bestmatch)
131  {
132  double result = 0;
133 
134  for (int i=0; i < fDim; i++)
135  {
136  if (pos[i] < fMin[i])
137  {
138  result += sqr(fMin[i] - pos[i]);
139  }
140  else if (pos[i] > fMax[i])
141  {
142  result += sqr(fMax[i] - pos[i]);
143  }
144 
145  if(result >= *bestmatch) return false ;
146  }
147 
148  return true ;
149  }
150 
151  int GetDim(){return fDim;}
152  double* GetMin(){return fMin;}
153  double* GetMax(){return fMax;}
154 
155 protected:
156  int fDim;
157  double *fMin, *fMax; /* minimum/maximum coords */
158 
159 private:
160  // should not be used
162  {
163  if(this == &rhs) return *this;
164  return *this;
165  }
166 };
167 
168 //______________________________________________________________________
169 // KDTree methods
170 G4KDTree::G4KDTree (int k) : fKDMap(new G4KDMap(k))
171 {
172  fDim = k;
173  fRoot = 0;
174  fDestr = 0;
175  fRect = 0;
176  fNbNodes = 0;
177 }
178 
180 {
181  if(fRoot) __Clear_Rec(fRoot);
182  fRoot = 0;
183 
184  if (fRect)
185  {
186  delete fRect;
187  fRect = 0;
188  }
189 
190  if(fKDMap) delete fKDMap;
191 }
192 
194 {
196  fRoot = 0;
197  fNbNodes = 0;
198 
199  if (fRect)
200  {
201  delete fRect;
202  fRect = 0;
203  }
204 }
205 
207 {
208  if(!node) return;
209 
210  if(node->GetLeft()) __Clear_Rec(node->GetLeft());
211  if(node->GetRight()) __Clear_Rec(node->GetRight());
212 
213  if(fDestr)
214  {
215  if(node->GetData())
216  {
217  fDestr(node->GetData());
218  node->SetData(0);
219  }
220  }
221  delete node;
222 }
223 
224 G4KDNode* G4KDTree::InsertMap(const double& x, const double& y, const double& z, void *data)
225 {
226  double buf[3];
227  buf[0] = x;
228  buf[1] = y;
229  buf[2] = z;
230  return InsertMap(buf, data);
231 }
232 
233 G4KDNode* G4KDTree::InsertMap(const double *pos, void *data)
234 {
235  G4KDNode* node = new G4KDNode(this,pos,data,0,-2) ;
236  // -2 = valeur intentionnellement absurde
237  fKDMap->Insert(node);
238  return node;
239 }
240 
242 {
243  int Nnodes = fKDMap->GetSize();
244 
245  G4KDNode* root = fKDMap->PopOutMiddle(0);
246 
247  fRoot = root;
249 
250  Nnodes--;
251 
252  for(int n = 0 ; n < Nnodes ; n = n+fDim)
253  {
254  for(int dim = 0 ; dim < fDim ; dim ++)
255  {
256  G4KDNode* node = fKDMap->PopOutMiddle(dim);
257  fRoot->Insert(node);
258  fRect->Extend(node->GetPosition());
259  }
260  }
261 }
262 
263 G4KDNode* G4KDTree::Insert(const double *pos, void *data)
264 {
265  G4KDNode* node = 0 ;
266  if(!fRoot)
267  {
268  fRoot = new G4KDNode(this,pos,data,0, 0);
269  node = fRoot;
270  fNbNodes = 0;
271  fNbNodes++;
272  }
273  else
274  {
275  if((node=fRoot->Insert(pos, data)))
276  {
277  fNbNodes++;
278  }
279  }
280 
281  if (fRect == 0)
282  {
283  fRect = new HyperRect(fDim,pos,pos);
284  }
285  else
286  {
287  fRect->Extend(pos);
288  }
289 
290  return node;
291 }
292 
293 G4KDNode* G4KDTree::Insert(const double& x, const double& y, const double& z, void *data)
294 {
295  double buf[3];
296  buf[0] = x;
297  buf[1] = y;
298  buf[2] = z;
299  return Insert(buf, data);
300 }
301 
302 //__________________________________________________________________
303 int G4KDTree::__NearestInRange(G4KDNode *node, const double *pos, const double& range_sq,
304  const double& range, G4KDTreeResult& list, int ordered, G4KDNode *source_node)
305 {
306  if(!node) return 0;
307 
308  double dist_sq(DBL_MAX), dx(DBL_MAX);
309  int ret(-1), added_res(0);
310 
311  if(node-> GetData() && node != source_node)
312  {
313  bool do_break = false ;
314  dist_sq = 0;
315  for(int i=0; i<fDim; i++)
316  {
317  dist_sq += sqr(node->GetPosition()[i] - pos[i]);
318  if(dist_sq > range_sq)
319  {
320  do_break = true;
321  break;
322  }
323  }
324  if(!do_break && dist_sq <= range_sq)
325  {
326  list.Insert(dist_sq, node);
327  added_res = 1;
328  }
329  }
330 
331  dx = pos[node->GetAxis()] - node->GetPosition()[node->GetAxis()];
332 
333  ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos, range_sq, range, list, ordered, source_node);
334  if(ret >= 0 && fabs(dx) <= range)
335  {
336  added_res += ret;
337  ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(), pos, range_sq, range, list, ordered, source_node);
338  }
339 
340  if(ret == -1)
341  {
342  return -1;
343  }
344  added_res += ret;
345 
346  return added_res;
347 }
348 
349 //__________________________________________________________________
350 void G4KDTree::__NearestToPosition(G4KDNode *node, const double *pos, G4KDNode *&result,
351  double *result_dist_sq, HyperRect* rect)
352 {
353  int dir = node->GetAxis();
354  int i;
355  double dummy(0.), dist_sq(-1.);
356  G4KDNode *nearer_subtree(0), *farther_subtree (0);
357  double *nearer_hyperrect_coord(0),*farther_hyperrect_coord(0);
358 
359  /* Decide whether to go left or right in the tree */
360  dummy = pos[dir] - node->GetPosition()[dir];
361  if (dummy <= 0)
362  {
363  nearer_subtree = node->GetLeft();
364  farther_subtree = node->GetRight();
365 
366  nearer_hyperrect_coord = rect->GetMax() + dir;
367  farther_hyperrect_coord = rect->GetMin() + dir;
368  }
369  else
370  {
371  nearer_subtree = node->GetRight();
372  farther_subtree = node->GetLeft();
373  nearer_hyperrect_coord = rect->GetMin() + dir;
374  farther_hyperrect_coord = rect->GetMax() + dir;
375  }
376 
377  if (nearer_subtree)
378  {
379  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
380  dummy = *nearer_hyperrect_coord;
381  *nearer_hyperrect_coord = node->GetPosition()[dir];
382  /* Recurse down into nearer subtree */
383  __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
384  /* Undo the slice */
385  *nearer_hyperrect_coord = dummy;
386  }
387 
388  /* Check the distance of the point at the current node, compare it
389  * with our best so far */
390  if(node->GetData())
391  {
392  dist_sq = 0;
393  bool do_break = false ;
394  for(i=0; i < fDim; i++)
395  {
396  dist_sq += sqr(node->GetPosition()[i] - pos[i]);
397  if(dist_sq > *result_dist_sq)
398  {
399  do_break = true;
400  break ;
401  }
402  }
403  if (!do_break && dist_sq < *result_dist_sq)
404  {
405  result = node;
406  *result_dist_sq = dist_sq;
407  }
408  }
409 
410  if (farther_subtree)
411  {
412  /* Get the hyperrect of the farther subtree */
413  dummy = *farther_hyperrect_coord;
414  *farther_hyperrect_coord = node->GetPosition()[dir];
415  /* Check if we have to recurse down by calculating the closest
416  * point of the hyperrect and see if it's closer than our
417  * minimum distance in result_dist_sq. */
418  if (rect->CompareDistSqr(pos,result_dist_sq))
419  {
420  /* Recurse down into farther subtree */
421  __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
422  }
423  /* Undo the slice on the hyperrect */
424  *farther_hyperrect_coord = dummy;
425  }
426 }
427 
429 {
430  // G4cout << "Nearest(pos)" << G4endl ;
431 
432  if (!fRect) return 0;
433 
434  G4KDNode *result(0);
435  double dist_sq = DBL_MAX;
436 
437  /* Duplicate the bounding hyperrectangle, we will work on the copy */
438  HyperRect *newrect = new HyperRect(*fRect);
439 
440  /* Our first guesstimate is the root node */
441  /* Search for the nearest neighbour recursively */
442  __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);
443 
444  /* Free the copy of the hyperrect */
445  delete newrect;
446 
447  /* Store the result */
448  if (result)
449  {
450  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
451  rset->Insert(dist_sq, result);
452  rset -> Rewind();
453  return rset;
454  }
455  else
456  {
457  return 0;
458  }
459 }
460 
461 //__________________________________________________________________
462 void G4KDTree::__NearestToNode(G4KDNode *source_node, G4KDNode *node,
463  const double *pos, std::vector<G4KDNode*>& result, double *result_dist_sq,
464  HyperRect* rect, int& nbresult)
465 {
466  int dir = node->GetAxis();
467  double dummy, dist_sq;
468  G4KDNode *nearer_subtree (0), *farther_subtree (0);
469  double *nearer_hyperrect_coord (0), *farther_hyperrect_coord (0);
470 
471  /* Decide whether to go left or right in the tree */
472  dummy = pos[dir] - node->GetPosition()[dir];
473  if (dummy <= 0)
474  {
475  nearer_subtree = node->GetLeft();
476  farther_subtree = node->GetRight();
477  nearer_hyperrect_coord = rect->GetMax() + dir;
478  farther_hyperrect_coord = rect->GetMin() + dir;
479  }
480  else
481  {
482  nearer_subtree = node->GetRight();
483  farther_subtree = node->GetLeft();
484  nearer_hyperrect_coord = rect->GetMin() + dir;
485  farther_hyperrect_coord = rect->GetMax() + dir;
486  }
487 
488  if (nearer_subtree)
489  {
490  /* Slice the hyperrect to get the hyperrect of the nearer subtree */
491  dummy = *nearer_hyperrect_coord;
492  *nearer_hyperrect_coord = node->GetPosition()[dir];
493  /* Recurse down into nearer subtree */
494  __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq, rect, nbresult);
495  /* Undo the slice */
496  *nearer_hyperrect_coord = dummy;
497  }
498 
499  /* Check the distance of the point at the current node, compare it
500  * with our best so far */
501  if(node->GetData() && node != source_node)
502  {
503  dist_sq = 0;
504  bool do_break = false;
505  for(int i=0; i < fDim; i++)
506  {
507  dist_sq += sqr(node->GetPosition()[i] - pos[i]);
508  if(dist_sq > *result_dist_sq)
509  {
510  do_break = true;
511  break ;
512  }
513  }
514  if(!do_break)
515  {
516  if (dist_sq < *result_dist_sq)
517  {
518  result.clear();
519  nbresult = 1 ;
520  result.push_back(node);
521  *result_dist_sq = dist_sq;
522  }
523  else if(dist_sq == *result_dist_sq)
524  {
525  result.push_back(node);
526  nbresult++;
527  }
528  }
529  }
530 
531  if (farther_subtree)
532  {
533  /* Get the hyperrect of the farther subtree */
534  dummy = *farther_hyperrect_coord;
535  *farther_hyperrect_coord = node->GetPosition()[dir];
536  /* Check if we have to recurse down by calculating the closest
537  * point of the hyperrect and see if it's closer than our
538  * minimum distance in result_dist_sq. */
539  // if (hyperrect_dist_sq(rect, pos) < *result_dist_sq)
540  if (rect->CompareDistSqr(pos,result_dist_sq))
541  {
542  /* Recurse down into farther subtree */
543  __NearestToNode(source_node, farther_subtree, pos, result, result_dist_sq, rect, nbresult);
544  }
545  /* Undo the slice on the hyperrect */
546  *farther_hyperrect_coord = dummy;
547  }
548 }
549 
551 {
552  // G4cout << "Nearest(node)" << G4endl ;
553  if (!fRect)
554  {
555  G4cout << "Tree empty" << G4endl ;
556  return 0;
557  }
558 
559  const double* pos = node->GetPosition();
560  std::vector<G4KDNode*> result;
561  double dist_sq = DBL_MAX;
562 
563  /* Duplicate the bounding hyperrectangle, we will work on the copy */
564  HyperRect *newrect = new HyperRect(*fRect);
565 
566  /* Search for the nearest neighbour recursively */
567  int nbresult = 0 ;
568 
569  __NearestToNode(node, fRoot, pos, result, &dist_sq, newrect, nbresult);
570 
571  /* Free the copy of the hyperrect */
572  delete newrect;
573 
574  /* Store the result */
575  if (!result.empty())
576  {
577  G4KDTreeResultHandle rset(new G4KDTreeResult(this));
578  int j = 0 ;
579  while (j<nbresult)
580  {
581  rset->Insert(dist_sq, result[j]);
582  j++;
583  }
584  rset->Rewind();
585 
586  return rset;
587  }
588  else
589  {
590  return 0;
591  }
592 }
593 
594 G4KDTreeResultHandle G4KDTree::Nearest(const double& x, const double& y, const double& z)
595 {
596  double pos[3];
597  pos[0] = x;
598  pos[1] = y;
599  pos[2] = z;
600  return Nearest(pos);
601 }
602 
603 G4KDTreeResultHandle G4KDTree::NearestInRange(const double *pos, const double& range)
604 {
605  int ret(-1);
606 
607  const double range_sq = sqr(range) ;
608 
609  G4KDTreeResultHandle rset = new G4KDTreeResult(this);
610  if((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
611  {
612  rset = 0;
613  return rset;
614  }
615  rset->Sort();
616  rset->Rewind();
617  return rset;
618 }
619 
621  const double& y,
622  const double& z,
623  const double& range)
624 {
625  double buf[3];
626  buf[0] = x;
627  buf[1] = y;
628  buf[2] = z;
629  return NearestInRange(buf, range);
630 }
631 
633 {
634  if(!node) return 0 ;
635  int ret(-1);
636 
637  G4KDTreeResult *rset = new G4KDTreeResult(this);
638 
639  const double range_sq = sqr(range) ;
640 
641  if((ret = __NearestInRange(fRoot, node->GetPosition(), range_sq, range, *rset, 0, node)) == -1)
642  {
643  delete rset;
644  return 0;
645  }
646  rset->Sort();
647  rset->Rewind();
648  return rset;
649 }
void(* fDestr)(void *)
Definition: G4KDTree.hh:67
void Clear()
Definition: G4KDTree.cc:193
int GetAxis()
Definition: G4KDNode.hh:112
void __Clear_Rec(G4KDNode *node)
Definition: G4KDTree.cc:206
G4KDNode * PopOutMiddle(int dimension)
Definition: G4KDMap.cc:92
~HyperRect()
Definition: G4KDTree.cc:97
HyperRect(const HyperRect &rect)
Definition: G4KDTree.cc:103
G4double z
Definition: TRTMaterials.hh:39
double * GetMin()
Definition: G4KDTree.cc:152
G4KDTree(int dim=3)
Definition: G4KDTree.cc:170
size_t GetSize()
Definition: G4KDMap.hh:94
HyperRect(int dim, const double *min, const double *max)
Definition: G4KDTree.cc:86
int fNbNodes
Definition: G4KDTree.hh:68
void Extend(const double *pos)
Definition: G4KDTree.cc:113
struct HyperRect * fRect
Definition: G4KDTree.hh:66
int fDim
Definition: G4KDTree.cc:156
G4KDNode * Insert(const double *p, void *data)
Definition: G4KDNode.cc:156
int __NearestInRange(G4KDNode *node, const double *pos, const double &range_sq, const double &range, G4KDTreeResult &list, int ordered, G4KDNode *source_node=0)
Definition: G4KDTree.cc:303
G4GLOB_DLL std::ostream G4cout
G4KDNode * GetLeft()
Definition: G4KDNode.hh:137
G4KDMap * fKDMap
Definition: G4KDTree.hh:69
G4KDTreeResultHandle Nearest(const double *pos)
Definition: G4KDTree.cc:428
void Insert(double, G4KDNode *)
double * fMin
Definition: G4KDTree.cc:157
G4KDNode stores one entity in G4KDTree This class is for internal use only.
Definition: G4KDNode.hh:50
const G4int n
void * GetData()
Definition: G4KDNode.hh:117
void Build()
Definition: G4KDTree.cc:241
friend class G4KDNode
Definition: G4KDTree.hh:64
void __NearestToNode(G4KDNode *source_node, G4KDNode *node, const double *pos, std::vector< G4KDNode * > &result, double *result_dist_sq, struct HyperRect *fRect, int &nbresult)
Definition: G4KDTree.cc:462
G4KDNode * GetRight()
Definition: G4KDNode.hh:142
G4KDNode * InsertMap(const double &x, const double &y, const double &z, void *data)
Definition: G4KDTree.cc:224
int fDim
Definition: G4KDTree.hh:65
void SetData(void *)
Definition: G4KDNode.hh:122
T max(const T t1, const T t2)
brief Return the largest of the two arguments
const double * GetPosition()
Definition: G4KDNode.hh:127
T min(const T t1, const T t2)
brief Return the smallest of the two arguments
void __NearestToPosition(G4KDNode *node, const double *pos, G4KDNode *&result, double *result_dist_sq, struct HyperRect *fRect)
Definition: G4KDTree.cc:350
HyperRect & operator=(const HyperRect &rhs)
Definition: G4KDTree.cc:161
G4KDTreeResultHandle NearestInRange(const double *pos, const double &range)
Definition: G4KDTree.cc:603
int GetDim()
Definition: G4KDTree.cc:151
#define G4endl
Definition: G4ios.hh:61
virtual ~G4KDTree()
Definition: G4KDTree.cc:179
double * fMax
Definition: G4KDTree.cc:157
T sqr(const T &x)
Definition: templates.hh:145
G4KDNode * fRoot
Definition: G4KDTree.hh:72
double * GetMax()
Definition: G4KDTree.cc:153
void * GetData(G4KDNode *)
Definition: G4KDNode.cc:45
#define DBL_MAX
Definition: templates.hh:83
G4KDNode * Insert(const double *pos, void *data)
Definition: G4KDTree.cc:263
static const G4double pos
void Insert(G4KDNode *pos)
Definition: G4KDMap.cc:73
bool CompareDistSqr(const double *pos, const double *bestmatch)
Definition: G4KDTree.cc:130
G4KDTreeResult enables to go through the nearest entities found by G4KDTree.