00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069 #include "globals.hh"
00070 #include <stdio.h>
00071 #include <stdlib.h>
00072 #include <cmath>
00073 #include "G4KDTree.hh"
00074 #include "G4KDNode.hh"
00075 #include "G4KDTreeResult.hh"
00076 #include <list>
00077 #include <iostream>
00078
00079 using namespace std;
00080
00081
00082 struct HyperRect
00083 {
00084 public:
00085 HyperRect(int dim, const double *min, const double *max)
00086 {
00087 fDim = dim;
00088 fMin = new double[fDim];
00089 fMax = new double[fDim];
00090 size_t size = fDim * sizeof(double);
00091 memcpy(fMin, min, size);
00092 memcpy(fMax, max, size);
00093 }
00094
00095
00096 ~HyperRect()
00097 {
00098 delete[] fMin;
00099 delete[] fMax;
00100 }
00101
00102 HyperRect(const HyperRect& rect)
00103 {
00104 fDim = rect.fDim;
00105 fMin = new double[fDim];
00106 fMax = new double[fDim];
00107 size_t size = fDim * sizeof(double);
00108 memcpy(fMin, rect.fMin, size);
00109 memcpy(fMax, rect.fMax, size);
00110 }
00111
00112 void Extend(const double *pos)
00113 {
00114 int i;
00115
00116 for (i=0; i < fDim; i++)
00117 {
00118 if (pos[i] < fMin[i])
00119 {
00120 fMin[i] = pos[i];
00121 }
00122 if (pos[i] > fMax[i])
00123 {
00124 fMax[i] = pos[i];
00125 }
00126 }
00127 }
00128
00129 bool CompareDistSqr(const double *pos, const double* bestmatch)
00130 {
00131 double result = 0;
00132
00133 for (int i=0; i < fDim; i++)
00134 {
00135 if (pos[i] < fMin[i])
00136 {
00137 result += sqr(fMin[i] - pos[i]);
00138 }
00139 else if (pos[i] > fMax[i])
00140 {
00141 result += sqr(fMax[i] - pos[i]);
00142 }
00143
00144 if(result >= *bestmatch) return false ;
00145 }
00146
00147 return true ;
00148 }
00149
00150 int GetDim(){return fDim;}
00151 double* GetMin(){return fMin;}
00152 double* GetMax(){return fMax;}
00153
00154 protected:
00155 int fDim;
00156 double *fMin, *fMax;
00157
00158 private:
00159
00160 HyperRect& operator=(const HyperRect& rhs)
00161 {
00162 if(this == &rhs) return *this;
00163 return *this;
00164 }
00165 };
00166
00167
00168
00169 G4KDTree::G4KDTree (int k)
00170 {
00171 fDim = k;
00172 fRoot = 0;
00173 fDestr = 0;
00174 fRect = 0;
00175 fNbNodes = 0;
00176 }
00177
00178 G4KDTree::~G4KDTree ()
00179 {
00180 if(fRoot) __Clear_Rec(fRoot);
00181 fRoot = 0;
00182
00183 if (fRect)
00184 {
00185 delete fRect;
00186 fRect = 0;
00187 }
00188 }
00189
00190 void G4KDTree::Clear()
00191 {
00192 __Clear_Rec(fRoot);
00193 fRoot = 0;
00194 fNbNodes = 0;
00195
00196 if (fRect)
00197 {
00198 delete fRect;
00199 fRect = 0;
00200 }
00201 }
00202
00203 void G4KDTree::__Clear_Rec(G4KDNode *node)
00204 {
00205 if(!node) return;
00206
00207 if(node->GetLeft()) __Clear_Rec(node->GetLeft());
00208 if(node->GetRight()) __Clear_Rec(node->GetRight());
00209
00210 if(fDestr)
00211 {
00212 if(node->GetData())
00213 {
00214 fDestr(node->GetData());
00215 node->SetData(0);
00216 }
00217 }
00218 delete node;
00219 }
00220
00221 G4KDNode* G4KDTree::Insert(const double *pos, void *data)
00222 {
00223 G4KDNode* node = 0 ;
00224 if(!fRoot)
00225 {
00226 fRoot = new G4KDNode(this,pos,data,0, 0);
00227 node = fRoot;
00228 fNbNodes = 0;
00229 fNbNodes++;
00230 }
00231 else
00232 {
00233 if((node=fRoot->Insert(pos, data)))
00234 {
00235 fNbNodes++;
00236 }
00237 }
00238
00239 if (fRect == 0)
00240 {
00241 fRect = new HyperRect(fDim,pos,pos);
00242 }
00243 else
00244 {
00245 fRect->Extend(pos);
00246 }
00247
00248 return node;
00249 }
00250
00251 G4KDNode* G4KDTree::Insert(const double& x, const double& y, const double& z, void *data)
00252 {
00253 double buf[3];
00254 buf[0] = x;
00255 buf[1] = y;
00256 buf[2] = z;
00257 return Insert(buf, data);
00258 }
00259
00260
00261 int G4KDTree::__NearestInRange(G4KDNode *node, const double *pos, const double& range_sq,
00262 const double& range, G4KDTreeResult& list, int ordered, G4KDNode *source_node)
00263 {
00264 if(!node) return 0;
00265
00266 double dist_sq(DBL_MAX), dx(DBL_MAX);
00267 int ret(-1), added_res(0);
00268
00269 if(node-> GetData() && node != source_node)
00270 {
00271 bool do_break = false ;
00272 dist_sq = 0;
00273 for(int i=0; i<fDim; i++)
00274 {
00275 dist_sq += sqr(node->GetPosition()[i] - pos[i]);
00276 if(dist_sq > range_sq)
00277 {
00278 do_break = true;
00279 break;
00280 }
00281 }
00282 if(!do_break && dist_sq <= range_sq)
00283 {
00284 list.Insert(dist_sq, node);
00285 added_res = 1;
00286 }
00287 }
00288
00289 dx = pos[node->GetAxis()] - node->GetPosition()[node->GetAxis()];
00290
00291 ret = __NearestInRange(dx <= 0.0 ? node->GetLeft() : node->GetRight(), pos, range_sq, range, list, ordered, source_node);
00292 if(ret >= 0 && fabs(dx) <= range)
00293 {
00294 added_res += ret;
00295 ret = __NearestInRange(dx <= 0.0 ? node->GetRight() : node->GetLeft(), pos, range_sq, range, list, ordered, source_node);
00296 }
00297
00298 if(ret == -1)
00299 {
00300 return -1;
00301 }
00302 added_res += ret;
00303
00304 return added_res;
00305 }
00306
00307
00308 void G4KDTree::__NearestToPosition(G4KDNode *node, const double *pos, G4KDNode *&result,
00309 double *result_dist_sq, HyperRect* rect)
00310 {
00311 int dir = node->GetAxis();
00312 int i;
00313 double dummy(0.), dist_sq(-1.);
00314 G4KDNode *nearer_subtree(0), *farther_subtree (0);
00315 double *nearer_hyperrect_coord(0),*farther_hyperrect_coord(0);
00316
00317
00318 dummy = pos[dir] - node->GetPosition()[dir];
00319 if (dummy <= 0)
00320 {
00321 nearer_subtree = node->GetLeft();
00322 farther_subtree = node->GetRight();
00323
00324 nearer_hyperrect_coord = rect->GetMax() + dir;
00325 farther_hyperrect_coord = rect->GetMin() + dir;
00326 }
00327 else
00328 {
00329 nearer_subtree = node->GetRight();
00330 farther_subtree = node->GetLeft();
00331 nearer_hyperrect_coord = rect->GetMin() + dir;
00332 farther_hyperrect_coord = rect->GetMax() + dir;
00333 }
00334
00335 if (nearer_subtree)
00336 {
00337
00338 dummy = *nearer_hyperrect_coord;
00339 *nearer_hyperrect_coord = node->GetPosition()[dir];
00340
00341 __NearestToPosition(nearer_subtree, pos, result, result_dist_sq, rect);
00342
00343 *nearer_hyperrect_coord = dummy;
00344 }
00345
00346
00347
00348 if(node->GetData())
00349 {
00350 dist_sq = 0;
00351 bool do_break = false ;
00352 for(i=0; i < fDim; i++)
00353 {
00354 dist_sq += sqr(node->GetPosition()[i] - pos[i]);
00355 if(dist_sq > *result_dist_sq)
00356 {
00357 do_break = true;
00358 break ;
00359 }
00360 }
00361 if (!do_break && dist_sq < *result_dist_sq)
00362 {
00363 result = node;
00364 *result_dist_sq = dist_sq;
00365 }
00366 }
00367
00368 if (farther_subtree)
00369 {
00370
00371 dummy = *farther_hyperrect_coord;
00372 *farther_hyperrect_coord = node->GetPosition()[dir];
00373
00374
00375
00376 if (rect->CompareDistSqr(pos,result_dist_sq))
00377 {
00378
00379 __NearestToPosition(farther_subtree, pos, result, result_dist_sq, rect);
00380 }
00381
00382 *farther_hyperrect_coord = dummy;
00383 }
00384 }
00385
00386 G4KDTreeResultHandle G4KDTree::Nearest(const double *pos)
00387 {
00388
00389
00390 if (!fRect) return 0;
00391
00392 G4KDNode *result(0);
00393 double dist_sq = DBL_MAX;
00394
00395
00396 HyperRect *newrect = new HyperRect(*fRect);
00397
00398
00399
00400 __NearestToPosition(fRoot, pos, result, &dist_sq, newrect);
00401
00402
00403 delete newrect;
00404
00405
00406 if (result)
00407 {
00408 G4KDTreeResultHandle rset = new G4KDTreeResult(this);
00409 rset->Insert(dist_sq, result);
00410 rset -> Rewind();
00411 return rset;
00412 }
00413 else
00414 {
00415 return 0;
00416 }
00417 }
00418
00419
00420 void G4KDTree::__NearestToNode(G4KDNode *source_node, G4KDNode *node,
00421 const double *pos, std::vector<G4KDNode*>& result, double *result_dist_sq,
00422 HyperRect* rect, int& nbresult)
00423 {
00424 int dir = node->GetAxis();
00425 double dummy, dist_sq;
00426 G4KDNode *nearer_subtree (0), *farther_subtree (0);
00427 double *nearer_hyperrect_coord (0), *farther_hyperrect_coord (0);
00428
00429
00430 dummy = pos[dir] - node->GetPosition()[dir];
00431 if (dummy <= 0)
00432 {
00433 nearer_subtree = node->GetLeft();
00434 farther_subtree = node->GetRight();
00435 nearer_hyperrect_coord = rect->GetMax() + dir;
00436 farther_hyperrect_coord = rect->GetMin() + dir;
00437 }
00438 else
00439 {
00440 nearer_subtree = node->GetRight();
00441 farther_subtree = node->GetLeft();
00442 nearer_hyperrect_coord = rect->GetMin() + dir;
00443 farther_hyperrect_coord = rect->GetMax() + dir;
00444 }
00445
00446 if (nearer_subtree)
00447 {
00448
00449 dummy = *nearer_hyperrect_coord;
00450 *nearer_hyperrect_coord = node->GetPosition()[dir];
00451
00452 __NearestToNode(source_node, nearer_subtree, pos, result, result_dist_sq, rect, nbresult);
00453
00454 *nearer_hyperrect_coord = dummy;
00455 }
00456
00457
00458
00459 if(node->GetData() && node != source_node)
00460 {
00461 dist_sq = 0;
00462 bool do_break = false;
00463 for(int i=0; i < fDim; i++)
00464 {
00465 dist_sq += sqr(node->GetPosition()[i] - pos[i]);
00466 if(dist_sq > *result_dist_sq)
00467 {
00468 do_break = true;
00469 break ;
00470 }
00471 }
00472 if(!do_break)
00473 {
00474 if (dist_sq < *result_dist_sq)
00475 {
00476 result.clear();
00477 nbresult = 1 ;
00478 result.push_back(node);
00479 *result_dist_sq = dist_sq;
00480 }
00481 else if(dist_sq == *result_dist_sq)
00482 {
00483 result.push_back(node);
00484 nbresult++;
00485 }
00486 }
00487 }
00488
00489 if (farther_subtree)
00490 {
00491
00492 dummy = *farther_hyperrect_coord;
00493 *farther_hyperrect_coord = node->GetPosition()[dir];
00494
00495
00496
00497
00498 if (rect->CompareDistSqr(pos,result_dist_sq))
00499 {
00500
00501 __NearestToNode(source_node, farther_subtree, pos, result, result_dist_sq, rect, nbresult);
00502 }
00503
00504 *farther_hyperrect_coord = dummy;
00505 }
00506 }
00507
00508 G4KDTreeResultHandle G4KDTree::Nearest(G4KDNode* node)
00509 {
00510
00511 if (!fRect)
00512 {
00513 G4cout << "Tree empty" << G4endl ;
00514 return 0;
00515 }
00516
00517 const double* pos = node->GetPosition();
00518 std::vector<G4KDNode*> result;
00519 double dist_sq = DBL_MAX;
00520
00521
00522 HyperRect *newrect = new HyperRect(*fRect);
00523
00524
00525 int nbresult = 0 ;
00526
00527 __NearestToNode(node, fRoot, pos, result, &dist_sq, newrect, nbresult);
00528
00529
00530 delete newrect;
00531
00532
00533 if (!result.empty())
00534 {
00535 G4KDTreeResultHandle rset(new G4KDTreeResult(this));
00536 int j = 0 ;
00537 while (j<nbresult)
00538 {
00539 rset->Insert(dist_sq, result[j]);
00540 j++;
00541 }
00542 rset->Rewind();
00543
00544 return rset;
00545 }
00546 else
00547 {
00548 return 0;
00549 }
00550 }
00551
00552 G4KDTreeResultHandle G4KDTree::Nearest(const double& x, const double& y, const double& z)
00553 {
00554 double pos[3];
00555 pos[0] = x;
00556 pos[1] = y;
00557 pos[2] = z;
00558 return Nearest(pos);
00559 }
00560
00561 G4KDTreeResultHandle G4KDTree::NearestInRange(const double *pos, const double& range)
00562 {
00563 int ret(-1);
00564
00565 const double range_sq = sqr(range) ;
00566
00567 G4KDTreeResultHandle rset = new G4KDTreeResult(this);
00568 if((ret = __NearestInRange(fRoot, pos, range_sq, range, *(rset()), 0)) == -1)
00569 {
00570 rset = 0;
00571 return rset;
00572 }
00573 rset->Sort();
00574 rset->Rewind();
00575 return rset;
00576 }
00577
00578 G4KDTreeResultHandle G4KDTree::NearestInRange(const double& x,
00579 const double& y,
00580 const double& z,
00581 const double& range)
00582 {
00583 double buf[3];
00584 buf[0] = x;
00585 buf[1] = y;
00586 buf[2] = z;
00587 return NearestInRange(buf, range);
00588 }
00589
00590 G4KDTreeResultHandle G4KDTree::NearestInRange( G4KDNode* node, const double& range)
00591 {
00592 if(!node) return 0 ;
00593 int ret(-1);
00594
00595 G4KDTreeResult *rset = new G4KDTreeResult(this);
00596
00597 const double range_sq = sqr(range) ;
00598
00599 if((ret = __NearestInRange(fRoot, node->GetPosition(), range_sq, range, *rset, 0, node)) == -1)
00600 {
00601 delete rset;
00602 return 0;
00603 }
00604 rset->Sort();
00605 rset->Rewind();
00606 return rset;
00607 }