src/Navaids/PositionedOctree.cxx

   1 /**
   2  * PositionedOctree - define a spatial octree containing Positioned items
   3  * arranged by their global cartesian position.
   4  */
   5
   6 // Written by James Turner, started 2012.
   7 //
   8 // Copyright (C) 2012 James Turner
   9 //
  10 // This program is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU General Public License as
  12 // published by the Free Software Foundation; either version 2 of the
  13 // License, or (at your option) any later version.
  14 //
  15 // This program is distributed in the hope that it will be useful, but
  16 // WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU General Public License
  21 // along with this program; if not, write to the Free Software
  22 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  23
  24 #ifdef HAVE_CONFIG_H
  25 # include "config.h"
  26 #endif
  27
  28 #include "PositionedOctree.hxx"
  29 #include "positioned.hxx"
  30
  31 #include <cassert>
  32 #include <algorithm> // for sort
  33 #include <cstring> // for memset
  34 #include <iostream>
  35
  36 #include <boost/foreach.hpp>
  37
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/structure/exception.hxx>
  40
  41 namespace flightgear
  42 {
  43
  44 namespace Octree
  45 {
  46
  47 Node* global_spatialOctree = NULL;
  48
  49 Leaf::Leaf(const SGBoxd& aBox, int64_t aIdent) :
  50   Node(aBox, aIdent),
  51   childrenLoaded(false)
  52 {
  53 }
  54
  55 void Leaf::visit(const SGVec3d& aPos, double aCutoff,
  56                    FGPositioned::Filter* aFilter,
  57                    FindNearestResults& aResults, FindNearestPQueue&)
  58 {
  59   int previousResultsSize = aResults.size();
  60   int addedCount = 0;
  61   NavDataCache* cache = NavDataCache::instance();
  62
  63   loadChildren();
  64
  65   ChildMap::const_iterator it = children.lower_bound(aFilter->minType());
  66   ChildMap::const_iterator end = children.upper_bound(aFilter->maxType());
  67
  68   for (; it != end; ++it) {
  69     FGPositioned* p = cache->loadById(it->second);
  70     double d = dist(aPos, p->cart());
  71     if (d > aCutoff) {
  72       continue;
  73     }
  74
  75     if (aFilter && !aFilter->pass(p)) {
  76       continue;
  77     }
  78
  79     ++addedCount;
  80     aResults.push_back(OrderedPositioned(p, d));
  81   }
  82
  83   if (addedCount == 0) {
  84     return;
  85   }
  86
  87   // keep aResults sorted
  88   // sort the new items, usually just one or two items
  89   std::sort(aResults.begin() + previousResultsSize, aResults.end());
  90
  91   // merge the two sorted ranges together - in linear time
  92   std::inplace_merge(aResults.begin(),
  93                      aResults.begin() + previousResultsSize, aResults.end());
  94 }
  95
  96 void Leaf::insertChild(FGPositioned::Type ty, PositionedID id)
  97 {
  98   assert(childrenLoaded);
  99   children.insert(children.end(), TypedPositioned(ty, id));
 100 }
 101
 102 void Leaf::loadChildren()
 103 {
 104   if (childrenLoaded) {
 105     return;
 106   }
 107
 108   NavDataCache* cache = NavDataCache::instance();
 109   BOOST_FOREACH(TypedPositioned tp, cache->getOctreeLeafChildren(guid())) {
 110     children.insert(children.end(), tp);
 111   } // of leaf members iteration
 112
 113   childrenLoaded = true;
 114 }
 115
 116 Branch::Branch(const SGBoxd& aBox, int64_t aIdent) :
 117   Node(aBox, aIdent),
 118   childrenLoaded(false)
 119 {
 120   memset(children, 0, sizeof(Node*) * 8);
 121 }
 122
 123 void Branch::visit(const SGVec3d& aPos, double aCutoff,
 124                    FGPositioned::Filter*,
 125                    FindNearestResults&, FindNearestPQueue& aQ)
 126 {
 127   loadChildren();
 128   for (unsigned int i=0; i<8; ++i) {
 129     if (!children[i]) {
 130       continue;
 131     }
 132
 133     double d = children[i]->distToNearest(aPos);
 134     if (d > aCutoff) {
 135       continue; // exceeded cutoff
 136     }
 137
 138     aQ.push(Ordered<Node*>(children[i], d));
 139   } // of child iteration
 140 }
 141
 142 Node* Branch::childForPos(const SGVec3d& aCart) const
 143 {
 144   assert(contains(aCart));
 145   int childIndex = 0;
 146
 147   SGVec3d center(_box.getCenter());
 148 // tests must match indices in SGbox::getCorner
 149   if (aCart.x() < center.x()) {
 150     childIndex += 1;
 151   }
 152
 153   if (aCart.y() < center.y()) {
 154     childIndex += 2;
 155   }
 156
 157   if (aCart.z() < center.z()) {
 158     childIndex += 4;
 159   }
 160
 161   return childAtIndex(childIndex);
 162 }
 163
 164 Node* Branch::childAtIndex(int childIndex) const
 165 {
 166   Node* child = children[childIndex];
 167   if (!child) { // lazy building of children
 168     SGBoxd cb(boxForChild(childIndex));
 169     double d2 = dot(cb.getSize(), cb.getSize());
 170
 171     assert(((_ident << 3) >> 3) == _ident);
 172
 173     // child index is 0..7, so 3-bits is sufficient, and hence we can
 174     // pack 20 levels of octree into a int64, which is plenty
 175     int64_t childIdent = (_ident << 3) | childIndex;
 176
 177     if (d2 < LEAF_SIZE_SQR) {
 178       child = new Leaf(cb, childIdent);
 179     } else {
 180       child = new Branch(cb, childIdent);
 181     }
 182
 183     children[childIndex] = child;
 184
 185     if (childrenLoaded) {
 186     // childrenLoad is done, so we're defining a new node - add it to the
 187     // cache too.
 188       NavDataCache::instance()->defineOctreeNode(const_cast<Branch*>(this), child);
 189     }
 190   }
 191
 192   return children[childIndex];
 193 }
 194
 195 void Branch::loadChildren() const
 196 {
 197   if (childrenLoaded) {
 198     return;
 199   }
 200
 201   int childrenMask = NavDataCache::instance()->getOctreeBranchChildren(guid());
 202   for (int i=0; i<8; ++i) {
 203     if ((1 << i) & childrenMask) {
 204       childAtIndex(i); // accessing will create!
 205     }
 206   } // of child index iteration
 207
 208 // set this after creating the child nodes, so the cache update logic
 209 // in childAtIndex knows any future created children need to be added.
 210   childrenLoaded = true;
 211 }
 212
 213 int Branch::childMask() const
 214 {
 215   int result = 0;
 216   for (int i=0; i<8; ++i) {
 217     if (children[i]) {
 218       result |= 1 << i;
 219     }
 220   }
 221
 222   return result;
 223 }
 224
 225 void findNearestN(const SGVec3d& aPos, unsigned int aN, double aCutoffM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
 226 {
 227   aResults.clear();
 228   FindNearestPQueue pq;
 229   FindNearestResults results;
 230   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 231   double cut = aCutoffM;
 232
 233   while (!pq.empty()) {
 234     if (!results.empty()) {
 235       // terminate the search if we have sufficent results, and we are
 236       // sure no node still on the queue contains a closer match
 237       double furthestResultOrder = results.back().order();
 238     //  std::cout << "furthest result:" << furthestResultOrder << ", top node order:" << pq.top().order() << std::endl;
 239       if ((results.size() >= aN) && (furthestResultOrder < pq.top().order())) {
 240         break;
 241       }
 242     }
 243
 244     Node* nd = pq.top().get();
 245     pq.pop();
 246
 247 //    std::cout << "visiting:" << std::oct << nd->guid() << "(" << std::dec << nd->guid() << ")" << std::endl;
 248     nd->visit(aPos, cut, aFilter, results, pq);
 249   } // of queue iteration
 250
 251   // depending on leaf population, we may have (slighty) more results
 252   // than requested
 253   unsigned int numResults = std::min((unsigned int) results.size(), aN);
 254   // copy results out
 255   aResults.resize(numResults);
 256   for (unsigned int r=0; r<numResults; ++r) {
 257     aResults[r] = results[r].get();
 258   }
 259 }
 260
 261 void findAllWithinRange(const SGVec3d& aPos, double aRangeM, FGPositioned::Filter* aFilter, FGPositioned::List& aResults)
 262 {
 263   aResults.clear();
 264   FindNearestPQueue pq;
 265   FindNearestResults results;
 266   pq.push(Ordered<Node*>(global_spatialOctree, 0));
 267   double rng = aRangeM;
 268
 269   while (!pq.empty()) {
 270     Node* nd = pq.top().get();
 271     pq.pop();
 272
 273     nd->visit(aPos, rng, aFilter, results, pq);
 274   } // of queue iteration
 275
 276   unsigned int numResults = results.size();
 277   // copy results out
 278   aResults.resize(numResults);
 279   for (unsigned int r=0; r<numResults; ++r) {
 280     aResults[r] = results[r].get();
 281   }
 282 }
 283
 284 } // of namespace Octree
 285
 286 } // of namespace flightgear